diff --git a/main.py b/main.py
index ca7b5b3..aa3d419 100644
--- a/main.py
+++ b/main.py
@@ -27,6 +27,7 @@ dmm2 = None
 MOTOR_EN1 = 1
 MOTOR_EN2 = 2
 DISABLED = None
+RPM_TARGET = 0
 
 def enable_motors():
     global DISABLED
@@ -76,6 +77,8 @@ def set_motors(rpm):
 
 
 async def process_mqtt(message):
+    global RPM_TARGET
+
     try:
         text = message.payload.decode()
     except UnicodeDecodeError:
@@ -86,7 +89,7 @@ async def process_mqtt(message):
 
     if topic == 'motion/set_rpm':
         rpm = int(float(text))
-        set_motors(rpm)
+        RPM_TARGET = rpm
 
 
 async def monitor_mqtt():
@@ -143,7 +146,7 @@ async def check_sync(rev1, rev2):
 
     difference = rev1 - rev2
 
-    if abs(difference) > 1.0:
+    if abs(difference) > 2.0:
         # out of sync, kill both motors
         set_motors(0)
         disable_motors()
@@ -154,9 +157,13 @@ async def check_sync(rev1, rev2):
         logging.error('MOTOR READINGS OUT OF SYNC! Motor1: %s, Motor2: %s, difference: %s', rev1, rev2, difference)
         await send_mqtt('server/motor_status', 'Motor desync')
 
+        return False
+
+    return True
+
 
 async def monitor_dyn4():
-    global dmm1, dmm2
+    global dmm1, dmm2, RPM_TARGET
 
     while True:
         await asyncio.sleep(0.1)
@@ -173,20 +180,63 @@ async def monitor_dyn4():
         if pos1 is False:
             dmm1 = False
             continue
-        rev1 = pos1 / ENCODER_PPR
+        rev1 = round(pos1 / ENCODER_PPR, 4)
 
         if ONE_MOTOR:
             revolutions = rev1
+            motor1_rpm = RPM_TARGET
         else:
             pos2 = await read_motor(dmm2, 'Motor2')
             if pos2 is False:
                 dmm2 = False
                 continue
-            rev2 = pos2 / ENCODER_PPR
+            rev2 = round(pos2 / ENCODER_PPR, 4)
 
             revolutions = (rev1 + rev2) / 2.0
 
-            await check_sync(rev1, rev2)
+            if not await check_sync(rev1, rev2):
+                continue
+
+            motor1_scaler = 1.0
+            motor2_scaler = 1.0
+            kp = 0.01
+            difference = rev1 - rev2
+
+            if abs(difference) > 0.25:
+                if RPM_TARGET > 0:
+                    if rev1 > rev2:
+                        motor1_scaler = 1.0 - kp * abs(difference)
+                        motor1_scaler = min(motor1_scaler, 1.0)  # clamp to range 0.5 - 1.0
+                        motor1_scaler = max(motor1_scaler, 0.5)
+                    elif rev2 > rev1:
+                        motor2_scaler = 1.0 - kp * abs(difference)
+                        motor2_scaler = min(motor2_scaler, 1.0)  # clamp to range 0.5 - 1.0
+                        motor2_scaler = max(motor2_scaler, 0.5)
+                elif RPM_TARGET < 0:
+                    if rev1 < rev2:
+                        motor1_scaler = 1.0 - kp * abs(difference)
+                        motor1_scaler = min(motor1_scaler, 1.0)  # clamp to range 0.5 - 1.0
+                        motor1_scaler = max(motor1_scaler, 0.5)
+                    elif rev2 < rev1:
+                        motor2_scaler = 1.0 - kp * abs(difference)
+                        motor2_scaler = min(motor2_scaler, 1.0)  # clamp to range 0.5 - 1.0
+                        motor2_scaler = max(motor2_scaler, 0.5)
+
+            motor1_rpm = int(RPM_TARGET * motor1_scaler)
+            motor2_rpm = int(RPM_TARGET * motor2_scaler)
+
+            if abs(difference) > 0.25:
+                logging.info('Pos difference: %s, rev1: %s, rev2: %s, scaler1: %s, scaler2: %s, target: %s, rpm1: %s, rpm2: %s',
+                    difference, rev1, rev2, motor1_scaler, motor2_scaler, RPM_TARGET, motor1_rpm, motor2_rpm)
+
+        if ONE_MOTOR:
+            logging.debug('Setting motor1: %s', motor1_rpm)
+            dmm1.set_speed(motor1_rpm)
+        else:
+            logging.debug('Setting motor1: %s, motor2: %s', motor1_rpm, motor2_rpm)
+            dmm1.set_speed(motor1_rpm)
+            dmm2.set_speed(motor2_rpm)
+
 
         topic = 'server/position'
         message = str(round(revolutions, 5))