Current state of experiments

ch-11/1-measuring_wheel_speeds/code.py

@@ -1,8 +1,5 @@
 import asyncio
-import board
-import busio
 import robot
-uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
 
 class Settings:
   speed = 0.7
@@ -17,7 +14,7 @@ async def motor_speed_loop():
     left_last = left_new
     right_speed = robot.ticks_to_mm(right_new - right_last) / Settings.time_interval
     right_last = right_new
-    uart.write(f"{left_speed:.3f},{right_speed:.3f},0\n".encode())
+    robot.uart.write(f"{left_speed:.3f},{right_speed:.3f},0\n".encode())
 
 async def stop_motors_after(seconds):
   await asyncio.sleep(seconds)
@@ -25,8 +22,8 @@ async def stop_motors_after(seconds):
 
 async def command_handler():
   while True:
-    if uart.in_waiting:
+    if robot.uart.in_waiting:
-      command = uart.readline().decode().strip()
+      command = robot.uart.readline().decode().strip()
       if command.startswith("M"):
         Settings.speed = float(command[1:])
       elif command.startswith("T"):
@@ -35,12 +32,12 @@ async def command_handler():
         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)
-        asyncio.create_task(stop_motors_after(float(command[1:])))
       elif command.startswith("?"):
-        uart.write(f"M{Settings.speed:.1f}\n".encode())
+        robot.uart.write(f"M{Settings.speed:.1f}\n".encode())
-        uart.write(f"T{Settings.time_interval:.1f}\n".encode())
+        robot.uart.write(f"T{Settings.time_interval:.1f}\n".encode())
         await asyncio.sleep(3)
     await asyncio.sleep(0)
 

ch-11/1-measuring_wheel_speeds/robot.py

@@ -4,6 +4,9 @@ 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

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

@@ -1,100 +0,0 @@
-import asyncio
-import board
-import busio
-import robot
-import time
-
-import pid_controller
-uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
-
-class Settings:
-  # work in ticks only  - 1 rpm
-  speed = 0.5
-  time_interval = 0.2
-  motors_enabled = False
-  dead_zone = 0.2
-
-class SpeedController:
-  def __init__(self, encoder, motor_fn) -> None:
-    self.encoder = encoder
-    self.motor_fn = motor_fn
-    #P1.4,I2.8,D1
-    #P0, I2.5, D1.1
-    self.pid = pid_controller.PIDController(0.1, 1.5, 0)
-    self.reset()
-
-  def reset(self):
-    self.last_ticks = self.encoder.read()
-    self.error = 0
-    self.control_signal = 0
-    self.pid.reset()
-
-  def update(self, dt):
-    current_ticks = self.encoder.read()
-    actual_speed_in_rpm = (current_ticks - self.last_ticks) / (dt * robot.ticks_per_revolution)
-    self.last_ticks = current_ticks
-    # calculate the error  
-    self.error = (Settings.speed * Settings.motors_enabled) - actual_speed_in_rpm
-    # calculate the control signal
-    self.control_signal = self.pid.calculate(self.error, dt)
-    self.motor_fn(self.control_signal)
-    
-
-left = SpeedController(robot.left_encoder, robot.set_left)
-
-async def speed_controller_loop():
-  last_time = time.monotonic()
-  while True:
-    await asyncio.sleep(Settings.time_interval)
-    current_time = time.monotonic()
-    dt = current_time - last_time
-    left.update(dt)
-    last_time = current_time
-    print(f"{dt:.2f}")
-    uart.write(f"{Settings.speed * Settings.motors_enabled}, {left.error:.2f},{left.control_signal:.2f}\n".encode())
-
-
-async def stop_motors_after(seconds):
-  await asyncio.sleep(seconds)
-  Settings.motors_enabled = False
-  # robot.stop()
-
-async def command_handler():
-  while True:
-    if uart.in_waiting:
-      command = uart.readline().decode().strip()
-      # PID settings
-      if command.startswith("P"):
-        left.pid.kp = float(command[1:])
-      elif command.startswith("I"):
-        left.pid.ki = float(command[1:])
-        left.pid.reset()
-      elif command.startswith("D"):
-        left.pid.kd = float(command[1:])
-      # Speed settings
-      elif command.startswith("M"):
-        Settings.speed = float(command[1:])
-      elif command.startswith("T"):
-        Settings.time_interval = float(command[1:])
-      # Start/stop commands
-      elif command == "O":
-        Settings.motors_enabled = False
-      elif command.startswith("O"):
-        asyncio.create_task(stop_motors_after(float(command[1:])))
-        Settings.motors_enabled = True
-        left.reset()
-      # Print settings
-      elif command.startswith("?"):
-        uart.write(f"M{Settings.speed:.1f}\n".encode())
-        uart.write(f"T{Settings.time_interval:.1f}\n".encode())
-        uart.write(f"P{left.pid.kp:.2f}:I{left.pid.ki:.2f}:D{left.pid.kd:.2f}\n".encode())
-        await asyncio.sleep(3)      
-    await asyncio.sleep(0)
-
-try:
-  asyncio.create_task(speed_controller_loop())
-  asyncio.create_task(stop_motors_after(10))
-  Settings.motors_enabled = True
-  asyncio.run(command_handler())
-finally:
-  robot.stop()

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

