Current state of experiments

.gitignore

@@ -5,3 +5,4 @@
 *.FCStd1
 secrets.py
 libs
+.idea/

ch-11/1-encoder-speed/encoder_speed.py

@@ -1,73 +0,0 @@
-import time
-import json
-
-from adafruit_esp32spi import adafruit_esp32spi_wsgiserver
-from adafruit_wsgi.wsgi_app import WSGIApp
-
-import robot
-import robot_wifi
-
-
-class SpeedCountApp:
-    def __init__(self):
-        self.intended_speed = 0.9
-        self.last_time = time.monotonic()
-        self.wifi = None
-        self.server = None
-
-    def setup_wifi(self, app):
-        print("Setting up wifi.")
-        self.wifi, esp = robot_wifi.connect_to_wifi()
-        self.server = adafruit_esp32spi_wsgiserver.WSGIServer(80, application=app)
-        adafruit_esp32spi_wsgiserver.set_interface(esp)
-        print("Starting server")
-
-        self.server.start()
-        ip_int = ".".join(str(int(n)) for n in esp.ip_address)
-        print(f"IP Address is {ip_int}")
-
-    def index(self, request):
-        new_time = time.monotonic()
-        time_delta = new_time - self.last_time
-        self.last_time = new_time
-
-        left_speed = robot.left_encoder.get_speed(time_delta)
-        right_speed = robot.right_encoder.get_speed(time_delta)
-        return (
-            200,
-            [("Content-Type", "application/json")],
-            [
-                json.dumps(
-                    {
-                        "left_speed": left_speed,
-                        "right_speed": right_speed,
-                        "time": self.last_time,
-                    }
-                )
-            ],
-        )
-
-    def main_loop(self):
-        robot.set_left(self.intended_speed)
-        robot.set_right(self.intended_speed)
-        while True:
-            try:
-                self.server.update_poll()
-            except RuntimeError as e:
-                print(f"Server poll error: {type(e)}, {e}")
-                print(f"Resetting ESP...")
-                self.wifi.reset()
-                print("Reset complete.")
-
-    def start(self):
-        app = WSGIApp()
-        app.route("/")(self.index)
-        print("Starting")
-        try:
-            self.setup_wifi(app)
-            self.main_loop()
-        finally:
-            robot.stop()
-
-
-SpeedCountApp().start()

ch-11/1-encoder-speed/robot_wifi.py

@@ -1,25 +0,0 @@
-import board
-import busio
-from digitalio import DigitalInOut
-from adafruit_esp32spi import adafruit_esp32spi
-from adafruit_esp32spi import adafruit_esp32spi_wifimanager
-
-try:
-    from secrets import secrets
-except ImportError:
-    print("WiFi secrets are kept in secrets.py, please add them there!")
-    raise
-
-
-def connect_to_wifi():
-    esp32_cs = DigitalInOut(board.GP10)
-    esp32_ready = DigitalInOut(board.GP9)
-    esp32_reset = DigitalInOut(board.GP8)
-
-    spi = busio.SPI(board.GP14, MOSI=board.GP11, MISO=board.GP12)
-    esp = adafruit_esp32spi.ESP_SPIcontrol(spi, esp32_cs, esp32_ready, esp32_reset)
-    esp.reset()
-    wifi = adafruit_esp32spi_wifimanager.ESPSPI_WiFiManager(esp, secrets)
-    wifi.connect()
-
-    return wifi, esp

ch-11/1-measuring_wheel_speeds/code.py

@@ -0,0 +1,48 @@
+import asyncio
+import robot
+
+class Settings:
+  speed = 0.7
+  time_interval = 0.2
+
+async def motor_speed_loop():
+  left_last, right_last = robot.left_encoder.read(), robot.right_encoder.read()
+  while True:
+    await asyncio.sleep(Settings.time_interval)
+    left_new, right_new = robot.left_encoder.read(), robot.right_encoder.read()
+    left_speed = robot.ticks_to_mm(left_new - left_last) / Settings.time_interval
+    left_last = left_new
+    right_speed = robot.ticks_to_mm(right_new - right_last) / Settings.time_interval
+    right_last = right_new
+    robot.uart.write(f"{left_speed:.3f},{right_speed:.3f},0\n".encode())
+
+async def stop_motors_after(seconds):
+  await asyncio.sleep(seconds)
+  robot.stop()
+
+async def command_handler():
+  while True:
+    if robot.uart.in_waiting:
+      command = robot.uart.readline().decode().strip()
+      if command.startswith("M"):
+        Settings.speed = float(command[1:])
+      elif command.startswith("T"):
+        Settings.time_interval = float(command[1:])
+      elif command == "O":
+        robot.stop()
+      elif command.startswith("O"):
+        await asyncio.sleep(5)
+        asyncio.create_task(stop_motors_after(float(command[1:])))
+        robot.set_left(Settings.speed)
+        robot.set_right(Settings.speed)
+      elif command.startswith("?"):
+        robot.uart.write(f"M{Settings.speed:.1f}\n".encode())
+        robot.uart.write(f"T{Settings.time_interval:.1f}\n".encode())
+        await asyncio.sleep(3)
+    await asyncio.sleep(0)
+
+try:
+  asyncio.create_task(motor_speed_loop())
+  asyncio.run(command_handler())
+finally:
+  robot.stop()

