Add derivative

rally.py

@@ -23,8 +23,9 @@ 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
+BASE_THROTTLE = 0.9  # Base throttle for both motors
+Kp_sync = 0.05  # Proportional gain for sync correction
+Kd_sync = 0.001
 
 # Create a digitalinout object for the user switch
 user_sw = digitalio.DigitalInOut(board.USER_SW)
@@ -71,8 +72,12 @@ while True:
     
     # 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)
+    derivative = (error - prev_error) / UPDATE_RATE
+    prev_error = error
+    correction = (Kp_sync * error) + (Kd_sync * derivative)
+    
+    left_throttle = BASE_THROTTLE - correction
+    right_throttle = BASE_THROTTLE + correction
 
     left_throttle = max(min(left_throttle, 1.0), -1.0)
     right_throttle = max(min(right_throttle, 1.0), -1.0)