@@ -1,27 +0,0 @@
-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/1.5-speed-control/pio_encoder.py

@@ -1,84 +0,0 @@
-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/1.5-speed-control/robot.py

@@ -1,68 +0,0 @@
-import board
-import pwmio
-import pio_encoder
-import busio
-import adafruit_vl53l1x
-import math
-
-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
-motor_dead_zone = 0.2
-
-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):
-    # stop completely if in the dead zone
-    if abs(speed) < motor_dead_zone:
-        motor[0].duty_cycle = 0
-        motor[1].duty_cycle = 0
-        return
-    # Swap motor pins if we reverse the speed
-    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-speed-control-acceleration/code.py

@@ -1,103 +0,0 @@
-import asyncio
-import time
-import robot
-import pid_controller
-
-class Settings:
-  speed = 0.17
-  distance = 1
-  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.error = 0
-    self.speed = 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  
-    self.error = (Settings.speed * Settings.motors_enabled) - self.actual_speed
-    # calculate the control signal
-    control_signal = self.pid.calculate(self.error, dt)
-    self.speed += control_signal
-    self.motor_fn(self.speed)
-
-
-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:.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()
-      # PID settings
-      if command.startswith("P"):
-        left.pid.kp = float(command[1:])
-        right.pid.kp = float(command[1:])
-      elif command.startswith("I"):
-        left.pid.ki = float(command[1:])
-        left.pid.reset()
-        right.pid.ki = float(command[1:])
-        right.pid.reset()
-      elif command.startswith("D"):
-        left.pid.kd = float(command[1:])
-        right.pid.kd = float(command[1:])
-      elif command.startswith("T"):
-        Settings.time_interval = float(command[1:])
-      # Speed settings
-      elif command.startswith("M"):
-        Settings.speed = float(command[1:])
-      # Start/stop commands
-      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"P{left.pid.kp:.2f}:I{left.pid.ki:.2f}:D{left.pid.kd:.2f}\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-acceleration/pid_controller.py

@@ -1,27 +0,0 @@
-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-acceleration/pio_encoder.py

@@ -1,84 +0,0 @@
-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-acceleration/robot.py

@@ -1,65 +0,0 @@
-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 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-speed-control/code.py

@@ -13,26 +13,27 @@ class SpeedController:
   def __init__(self, encoder, motor_fn):
     self.encoder = encoder
     self.motor_fn = motor_fn
-    self.pid = pid_controller.PIDController(0, 6, 0)
+    self.pid = pid_controller.PIDController(3, 0, 1)
     self.reset()
 
   def reset(self):
     self.last_ticks = self.encoder.read()
-    self.error = 0
+    self.pwm = 0
-    self.control_signal = 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
-    speed_in_m_per_s = robot.ticks_to_mm(speed_in_ticks) / 1000
+    self.actual_speed = robot.ticks_to_mm(speed_in_ticks) / 1000
-    # calculate the error  
+    # calculate the error
-    self.error = (Settings.speed * Settings.motors_enabled) - speed_in_m_per_s
+    error = (Settings.speed * Settings.motors_enabled) - self.actual_speed
     # calculate the control signal
-    self.control_signal = self.pid.calculate(self.error, dt)
+    control_signal = self.pid.calculate(error, dt)
-    self.motor_fn(self.control_signal)
+    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)
@@ -47,7 +48,7 @@ async def motor_speed_loop():
     last_time = current_time
     left.update(dt)
     right.update(dt)
-    robot.uart.write(f"0, {left.error:.2f},{right.error:.2f}\n".encode())
+    robot.uart.write(f"0, {left.actual_speed:.2f},{Settings.speed * Settings.motors_enabled:.2f}\n".encode())
 
 
 async def stop_motors_after(seconds):
@@ -59,24 +60,10 @@ async def command_handler():
   while True:
     if robot.uart.in_waiting:
       command = robot.uart.readline().decode().strip()
-      # PID settings
+      if command.startswith("M"):
-      if command.startswith("P"):
+        Settings.speed = float(command[1:])
-        left.pid.kp = float(command[1:])
-        right.pid.kp = float(command[1:])
-      elif command.startswith("I"):
-        left.pid.ki = float(command[1:])
-        left.pid.reset()
-        right.pid.ki = float(command[1:])
-        right.pid.reset()
-      elif command.startswith("D"):
-        left.pid.kd = float(command[1:])
-        right.pid.kd = float(command[1:])
       elif command.startswith("T"):
         Settings.time_interval = float(command[1:])
