45 lines
1.2 KiB
Python
45 lines
1.2 KiB
Python
import time
|
|
import board
|
|
import busio
|
|
import robot
|
|
|
|
uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
|
|
|
|
class PIController:
|
|
def __init__(self, kp, ki):
|
|
self.kp = kp
|
|
self.ki = ki
|
|
self.integral = 0
|
|
|
|
def calculate(self, error, dt):
|
|
self.integral += error * dt
|
|
return self.kp * error + self.ki * self.integral
|
|
|
|
|
|
## We'll set up a single distance sensor, and keep a set distance from an object
|
|
robot.left_distance.distance_mode = 1
|
|
robot.left_distance.start_ranging()
|
|
|
|
distance_set_point = 10
|
|
distance_controller = PIController(-0.19, -0.005)
|
|
|
|
prev_time = time.monotonic()
|
|
while True:
|
|
if robot.left_distance.data_ready:
|
|
distance = robot.left_distance.distance
|
|
error = distance_set_point - distance
|
|
|
|
current_time = time.monotonic()
|
|
speed = distance_controller.calculate(error, current_time - prev_time)
|
|
prev_time = current_time
|
|
# Control the motors with the speed
|
|
if abs(speed) < 0.35:
|
|
speed = 0
|
|
uart.write(f"{error},{speed},"
|
|
f"{distance_controller.integral}\n".encode())
|
|
print(f"{error},{speed},{distance_controller.integral}")
|
|
robot.set_left(speed)
|
|
robot.set_right(speed)
|
|
robot.left_distance.clear_interrupt()
|
|
time.sleep(0.05)
|