Ch-10 TR feedback

This commit is contained in:
Danny Staple
2022-12-17 16:05:25 +00:00
parent b2228889e8
commit 45ba981386
4 changed files with 20 additions and 19 deletions
@@ -14,22 +14,22 @@ class PController:
## We'll set up a single distance sensor, and keep a set distance from an object ## We'll set up a single distance sensor, and keep a set distance from an object
robot.left_distance.distance_mode = 1 robot.right_distance.distance_mode = 1
robot.left_distance.start_ranging() robot.right_distance.start_ranging()
distance_set_point = 10 distance_set_point = 10
distance_controller = PController(-0.1) distance_controller = PController(-0.1)
while True: while True:
if robot.left_distance.data_ready: if robot.right_distance.data_ready:
distance = robot.left_distance.distance distance = robot.right_distance.distance
error = distance_set_point - distance error = distance_set_point - distance
speed = distance_controller.calculate(error) speed = distance_controller.calculate(error)
if abs(speed) < 0.2: if abs(speed) < 0.3:
speed = 0 speed = 0
uart.write(f"{error},{speed}\n".encode()) uart.write(f"{error},{speed}\n".encode())
print(f"{error},{speed}") print(f"{error},{speed}")
robot.set_left(speed) robot.set_left(speed)
robot.set_right(speed) robot.set_right(speed)
robot.left_distance.clear_interrupt() robot.right_distance.clear_interrupt()
time.sleep(0.05) time.sleep(0.05)
+6 -6
View File
@@ -17,28 +17,28 @@ class PIController:
## We'll set up a single distance sensor, and keep a set distance from an object ## We'll set up a single distance sensor, and keep a set distance from an object
robot.left_distance.distance_mode = 1 robot.right_distance.distance_mode = 1
robot.left_distance.start_ranging() robot.right_distance.start_ranging()
distance_set_point = 10 distance_set_point = 10
distance_controller = PIController(-0.19, -0.005) distance_controller = PIController(-0.19, -0.005)
prev_time = time.monotonic() prev_time = time.monotonic()
while True: while True:
if robot.left_distance.data_ready: if robot.right_distance.data_ready:
distance = robot.left_distance.distance distance = robot.right_distance.distance
error = distance_set_point - distance error = distance_set_point - distance
current_time = time.monotonic() current_time = time.monotonic()
speed = distance_controller.calculate(error, current_time - prev_time) speed = distance_controller.calculate(error, current_time - prev_time)
prev_time = current_time prev_time = current_time
# Control the motors with the speed # Control the motors with the speed
if abs(speed) < 0.35: if abs(speed) < 0.3:
speed = 0 speed = 0
uart.write(f"{error},{speed}," uart.write(f"{error},{speed},"
f"{distance_controller.integral}\n".encode()) f"{distance_controller.integral}\n".encode())
print(f"{error},{speed},{distance_controller.integral}") print(f"{error},{speed},{distance_controller.integral}")
robot.set_left(speed) robot.set_left(speed)
robot.set_right(speed) robot.set_right(speed)
robot.left_distance.clear_interrupt() robot.right_distance.clear_interrupt()
time.sleep(0.05) time.sleep(0.05)
+6 -6
View File
@@ -8,28 +8,28 @@ uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
## We'll set up a single distance sensor, and keep a set distance from an object ## We'll set up a single distance sensor, and keep a set distance from an object
robot.left_distance.distance_mode = 1 robot.right_distance.distance_mode = 1
robot.left_distance.start_ranging() robot.right_distance.start_ranging()
distance_set_point = 10 distance_set_point = 10
distance_controller = PIDController(-0.09, -0.02, -0.07) distance_controller = PIDController(-0.09, -0.02, -0.07)
prev_time = time.monotonic() prev_time = time.monotonic()
while True: while True:
if robot.left_distance.data_ready: if robot.right_distance.data_ready:
distance = robot.left_distance.distance distance = robot.right_distance.distance
error = distance_set_point - distance error = distance_set_point - distance
current_time = time.monotonic() current_time = time.monotonic()
speed = distance_controller.calculate(error, current_time - prev_time) speed = distance_controller.calculate(error, current_time - prev_time)
prev_time = current_time prev_time = current_time
# Control the motors with the speed # Control the motors with the speed
if abs(speed) < 0.35: if abs(speed) < 0.3:
speed = 0 speed = 0
uart.write(f"{error},{speed},{distance_controller.integral},{distance_controller.derivative}\n".encode()) uart.write(f"{error},{speed},{distance_controller.integral},{distance_controller.derivative}\n".encode())
print(f"{error},{speed},{distance_controller.integral},{distance_controller.derivative}") print(f"{error},{speed},{distance_controller.integral},{distance_controller.derivative}")
robot.set_left(speed) robot.set_left(speed)
robot.set_right(speed) robot.set_right(speed)
# reset the distance sensor # reset the distance sensor
robot.left_distance.clear_interrupt() robot.right_distance.clear_interrupt()
time.sleep(0.05) time.sleep(0.05)
+2 -1
View File
@@ -25,7 +25,8 @@ while True:
current_time = time.monotonic() current_time = time.monotonic()
deflection = distance_controller.calculate(error, current_time - prev_time) deflection = distance_controller.calculate(error, current_time - prev_time)
prev_time = current_time prev_time = current_time
uart.write(f"{error},{deflection}\n".encode()) # ,{distance_controller.derivative} uart.write(f"{error},{deflection},"
f"{distance_controller.derivative}\n".encode())
if motors_active: if motors_active:
robot.set_left(speed - deflection) robot.set_left(speed - deflection)
robot.set_right(speed + deflection) robot.set_right(speed + deflection)