Update the pd algorithm with motor specific error feedback

rally.py

@@ -11,6 +11,7 @@ from adafruit_motor import motor
 diameter = 0.047 # 47mm wheel diameter
 circumference = diameter * math.pi
 target_dist = 5
+target_vel = 1.0 / circumference
 
 # Encoder constants
 GEAR_RATIO = 50                     # The gear ratio of the motor
@@ -24,9 +25,10 @@ DECAY_MODE = motor.SLOW_DECAY
 UPDATE_RATE = 100  # Hz
 SLEEP_TIME = 1 / UPDATE_RATE
 BASE_THROTTLE = 0.85  # Base throttle for both motors
-Kp_sync = 1.5  # Proportional gain for sync correction
-Kd_sync = 0.04
-prev_error = 0
+Kp = 0.4  # Proportional gain for sync correction
+Kd = 0.2
+prev_left = 0
+prev_right = 0
 
 # Create a digitalinout object for the user switch
 user_sw = digitalio.DigitalInOut(board.USER_SW)
@@ -72,10 +74,9 @@ while sonar.distance < 20:
     if sonar.distance >= 20:
         break
 
-print(">>>>> Starting movement!!!")
-
 while True:
     # Find the motor revolutions
+    
     lastrevleft = revleft
     lastrevright = revright
     revleft = to_revs(enc_left.position)
@@ -85,13 +86,20 @@ while True:
     vel_right = vel(revright, lastrevright)
     
     # Encoder error due to friction.
-    error = vel_left - vel_right
-    derivative = (error - prev_error) / UPDATE_RATE
-    prev_error = error
-    correction = (Kp_sync * error) #+ (Kd_sync * derivative)
+    lefterror = target_vel - vel_left
+    righterror = target_vel - vel_right
     
-    left_throttle = BASE_THROTTLE - correction
-    right_throttle = BASE_THROTTLE + correction
+    left_der = (lefterror - prev_left) / UPDATE_RATE
+    right_der = (righterror - prev_right) / UPDATE_RATE
+    
+    prev_left = lefterror
+    prev_right = righterror
+    
+    rightcorr = (Kp * righterror) + (Kd * right_der)
+    leftcorr = (Kp * lefterror) + (Kd * left_der)
+    
+    left_throttle = BASE_THROTTLE + leftcorr
+    right_throttle = BASE_THROTTLE + rightcorr
 
     left_throttle = max(min(left_throttle, 1.0), -1.0)
     right_throttle = max(min(right_throttle, 1.0), -1.0)