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

Update the pd algorithm with motor specific error feedback

Browse source
This commit is contained in:
Γιώργος Αντρέου 2025-06-28 23:29:28 +03:00
parent eecee24643
commit 7028738f4e
1 changed files with 19 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

30
rally.py
View file

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