Chapter 10 code

ch-10/2-pi-distance-control/code.py

@@ -0,0 +1,44 @@
+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)