George_Andreou/Robotex-Rally
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add derivative

Browse source
This commit is contained in:
Γιώργος Αντρέου 2025-06-23 02:16:34 +03:00
parent 5c94f1225b
commit 4fc0f15a88
1 changed files with 9 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

13
rally.py
View file

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