Ch-10 TR feedback
This commit is contained in:
@@ -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)
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|||||||
Reference in New Issue
Block a user