Add error correction

rally.py

@@ -21,6 +21,11 @@ ENCODER_NAMES = ["A", "D"]
 FREQUENCY = 25000               
 DECAY_MODE = motor.SLOW_DECAY
 
+UPDATE_RATE = 100  # Hz
+SLEEP_TIME = 1 / UPDATE_RATE
+BASE_THROTTLE = 0.95  # Base throttle for both motors
+Kp_sync = 0.05        # Proportional gain for sync correction
+
 # Create a digitalinout object for the user switch
 user_sw = digitalio.DigitalInOut(board.USER_SW)
 user_sw.direction = digitalio.Direction.INPUT
@@ -58,14 +63,21 @@ while sonar.distance < 20:
         break
 
 print(">>>>> Starting movement!!!")
-mot_right.throttle = 1
-mot_left.throttle = - 1    
 
 while True:
     # Find the motor revolutions
     revleft = - enc_left.position / COUNTS_PER_REV
     revright = enc_right.position / COUNTS_PER_REV
     
+    # Encoder error due to friction.
+    error = enc_left.position - enc_right.position
+    left_throttle = BASE_THROTTLE - (error * Kp_sync)
+    right_throttle = BASE_THROTTLE + (error * Kp_sync)
+
+    left_throttle = max(min(left_throttle, 1.0), -1.0)
+    right_throttle = max(min(right_throttle, 1.0), -1.0)
+
+    
     distleft = revleft * circumference
     distright = revright * circumference
     
@@ -75,8 +87,8 @@ while True:
         
     if avg >= target_dist:
         print("Target reached!!!")
-        mot_left.throttle = 0
-        mot_right.throttle = 0
+        mot_left.throttle = left_throttle
+        mot_right.throttle = right_throttle
         break
     
     mot_right.throttle = 1