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