97 lines
2.7 KiB
Python
97 lines
2.7 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 = 3
|
|
|
|
# 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.95 # Base throttle for both motors
|
|
Kp_sync = 0.05 # Proportional gain for sync correction
|
|
|
|
# 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
|
|
|
|
# Set the sensor pins
|
|
sonar = adafruit_hcsr04.HCSR04(trigger_pin=board.TRIG, echo_pin=board.ECHO)
|
|
|
|
|
|
|
|
while sonar.distance < 20:
|
|
print("I am waiting...", sonar.distance)
|
|
|
|
if sonar.distance >= 20:
|
|
break
|
|
|
|
print(">>>>> Starting movement!!!")
|
|
|
|
while True:
|
|
# Find the motor revolutions
|
|
revleft = - enc_left.position / COUNTS_PER_REV
|
|
revright = enc_right.position / COUNTS_PER_REV
|
|
|
|
# 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)
|
|
|
|
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 = (distleft + distright) / 2
|
|
|
|
print(f"Distance traveled: {avg:.3f} m")
|
|
|
|
if avg >= target_dist:
|
|
print("Target reached!!!")
|
|
mot_left.throttle = left_throttle
|
|
mot_right.throttle = right_throttle
|
|
break
|
|
|
|
mot_right.throttle = 1
|
|
mot_left.throttle = - 1
|
|
|
|
time.sleep(0.05)
|