diff --git a/rally.py b/rally.py index 543e796..8a40635 100644 --- a/rally.py +++ b/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