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

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

Browse source
This commit is contained in:
Γιώργος Αντρέου 2025-06-27 21:25:01 +03:00
parent 1a3c2a1c1f
commit 078b5d4fa3
1 changed files with 28 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

37
rally.py
View file

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