-      # Speed settings
-      elif command.startswith("M"):
-        Settings.speed = float(command[1:])
-      # Start/stop commands
       elif command == "O":
         Settings.motors_enabled = False
       elif command.startswith("O"):
@@ -88,7 +75,6 @@ async def command_handler():
       # Print settings
       elif command.startswith("?"):
         robot.uart.write(f"M{Settings.speed:.1f}\n".encode())
-        robot.uart.write(f"P{left.pid.kp:.2f}:I{left.pid.ki:.2f}:D{left.pid.kd:.2f}\n".encode())
         robot.uart.write(f"T{Settings.time_interval:.1f}\n".encode())
         await asyncio.sleep(3)
     await asyncio.sleep(0)

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

@@ -45,6 +45,10 @@ def stop():
 
 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

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

@@ -3,58 +3,65 @@ import time
 import robot
 import pid_controller
 
-class Settings:
-  speed = 0.17
-  time_interval = 0.2
-  motors_enabled = False
 
-
+class DistanceController:
-class SpeedController:
   def __init__(self, encoder, motor_fn):
     self.encoder = encoder
     self.motor_fn = motor_fn
-    self.pid = pid_controller.PIDController(3, 0, 1)
+    # 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.last_ticks = self.encoder.read()
+    self.start_time = time.monotonic()
-    self.error = 0
+    self.total_distance_in_ticks = 0
-    self.speed = 0
+    self.total_time = 0.1
-    self.actual_speed = 0
-    self.pid.reset()
 
-  def update(self, dt):
+  def set_distance(self, new_distance):
-    current_ticks = self.encoder.read()
+    self.reset()
-    speed_in_ticks = (current_ticks - self.last_ticks) / dt
+    self.total_distance_in_ticks = robot.mm_to_ticks(new_distance * 1000)
-    self.last_ticks = current_ticks
+    self.total_time = new_distance / self.speed
-    self.actual_speed = robot.ticks_to_mm(speed_in_ticks) / 1000
+
-    # calculate the error  
+  async def loop(self):
-    self.error = (Settings.speed * Settings.motors_enabled) - self.actual_speed
+    left = DistanceController(robot.left_encoder, robot.set_left)
-    # calculate the control signal
+    right = DistanceController(robot.right_encoder, robot.set_right)
-    control_signal = self.pid.calculate(self.error, dt)
+    last_time = time.monotonic()
-    self.speed += control_signal
+    while True:
-    self.motor_fn(self.speed)
+      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)
 
 
-left = SpeedController(robot.left_encoder, robot.set_left)
+distance_tracker = DistanceTracker()
-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:.2f}\n".encode())
-
-
-async def stop_motors_after(seconds):
-  await asyncio.sleep(seconds)
-  Settings.motors_enabled = False
 
 
 async def command_handler():
@@ -62,41 +69,25 @@ async def command_handler():
     if robot.uart.in_waiting:
       command = robot.uart.readline().decode().strip()
       # PID settings
-      if command.startswith("P"):
+      if command.startswith("M"):
-        left.pid.kp = float(command[1:])
+        distance_tracker.speed = float(command[1:])
-        right.pid.kp = float(command[1:])
-      elif command.startswith("I"):
-        left.pid.ki = float(command[1:])
-        left.pid.reset()
-        right.pid.ki = float(command[1:])
-        right.pid.reset()
-      elif command.startswith("D"):
-        left.pid.kd = float(command[1:])
-        right.pid.kd = float(command[1:])
       elif command.startswith("T"):
-        Settings.time_interval = float(command[1:])
+        distance_tracker.time_interval = float(command[1:])
-      # Speed settings
-      elif command.startswith("M"):
-        Settings.speed = float(command[1:])
       # Start/stop commands
       elif command == "O":
-        Settings.motors_enabled = False
+        distance_tracker.set_distance(0)
       elif command.startswith("O"):
         await asyncio.sleep(5)
-        asyncio.create_task(stop_motors_after(float(command[1:])))
+        distance_tracker.set_distance(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"M{distance_tracker.speed:.1f}\n".encode())
-        robot.uart.write(f"P{left.pid.kp:.2f}:I{left.pid.ki:.2f}:D{left.pid.kd:.2f}\n".encode())
+        robot.uart.write(f"T{distance_tracker.time_interval:.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.create_task(distance_tracker.loop())
   asyncio.run(command_handler())
 finally:
   robot.stop()

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

@@ -18,6 +18,10 @@ 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)
@@ -45,6 +49,10 @@ def stop():
 
 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