129 lines
3.4 KiB
Python
129 lines
3.4 KiB
Python
import time
|
|
import math
|
|
import board
|
|
import adafruit_hcsr04
|
|
import pwmio
|
|
import digitalio
|
|
import rotaryio
|
|
from adafruit_motor import motor
|
|
|
|
# Mathematics
|
|
diameter = 0.047 # 47mm wheel diameter
|
|
circumference = diameter * math.pi
|
|
target_dist = 5
|
|
target_vel = 1.0 / circumference
|
|
|
|
# Encoder constants
|
|
GEAR_RATIO = 50 # The gear ratio of the motor
|
|
COUNTS_PER_REV = 12 * GEAR_RATIO # The counts per revolution of the motor's output shaft
|
|
ENCODER_NAMES = ["A", "D"]
|
|
|
|
# Motor constants
|
|
FREQUENCY = 25000
|
|
DECAY_MODE = motor.SLOW_DECAY
|
|
|
|
UPDATE_RATE = 100 # Hz
|
|
SLEEP_TIME = 1 / UPDATE_RATE
|
|
BASE_THROTTLE = 0.85 # Base throttle for both motors
|
|
Kp = 0.4 # Proportional gain for sync correction
|
|
Kd = 0.2
|
|
prev_left = 0
|
|
prev_right = 0
|
|
|
|
# Create a digitalinout object for the user switch
|
|
user_sw = digitalio.DigitalInOut(board.USER_SW)
|
|
user_sw.direction = digitalio.Direction.INPUT
|
|
user_sw.pull = digitalio.Pull.UP
|
|
|
|
# Set the pwm and motor pins
|
|
pwm_a_p = pwmio.PWMOut(board.MOTOR_A_P, frequency=FREQUENCY)
|
|
pwm_a_n = pwmio.PWMOut(board.MOTOR_A_N, frequency=FREQUENCY)
|
|
|
|
pwm_d_p = pwmio.PWMOut(board.MOTOR_D_P, frequency=FREQUENCY)
|
|
pwm_d_n = pwmio.PWMOut(board.MOTOR_D_N, frequency=FREQUENCY)
|
|
|
|
mot_right = motor.DCMotor(pwm_a_p, pwm_a_n)
|
|
mot_left = motor.DCMotor(pwm_d_p, pwm_d_n)
|
|
|
|
mot_right.decay_mode = DECAY_MODE
|
|
mot_left.decay_mode = DECAY_MODE
|
|
|
|
# Create the encoder objects
|
|
enc_right = rotaryio.IncrementalEncoder(board.ENCODER_A_B, board.ENCODER_A_A, divisor=1)
|
|
enc_left = rotaryio.IncrementalEncoder(board.ENCODER_D_B, board.ENCODER_D_A, divisor=1)
|
|
|
|
enc_right.position = 0
|
|
enc_left.position = 0
|
|
lastrevleft = 0
|
|
lastrevright = 0
|
|
revleft = 0
|
|
revright = 0
|
|
|
|
# Set the sensor pins
|
|
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:
|
|
print("I am waiting...", sonar.distance)
|
|
|
|
if sonar.distance >= 20:
|
|
break
|
|
|
|
while True:
|
|
# Find the motor revolutions
|
|
|
|
lastrevleft = revleft
|
|
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.
|
|
lefterror = target_vel - vel_left
|
|
righterror = target_vel - vel_right
|
|
|
|
left_der = (lefterror - prev_left) / UPDATE_RATE
|
|
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)
|
|
right_throttle = max(min(right_throttle, 1.0), -1.0)
|
|
|
|
|
|
distleft = revleft * circumference
|
|
distright = revright * circumference
|
|
|
|
avg = (abs(distleft) + abs(distright)) / 2
|
|
|
|
print(f"Distance traveled: {avg:.3f} m")
|
|
|
|
|
|
if avg >= target_dist:
|
|
print("Target reached!!!")
|
|
mot_left.throttle = 0
|
|
mot_right.throttle = 0
|
|
break
|
|
|
|
|
|
mot_right.throttle = right_throttle
|
|
mot_left.throttle = - left_throttle
|
|
|
|
print(right_throttle, left_throttle)
|
|
|
|
time.sleep(0.05)
|
|
print("Finished!!!")
|