61 lines
2.0 KiB
Python
61 lines
2.0 KiB
Python
import time
|
|
import board
|
|
import busio
|
|
import robot
|
|
from pid_controller import PIDController
|
|
|
|
uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
|
|
|
|
|
|
robot.right_distance.distance_mode = 1
|
|
robot.right_distance.start_ranging()
|
|
|
|
speed = 0.7
|
|
distance_set_point = 15
|
|
distance_controller = PIDController(0.046, 0.0, 0)
|
|
print("Waiting for bytes on UART...")
|
|
|
|
prev_time = time.monotonic()
|
|
motors_active = False
|
|
while True:
|
|
if robot.right_distance.data_ready:
|
|
distance = robot.right_distance.distance
|
|
error = distance_set_point - distance
|
|
|
|
current_time = time.monotonic()
|
|
deflection = distance_controller.calculate(error, current_time - prev_time)
|
|
prev_time = current_time
|
|
uart.write(f"{error},{deflection}\n".encode()) # ,{distance_controller.derivative}
|
|
if motors_active:
|
|
robot.set_left(speed - deflection)
|
|
robot.set_right(speed + deflection)
|
|
# reset the distance sensor
|
|
robot.right_distance.clear_interrupt()
|
|
time.sleep(0.05)
|
|
if uart.in_waiting:
|
|
command = uart.readline().decode().strip()
|
|
if command.startswith("M"):
|
|
speed = float(command[1:])
|
|
elif command == "O":
|
|
motors_active = not motors_active
|
|
robot.set_left(0)
|
|
robot.set_right(0)
|
|
distance_controller.integral = 0
|
|
elif command.startswith("P"):
|
|
distance_controller.kp = float(command[1:])
|
|
elif command.startswith("I"):
|
|
distance_controller.ki = float(command[1:])
|
|
elif command.startswith("D"):
|
|
distance_controller.kd = float(command[1:])
|
|
elif command.startswith("S"):
|
|
distance_set_point = float(command[1:])
|
|
elif command.startswith("?"):
|
|
uart.write(f"P{distance_controller.kp:.3f}\n".encode())
|
|
uart.write(f"I{distance_controller.ki:.3f}\n".encode())
|
|
uart.write(f"D{distance_controller.kd:.3f}\n".encode())
|
|
uart.write(f"S{distance_set_point:.3f}\n".encode())
|
|
uart.write(f"M{speed:.1f}\n".encode())
|
|
time.sleep(3)
|
|
elif command.startswith("R"):
|
|
distance_controller.integral = 0
|