Current chapter 11

ch-11/3-known-distance/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
+    # self.pid = pid_controller.PIDController(1.9, 0.5,	0.3, d_filter_gain=1) 
+    self.pid = pid_controller.PIDController(3.25, 0.5,	0.5, d_filter_gain=1) 
+    # works well with direct pwm control
+    # started with p at 0, and started increasing until it started to oscillate
+    # then reduced.
+    # started increasing i in 0.1 increments, to take up the steady state error
+    # once this was gone, there was still an overshoot.
+    # increase d in 0.1 increments until the overshoot was gone. 
+    self.start_ticks = self.encoder.read()
+    self.error = 0
+
+  def update(self, dt, expected):
+    self.actual = self.encoder.read() - self.start_ticks
+    # calculate the error
+    self.error = (expected - self.actual) / robot.ticks_per_revolution
+    # calculate the control signal
+    control_signal = self.pid.calculate(self.error, dt)
+    self.motor_fn(control_signal)
+ 
+class DistanceTracker:
+  def __init__(self):
+    self.speed = 0.17
+    self.time_interval = 0.2
+    self.start_time = time.monotonic()
+    self.current_position = 0
+    self.total_distance_in_ticks = 0
+    self.total_time = 0.1
+
+  def set_distance(self, new_distance):
+    # add the last travelled distance to the current position
+    self.current_position += self.total_distance_in_ticks
+    # calculate the new additional distance
+    self.total_distance_in_ticks = robot.m_to_ticks * new_distance
+    self.total_time = max(0.1, abs(new_distance / self.speed))
+    self.start_time = time.monotonic()
+
+  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 + self.current_position
+      left.update(dt, expected)
+      right.update(dt, expected)
+      robot.uart.write(f"0, {expected:.2f},{left.actual:.2f}\n".encode())
+
+
+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/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/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/robot.py

@@ -0,0 +1,68 @@
+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_m = (wheel_circumference_mm / ticks_per_revolution) / 1000
+m_to_ticks = 1 / ticks_to_m
+
+
+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)