ch-11/1-measuring_wheel_speeds/pid_controller.py

@@ -0,0 +1,20 @@
+class PIDController:
+    def __init__(self, kp, ki, kd, d_filter_gain=0.1):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.d_filter_gain = d_filter_gain
+
+        self.integral = 0
+        self.error_prev = 0
+        self.derivative = 0
+
+    def calculate(self, error, dt):
+        self.integral += error * dt
+
+        # Add a low pass filter to the difference
+        difference = (error - self.error_prev) * self.d_filter_gain
+        self.error_prev += difference
+        self.derivative = difference / dt
+
+        return self.kp * error + self.ki * self.integral + self.kd * self.derivative

ch-11/1-measuring_wheel_speeds/pio_encoder.py

@@ -1,6 +1,8 @@
 import rp2pio
 import adafruit_pioasm
 import array
+import asyncio
+
 
 program = """
 ; use the osr for count
@@ -67,18 +69,16 @@ class QuadratureEncoder:
         )
         self.reversed = reversed
         self._buffer = array.array("i", [0])
-        self.previous_reading = 0
+        asyncio.create_task(self.poll_loop())
+
+    async def poll_loop(self):
+        while True:
+            await asyncio.sleep(0)
+            while self.sm.in_waiting:
+                self.sm.readinto(self._buffer)
 
     def read(self):
-        while self.sm.in_waiting:
-            self.sm.readinto(self._buffer)
         if self.reversed:
             return -self._buffer[0]
         else:
             return self._buffer[0]
-
-    def get_speed(self, delta_time):
-        new_read = self.read()
-        distance = new_read - self.previous_reading
-        self.previous_reading = new_read
-        return distance / delta_time

ch-11/1-measuring_wheel_speeds/robot.py

@@ -3,18 +3,31 @@ import pwmio
 import pio_encoder
 import busio
 import adafruit_vl53l1x
+import math
+import busio
 
+uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
 
-motor_A1 = pwmio.PWMOut(board.GP17)
-motor_A2 = pwmio.PWMOut(board.GP16)
-motor_B1 = pwmio.PWMOut(board.GP18)
-motor_B2 = pwmio.PWMOut(board.GP19)
+wheel_diameter_mm = 70
+wheel_circumference_mm = math.pi * wheel_diameter_mm
+gear_ratio = 298
+encoder_poles = 28
+ticks_per_revolution = encoder_poles * gear_ratio
+ticks_to_mm_const = wheel_circumference_mm / ticks_per_revolution
+
+def ticks_to_mm(ticks):
+    return ticks_to_mm_const * ticks
+
+motor_A2 = pwmio.PWMOut(board.GP17, frequency=100)
+motor_A1 = pwmio.PWMOut(board.GP16, frequency=100)
+motor_B2 = pwmio.PWMOut(board.GP18, frequency=100)
+motor_B1 = pwmio.PWMOut(board.GP19, frequency=100)
 
 right_motor = motor_A1, motor_A2
 left_motor = motor_B1, motor_B2
 
-right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21, reversed=True)
-left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27)
+right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21)
+left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27, reversed=True)
 
 i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1)
 i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)

ch-11/2-speed-control/code.py

@@ -0,0 +1,86 @@
+import asyncio
+import time
+import robot
+import pid_controller
+
+class Settings:
+  speed = 0.17
+  time_interval = 0.2
+  motors_enabled = False
+
+
+class SpeedController:
+  def __init__(self, encoder, motor_fn):
+    self.encoder = encoder
+    self.motor_fn = motor_fn
+    self.pid = pid_controller.PIDController(3, 0, 1)
+    self.reset()
+
+  def reset(self):
+    self.last_ticks = self.encoder.read()
+    self.pwm = 0
+    self.actual_speed = 0
+    self.pid.reset()
+
+  def update(self, dt):
+    current_ticks = self.encoder.read()
+    speed_in_ticks = (current_ticks - self.last_ticks) / dt
+    self.last_ticks = current_ticks
+    self.actual_speed = robot.ticks_to_mm(speed_in_ticks) / 1000
+    # calculate the error
+    error = (Settings.speed * Settings.motors_enabled) - self.actual_speed
+    # calculate the control signal
+    control_signal = self.pid.calculate(error, dt)
+    self.pwm += control_signal
+    self.motor_fn(self.pwm)
+
+
+left = SpeedController(robot.left_encoder, robot.set_left)
+right = SpeedController(robot.right_encoder, robot.set_right)
+
+
+async def motor_speed_loop():
+  last_time = time.monotonic()
+  while True:
+    await asyncio.sleep(Settings.time_interval)
+    current_time = time.monotonic()
+    dt = current_time - last_time
+    last_time = current_time
+    left.update(dt)
+    right.update(dt)
+    robot.uart.write(f"0, {left.actual_speed:.2f},{Settings.speed * Settings.motors_enabled:.2f}\n".encode())
+
+
+async def stop_motors_after(seconds):
+  await asyncio.sleep(seconds)
+  Settings.motors_enabled = False
+
+
+async def command_handler():
+  while True:
+    if robot.uart.in_waiting:
+      command = robot.uart.readline().decode().strip()
+      if command.startswith("M"):
+        Settings.speed = float(command[1:])
+      elif command.startswith("T"):
+        Settings.time_interval = float(command[1:])
+      elif command == "O":
+        Settings.motors_enabled = False
+      elif command.startswith("O"):
+        await asyncio.sleep(5)
+        asyncio.create_task(stop_motors_after(float(command[1:])))
+        Settings.motors_enabled = True
+        left.reset()
+        right.reset()
+      # Print settings
+      elif command.startswith("?"):
+        robot.uart.write(f"M{Settings.speed:.1f}\n".encode())
+        robot.uart.write(f"T{Settings.time_interval:.1f}\n".encode())
+        await asyncio.sleep(3)
+    await asyncio.sleep(0)
+
+try:
+  asyncio.create_task(motor_speed_loop())
+  asyncio.run(command_handler())
+finally:
+  robot.stop()

ch-11/2-speed-control/pid_controller.py

@@ -0,0 +1,27 @@
+class PIDController:
+    def __init__(self, kp, ki, kd, d_filter_gain=0.1, imax=None, imin=None):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.d_filter_gain = d_filter_gain
+        self.imax = imax
+        self.imin = imin
+        self.reset()
+
+    def reset(self):
+        self.integral = 0
+        self.error_prev = 0
+        self.derivative = 0
+
+    def calculate(self, error, dt):
+        self.integral += error * dt
+        if self.imax is not None and self.integral > self.imax:
+            self.integral = self.imax
+        if self.imin is not None and self.integral < self.imin:
+            self.integral = self.imin
+        # Add a low pass filter to the difference
+        difference = (error - self.error_prev) * self.d_filter_gain
+        self.error_prev += difference
+        self.derivative = difference / dt
+
+        return self.kp * error + self.ki * self.integral + self.kd * self.derivative

ch-11/2-speed-control/pio_encoder.py

@@ -0,0 +1,84 @@
+import rp2pio
+import adafruit_pioasm
+import array
+import asyncio
+
+
+program = """
+; use the osr for count
+; input pins c1 c2
+
+    set y, 0            ; clear y
+    mov osr, y          ; and clear osr
+read:
+    ; x will be the old value
+    ; y the new values
+    mov x, y            ; store old Y in x
+    in null, 32         ; Clear ISR - using y
+    in pins, 2          ; read two pins into y
+    mov y, isr
+    jmp x!=y, different ; Jump if its different
+    jmp read            ; otherwise loop back to read
+
+different:
+    ; x has old value, y has new.
+    ; extract the upper bit of X.
+    in x, 31             ; get bit 31 - old p1 (remember which direction it came in)
+    in null, 31         ; keep only 1 bit
+    mov x, isr          ; put this back in x
+    jmp !x, c1_old_zero
+
+c1_old_not_zero:
+    jmp pin, count_up
+    jmp count_down
+
+c1_old_zero:
+    jmp pin, count_down
+    ; fall through
+count_up:
+    ; for a clockwise move - we'll add 1 by inverting
+    mov x, ~ osr        ; store inverted OSR on x
+    jmp x--, fake       ; use jump to take off 1
+fake:
+    mov x, ~ x          ; invert back
+    jmp send
+count_down:
+    ; for a clockwise move, just take one off
+    mov x, osr          ; store osr in x
+    jmp x--, send       ; dec and send
+send:
+    ; send x.
+    mov isr, x          ; send it
+    push noblock        ; put ISR into input FIFO
+    mov osr, x          ; put X back in OSR
+    jmp read            ; loop back
+"""
+
+assembled = adafruit_pioasm.assemble(program)
+
+
+class QuadratureEncoder:
+    def __init__(self, first_pin, second_pin, reversed=False):
+        """Encoder with 2 pins. Must use sequential pins on the board"""
+        self.sm = rp2pio.StateMachine(
+            assembled,
+            frequency=0,
+            first_in_pin=first_pin,
+            jmp_pin=second_pin,
+            in_pin_count=2,
+        )
+        self.reversed = reversed
+        self._buffer = array.array("i", [0])
+        asyncio.create_task(self.poll_loop())
+
+    async def poll_loop(self):
+        while True:
+            await asyncio.sleep(0)
+            while self.sm.in_waiting:
+                self.sm.readinto(self._buffer)
+
+    def read(self):
+        if self.reversed:
+            return -self._buffer[0]
+        else:
+            return self._buffer[0]

ch-11/2-speed-control/robot.py

@@ -0,0 +1,69 @@
+import board
+import pwmio
+import pio_encoder
+import busio
+import adafruit_vl53l1x
+import math
+import busio
+
+uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
+
+wheel_diameter_mm = 70
+wheel_circumference_mm = math.pi * wheel_diameter_mm
+gear_ratio = 298
+encoder_poles = 28
+ticks_per_revolution = encoder_poles * gear_ratio
+ticks_to_mm_const = wheel_circumference_mm / ticks_per_revolution
+
+def ticks_to_mm(ticks):
+    return ticks_to_mm_const * ticks
+
+motor_A2 = pwmio.PWMOut(board.GP17, frequency=100)
+motor_A1 = pwmio.PWMOut(board.GP16, frequency=100)
+motor_B2 = pwmio.PWMOut(board.GP18, frequency=100)
+motor_B1 = pwmio.PWMOut(board.GP19, frequency=100)
+
+right_motor = motor_A1, motor_A2
+left_motor = motor_B1, motor_B2
+
+right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21)
+left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27, reversed=True)
+
+i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1)
+i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)
+
+left_distance = adafruit_vl53l1x.VL53L1X(i2c0)
+right_distance = adafruit_vl53l1x.VL53L1X(i2c1)
+
+
+def stop():
+    motor_A1.duty_cycle = 0
+    motor_A2.duty_cycle = 0
+    motor_B1.duty_cycle = 0
+    motor_B2.duty_cycle = 0
+
+
+def set_speed(motor, speed):
+    # Swap motor pins if we reverse the speed
+    if abs(speed) < 0.1:
+        motor[0].duty_cycle = 0
+        motor[1].duty_cycle = 1
+        return
+    if speed < 0:
+        direction = motor[1], motor[0]
+        speed = -speed
+    else:
+        direction = motor
+    speed = min(speed, 1)  # limit to 1.0
+    max_speed = 2 ** 16 - 1
+
+    direction[0].duty_cycle = int(max_speed * speed)
+    direction[1].duty_cycle = 0
+
+
+def set_left(speed):
+    set_speed(left_motor, speed)
+
+
+def set_right(speed):
+    set_speed(right_motor, speed)

ch-11/2-straight-line-speed/pid.py

@@ -1,39 +0,0 @@
-class PID:
-    def __init__(self, proportional_k, integral_k, differential_k, set_point):
-        self.proportional_k = proportional_k
-        self.integral_k = integral_k
-        self.differential_k = differential_k
-        self.set_point = set_point
-
-        self.error_sum = 0
-        self.last_value = 0
-        self.min_output = -1
-        self.max_output = 1
-
-        self.dead_zone = 0.3
-
-    def update(self, measurement, time_delta):
-        error_value = measurement - self.set_point
-        proportional = error_value * self.proportional_k
-
-        # calculate integral
-        self.error_sum += error_value * time_delta
-        # clamp it
-        self.error_sum = min(self.max_output, self.error_sum)
-        self.error_sum = max(self.min_output, self.error_sum)
-
-        integral = self.error_sum * self.integral_k
-
-        differentiated_error = (error_value - self.last_value) / time_delta
-        differential = differentiated_error * self.differential_k
-        self.last_value = error_value
-
-        output = proportional + integral + differential
-        # clamp output
-        if abs(output) < self.dead_zone:
-            output = 0
-        else:
-            output = min(self.max_output, output)
-            output = max(self.min_output, output)
-
-        return output

ch-11/2-straight-line-speed/robot_wifi.py

@@ -1,25 +0,0 @@
-import board
-import busio
-from digitalio import DigitalInOut
-from adafruit_esp32spi import adafruit_esp32spi
-from adafruit_esp32spi import adafruit_esp32spi_wifimanager
-
-try:
-    from secrets import secrets
-except ImportError:
-    print("WiFi secrets are kept in secrets.py, please add them there!")
-    raise
-
-
-def connect_to_wifi():
-    esp32_cs = DigitalInOut(board.GP10)
-    esp32_ready = DigitalInOut(board.GP9)
-    esp32_reset = DigitalInOut(board.GP8)
-
-    spi = busio.SPI(board.GP14, MOSI=board.GP11, MISO=board.GP12)
-    esp = adafruit_esp32spi.ESP_SPIcontrol(spi, esp32_cs, esp32_ready, esp32_reset)
-    esp.reset()
-    wifi = adafruit_esp32spi_wifimanager.ESPSPI_WiFiManager(esp, secrets)
-    wifi.connect()
-
-    return wifi, esp

ch-11/2-straight-line-speed/speed_control.py

@@ -1,109 +0,0 @@
-import time
-import json
-
-from adafruit_esp32spi import adafruit_esp32spi_wsgiserver
-from adafruit_wsgi.wsgi_app import WSGIApp
-
-import robot
-import robot_wifi
-import pid
-
-
-class SpeedControlApp:
-    def __init__(self):
-        self.wifi = None
-        self.server = None
-
-        self.intended_speed = 0.9
-        self.last_time = time.monotonic()
-        self.speed_to_encoder_factor = 1/5100
-
-        self.left_speed_pid = pid.PID(0, -0.7, 0, self.intended_speed)
-        self.right_speed_pid = pid.PID(0, -0.7, 0, self.intended_speed)
-        self.left_pid_output = 0
-        self.right_pid_output = 0
-        self.left_speed = 0
-        self.right_speed = 0
-
-
-    def setup_wifi(self, app):
-        print("Setting up wifi.")
-        self.wifi, esp = robot_wifi.connect_to_wifi()
-        self.server = adafruit_esp32spi_wsgiserver.WSGIServer(80, application=app)
-        adafruit_esp32spi_wsgiserver.set_interface(esp)
-        print("Starting server")
-
-        self.server.start()
-        ip_int = ".".join(str(int(n)) for n in esp.ip_address)
-        print(f"IP Address is {ip_int}")
-
-    def update(self):
-        new_time = time.monotonic()
-        time_delta = new_time - self.last_time
-        self.last_time = new_time
-
-        self.left_speed = robot.left_encoder.get_speed(time_delta) * self.speed_to_encoder_factor
-        self.right_speed = robot.right_encoder.get_speed(time_delta) * self.speed_to_encoder_factor
-
-        self.left_pid_output = self.left_speed_pid.update(self.left_speed, time_delta)
-        self.right_pid_output = self.right_speed_pid.update(self.right_speed, time_delta)
-
-        # print({
-        #         "left_speed": self.left_speed,
-        #         "left_pid": self.left_pid_output,
-        #         "right_speed": self.right_speed,
-        #         "right_pid": self.right_pid_output,
-        #         "time": self.last_time,
-        #         "error_sum": self.left_speed_pid.error_sum
-        #     })
-
-        # robot.set_left(self.left_pid_output)
-        # robot.set_right(self.right_pid_output)
-
-    def movement_generator(self):
-        while True:
-            self.update()
-            data = json.dumps(
-                    {
-                        "left_speed": self.left_speed,
-                        "left_pid": self.left_pid_output,
-                        "right_speed": self.right_speed,
-                        "right_pid": self.right_pid_output,
-                        "time": self.last_time,
-                    }
-                ) + "/n"
-            print(data)
-            yield data
-
-    def index(self, request):
-        return (
-            200,
-            [("Content-Type", "application/json")],
-            self.movement_generator(),
-        )
-
-    def main_loop(self):
-        while True:
-            try:
-                self.update()
-
-                # time.sleep(0.1)
-                self.server.update_poll()
-            except RuntimeError as e:
-                print(f"Server poll error: {type(e)}, {e}")
-                print(f"Resetting ESP...")
-                self.wifi.reset()
-                print("Reset complete.")
-
-    def start(self):
-        app = WSGIApp()
-        app.route("/")(self.index)
-        print("Starting")
-        try:
-            self.setup_wifi(app)
-            self.main_loop()
-        finally:
-            robot.stop()
-
-
-SpeedControlApp().start()

ch-11/3-known-distance-acceleration/code.py

@@ -0,0 +1,93 @@
+import asyncio
+import time
+import robot
+import pid_controller
+
+
+class DistanceController:
+  def __init__(self, encoder, motor_fn):
+    self.encoder = encoder
+    self.motor_fn = motor_fn
+    # accel
+    # self.pid = pid_controller.PIDController(0.00000, 0,	0.00008, d_filter_gain=1) 
+    self.pid = pid_controller.PIDController(0.00000, 0.0000,	0.00001, d_filter_gain=1) 
+    self.start_ticks = self.encoder.read()
+    self.pwm = 0
+    self.error = 0
+
+  def update(self, dt, expected, debug=False):
+    actual = self.encoder.read() - self.start_ticks
+    # calculate the error
+    self.error = expected - actual
+
+    # calculate the control signal
+    control_signal = self.pid.calculate(self.error, dt)
+    print(control_signal)
+    # self.pwm += control_signal
+    if debug:
+      robot.uart.write(f"0, {expected:.2f},{actual:.2f}\n".encode())
+    # self.motor_fn(self.pwm)
+    self.motor_fn(control_signal)
+
+class DistanceTracker:
+  def __init__(self):
+    self.speed = 0.10
+    self.time_interval = 0.2
+    self.reset()
+
+  def reset(self):
+    self.start_time = time.monotonic()
+    self.total_distance_in_ticks = 0
+    self.total_time = 0.1
+
+  def set_distance(self, new_distance):
+    self.reset()
+    self.total_distance_in_ticks = robot.mm_to_ticks(new_distance * 1000)
+    self.total_time = new_distance / self.speed
+
+  async def loop(self):
+    left = DistanceController(robot.left_encoder, robot.set_left)
+    right = DistanceController(robot.right_encoder, robot.set_right)
+    last_time = time.monotonic()
+    while True:
+      await asyncio.sleep(self.time_interval)
+      current_time = time.monotonic()
+      dt = current_time - last_time
+      last_time = current_time
+      elapsed_time = current_time - self.start_time
+      time_proportion = min(1, elapsed_time / self.total_time)
+      expected = time_proportion * self.total_distance_in_ticks
+      left.update(dt, expected, debug=True)
+      right.update(dt, expected)
+
+
+distance_tracker = DistanceTracker()
+
+
+async def command_handler():
+  while True:
+    if robot.uart.in_waiting:
+      command = robot.uart.readline().decode().strip()
+      # PID settings
+      if command.startswith("M"):
+        distance_tracker.speed = float(command[1:])
+      elif command.startswith("T"):
+        distance_tracker.time_interval = float(command[1:])
+      # Start/stop commands
+      elif command == "O":
+        distance_tracker.set_distance(0)
+      elif command.startswith("O"):
+        await asyncio.sleep(5)
+        distance_tracker.set_distance(float(command[1:]))
+      # Print settings
+      elif command.startswith("?"):
+        robot.uart.write(f"M{distance_tracker.speed:.1f}\n".encode())
+        robot.uart.write(f"T{distance_tracker.time_interval:.1f}\n".encode())
+        await asyncio.sleep(3)
+    await asyncio.sleep(0)
+
+try:
+  asyncio.create_task(distance_tracker.loop())
+  asyncio.run(command_handler())
+finally:
+  robot.stop()

ch-11/3-known-distance-acceleration/pid_controller.py

@@ -0,0 +1,27 @@
+class PIDController:
+    def __init__(self, kp, ki, kd, d_filter_gain=0.1, imax=None, imin=None):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.d_filter_gain = d_filter_gain
+        self.imax = imax
+        self.imin = imin
+        self.reset()
+
+    def reset(self):
+        self.integral = 0
+        self.error_prev = 0
+        self.derivative = 0
+
+    def calculate(self, error, dt):
+        self.integral += error * dt
+        if self.imax is not None and self.integral > self.imax:
+            self.integral = self.imax
+        if self.imin is not None and self.integral < self.imin:
+            self.integral = self.imin
+        # Add a low pass filter to the difference
+        difference = (error - self.error_prev) * self.d_filter_gain
+        self.error_prev += difference
+        self.derivative = difference / dt
+
+        return self.kp * error + self.ki * self.integral + self.kd * self.derivative

ch-11/3-known-distance-acceleration/pio_encoder.py

@@ -0,0 +1,84 @@
+import rp2pio
+import adafruit_pioasm
+import array
+import asyncio
+
+
+program = """
+; use the osr for count
+; input pins c1 c2
+
+    set y, 0            ; clear y
+    mov osr, y          ; and clear osr
+read:
+    ; x will be the old value
+    ; y the new values
+    mov x, y            ; store old Y in x
+    in null, 32         ; Clear ISR - using y
+    in pins, 2          ; read two pins into y
+    mov y, isr
+    jmp x!=y, different ; Jump if its different
+    jmp read            ; otherwise loop back to read
+
+different:
+    ; x has old value, y has new.
+    ; extract the upper bit of X.
+    in x, 31             ; get bit 31 - old p1 (remember which direction it came in)
+    in null, 31         ; keep only 1 bit
+    mov x, isr          ; put this back in x
+    jmp !x, c1_old_zero
+
+c1_old_not_zero:
+    jmp pin, count_up
+    jmp count_down
+
+c1_old_zero:
+    jmp pin, count_down
+    ; fall through
+count_up:
+    ; for a clockwise move - we'll add 1 by inverting
+    mov x, ~ osr        ; store inverted OSR on x
+    jmp x--, fake       ; use jump to take off 1
+fake:
+    mov x, ~ x          ; invert back
+    jmp send
+count_down:
+    ; for a clockwise move, just take one off
+    mov x, osr          ; store osr in x
+    jmp x--, send       ; dec and send
+send:
+    ; send x.
+    mov isr, x          ; send it
+    push noblock        ; put ISR into input FIFO
+    mov osr, x          ; put X back in OSR
+    jmp read            ; loop back
+"""
+
+assembled = adafruit_pioasm.assemble(program)
+
+
+class QuadratureEncoder:
+    def __init__(self, first_pin, second_pin, reversed=False):
+        """Encoder with 2 pins. Must use sequential pins on the board"""
+        self.sm = rp2pio.StateMachine(
+            assembled,
+            frequency=0,
+            first_in_pin=first_pin,
+            jmp_pin=second_pin,
+            in_pin_count=2,
+        )
+        self.reversed = reversed
+        self._buffer = array.array("i", [0])
+        asyncio.create_task(self.poll_loop())
+
+    async def poll_loop(self):
+        while True:
+            await asyncio.sleep(0)
+            while self.sm.in_waiting:
+                self.sm.readinto(self._buffer)
+
+    def read(self):
+        if self.reversed:
+            return -self._buffer[0]
+        else:
+            return self._buffer[0]

ch-11/3-known-distance-acceleration/robot.py

@@ -0,0 +1,73 @@
+import board
+import pwmio
+import pio_encoder
+import busio
+import adafruit_vl53l1x
+import math
+import busio
+
+uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
+
+wheel_diameter_mm = 70
+wheel_circumference_mm = math.pi * wheel_diameter_mm
+gear_ratio = 298
+encoder_poles = 28
+ticks_per_revolution = encoder_poles * gear_ratio
+ticks_to_mm_const = wheel_circumference_mm / ticks_per_revolution
+
+def ticks_to_mm(ticks):
+    return ticks_to_mm_const * ticks
+
+def mm_to_ticks(mm):
+    return mm / ticks_to_mm_const
+
+
+motor_A2 = pwmio.PWMOut(board.GP17, frequency=100)
+motor_A1 = pwmio.PWMOut(board.GP16, frequency=100)
+motor_B2 = pwmio.PWMOut(board.GP18, frequency=100)
+motor_B1 = pwmio.PWMOut(board.GP19, frequency=100)
+
+right_motor = motor_A1, motor_A2
+left_motor = motor_B1, motor_B2
+
+right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21)
+left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27, reversed=True)
+
+i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1)
+i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)
+
+left_distance = adafruit_vl53l1x.VL53L1X(i2c0)
+right_distance = adafruit_vl53l1x.VL53L1X(i2c1)
+
+
+def stop():
+    motor_A1.duty_cycle = 0
+    motor_A2.duty_cycle = 0
+    motor_B1.duty_cycle = 0
+    motor_B2.duty_cycle = 0
+
+
+def set_speed(motor, speed):
+    # Swap motor pins if we reverse the speed
+    if abs(speed) < 0.1:
+        motor[0].duty_cycle = 0
+        motor[1].duty_cycle = 1
+        return
+    if speed < 0:
+        direction = motor[1], motor[0]
+        speed = -speed
+    else:
+        direction = motor
+    speed = min(speed, 1)  # limit to 1.0
+    max_speed = 2 ** 16 - 1
+
+    direction[0].duty_cycle = int(max_speed * speed)
+    direction[1].duty_cycle = 0
+
+
+def set_left(speed):
+    set_speed(left_motor, speed)
+
+
+def set_right(speed):
+    set_speed(right_motor, speed)

ch-11/_test_encoder_output/code.py

@@ -0,0 +1,34 @@
+import time
+import board
+import busio
+import robot
+
+uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
+
+speed = 0.7
+stop_time = 0
+
+while True:
+  current_time = time.monotonic()
+  if current_time > stop_time:
+    robot.set_left(0)
+    robot.set_right(0)
+  left_value = robot.left_encoder.read()
+  right_value = robot.right_encoder.read()
+  uart.write(f"{left_value:.3f},{right_value:.3f}\n".encode() )
+  time.sleep(0.02)
+  if uart.in_waiting:
+    command = uart.readline().decode().strip()
+    if command.startswith("M"):
+      speed = float(command[1:])
+    elif command == "O":
+      stop_time = 0
+      robot.set_left(0)
+      robot.set_right(0)
+    elif command.startswith("O"):
+      stop_time = float(command[1:]) + current_time
+      robot.set_left(speed)
+      robot.set_right(speed)
+    elif command.startswith("?"):
+      uart.write(f"M{speed:.1f}\n".encode())
+      time.sleep(3)

ch-11/_test_encoder_output/lpf.py

@@ -0,0 +1,8 @@
+class LPF:
+  """Low pass filter."""
+  def __init__(self, alpha):
+    self.alpha = alpha
+    self.last = 0
+  def update(self, value):
+    self.last = self.alpha * value + (1 - self.alpha) * self.last
+    return self.last

ch-11/_test_encoder_output/pid_controller.py

@@ -0,0 +1,20 @@
+class PIDController:
+    def __init__(self, kp, ki, kd, d_filter_gain=0.1):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.d_filter_gain = d_filter_gain
+
+        self.integral = 0
+        self.error_prev = 0
+        self.derivative = 0
+
+    def calculate(self, error, dt):
+        self.integral += error * dt
+
+        # Add a low pass filter to the difference
+        difference = (error - self.error_prev) * self.d_filter_gain
+        self.error_prev += difference
+        self.derivative = difference / dt
+
+        return self.kp * error + self.ki * self.integral + self.kd * self.derivative

ch-11/_test_encoder_output/pio_encoder.py

@@ -67,7 +67,6 @@ class QuadratureEncoder:
         )
         self.reversed = reversed
         self._buffer = array.array("i", [0])
-        self.previous_reading = 0
 
     def read(self):
         while self.sm.in_waiting:
@@ -76,9 +75,3 @@ class QuadratureEncoder:
             return -self._buffer[0]
         else:
             return self._buffer[0]
-
-    def get_speed(self, delta_time):
-        new_read = self.read()
-        distance = new_read - self.previous_reading
-        self.previous_reading = new_read
-        return distance / delta_time

ch-11/_test_encoder_output/robot.py

@@ -3,12 +3,20 @@ import pwmio
 import pio_encoder
 import busio
 import adafruit_vl53l1x
+import math
 
+wheel_diameter_mm = 70
+wheel_circumference_mm = math.pi * wheel_diameter_mm
+ticks_per_revolution = 2800
+ticks_to_mm_const = wheel_circumference_mm / ticks_per_revolution
 
-motor_A1 = pwmio.PWMOut(board.GP17)
-motor_A2 = pwmio.PWMOut(board.GP16)
-motor_B1 = pwmio.PWMOut(board.GP18)
-motor_B2 = pwmio.PWMOut(board.GP19)
+def ticks_to_mm(ticks):
+    return ticks_to_mm_const * ticks
+
+motor_A1 = pwmio.PWMOut(board.GP17, frequency=100)
+motor_A2 = pwmio.PWMOut(board.GP16, frequency=100)
+motor_B1 = pwmio.PWMOut(board.GP18, frequency=100)
+motor_B2 = pwmio.PWMOut(board.GP19, frequency=100)
 
 right_motor = motor_A1, motor_A2
 left_motor = motor_B1, motor_B2

ch-11/pc-live-graph/live_graph.py

@@ -1,59 +0,0 @@
-""" Turn JSON data stream into graphs"""
-import requests
-import json
-
-import matplotlib.pyplot as plt
-from matplotlib.animation import FuncAnimation
-
-url = "http://192.168.1.128"
-
-
-class AnimatedGraph:
-    def __init__(self):
-        self.fields = {}
-        self.samples = 100
-        self.reset()
-        print("getting source to iterate over")
-        self.source = self._graph_source()
-        print("init completed")
-
-    def reset(self):
-        for field in self.fields:
-            self.fields[field] = []
-
-    def _graph_source(self):
-        while True:
-            try:
-                with requests.get(url, timeout=1, stream=True) as response:
-                    print(f"status: {response.status_code}")
-                    yield from response.iter_lines()
-            except requests.exceptions.RequestException:
-                pass
-
-    def make_frame(self, frame):
-        print("loading next item")
-        item = json.loads(next(self.source))
-        print("item loaded")
-        if 'time' in self.fields and item["time"] < self.fields['time'][-1]:
-            self.reset()
-        for field in item:
-            if field not in self.fields:
-                self.fields[field] = []
-            self.fields[field].append(item[field])
-
-        if len(self.fields['time'] ) > self.samples:
-            for field in self.fields:
-                self.fields[field] = self.fields[field][-self.samples:]
-
-        plt.cla()  # clear axes.
-        # plot the items
-        for field in self.fields:
-            if field != "time":
-                plt.plot("time", field, data=self.fields)
-
-        plt.legend(loc="upper right")
-
-# Create the animation. gcf - get current figure. random_stream - callback func.
-animation = FuncAnimation(plt.gcf(), AnimatedGraph().make_frame, interval=200)
-plt.tight_layout()
-plt.show()

ch-11/pc-live-graph/requirements.txt

@@ -1,2 +0,0 @@
-matplotlib
-requests