Changes to the constants and replacement of revolution error with velocity error.

rally.py

@@ -10,7 +10,7 @@ from adafruit_motor import motor
 # Mathematics
 diameter = 0.047 # 47mm wheel diameter
 circumference = diameter * math.pi
-target_dist = 3
+target_dist = 5
 
 # Encoder constants
 GEAR_RATIO = 50                     # The gear ratio of the motor
@@ -23,9 +23,9 @@ DECAY_MODE = motor.SLOW_DECAY
 
 UPDATE_RATE = 100  # Hz
 SLEEP_TIME = 1 / UPDATE_RATE
-BASE_THROTTLE = 0.9  # Base throttle for both motors
-Kp_sync = 0.05  # Proportional gain for sync correction
-Kd_sync = 0.001
+BASE_THROTTLE = 0.85  # Base throttle for both motors
+Kp_sync = 1  # Proportional gain for sync correction
+Kd_sync = 0.01
 prev_error = 0
 
 # Create a digitalinout object for the user switch
@@ -52,11 +52,19 @@ enc_left = rotaryio.IncrementalEncoder(board.ENCODER_D_B, board.ENCODER_D_A, div
 
 enc_right.position = 0
 enc_left.position = 0
+lastrevleft = 0
+lastrevright = 0
+revleft = 0
+revright = 0
 
 # Set the sensor pins
 sonar = adafruit_hcsr04.HCSR04(trigger_pin=board.TRIG, echo_pin=board.ECHO)
 
-
+def to_revs(position):
+    return position / COUNTS_PER_REV
+    
+def vel(rev, last_rev):
+    return (rev - last_rev) / UPDATE_RATE
 
 while sonar.distance < 20:
     print("I am waiting...", sonar.distance)
@@ -68,14 +76,19 @@ print(">>>>> Starting movement!!!")
 
 while True:
     # Find the motor revolutions
-    revleft = - enc_left.position / COUNTS_PER_REV
-    revright = enc_right.position / COUNTS_PER_REV
+    lastrevleft = revleft
+    lastrevright = revright
+    revleft = to_revs(enc_left.position)
+    revright = to_revs(enc_right.position)
+    
+    vel_left = vel(revleft, lastrevleft)
+    vel_right = vel(revright, lastrevright)
     
     # Encoder error due to friction.
-    error = enc_left.position - enc_right.position
+    error = vel_left - vel_right
     derivative = (error - prev_error) / UPDATE_RATE
     prev_error = error
-    correction = (Kp_sync * error) + (Kd_sync * derivative)
+    correction = (Kp_sync * error) #+ (Kd_sync * derivative)
     
     left_throttle = BASE_THROTTLE - correction
     right_throttle = BASE_THROTTLE + correction
@@ -87,9 +100,10 @@ while True:
     distleft = revleft * circumference
     distright = revright * circumference
     
-    avg = (distleft + distright) / 2
+    avg = (abs(distleft) + abs(distright)) / 2
 
     print(f"Distance traveled: {avg:.3f} m")
+    
         
     if avg >= target_dist:
         print("Target reached!!!")
@@ -97,8 +111,11 @@ while True:
         mot_right.throttle = 0
         break
     
-    mot_right.throttle = - right_throttle
+    
+    mot_right.throttle = right_throttle
     mot_left.throttle = - left_throttle
+        
+    print(right_throttle, left_throttle)
     
     time.sleep(0.05)
 print("Finished!!!")