Working speed control

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

@@ -1,23 +1,19 @@
 import asyncio
-import board
-import busio
-import robot
 import time
+import robot
 import pid_controller
-uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
-
 
 class Settings:
-  speed = 0.15
+  speed = 0.17
   time_interval = 0.2
   motors_enabled = False
 
 
 class SpeedController:
-  def __init__(self, encoder, motor_fn) -> None:
+  def __init__(self, encoder, motor_fn):
     self.encoder = encoder
     self.motor_fn = motor_fn
-    self.pid = pid_controller.PIDController(1, 6, 0)
+    self.pid = pid_controller.PIDController(0, 6, 0)
     self.reset()
 
   def reset(self):
@@ -48,20 +44,21 @@ async def motor_speed_loop():
     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)
-    last_time = current_time
+    robot.uart.write(f"0, {left.error:.2f},{right.error:.2f}\n".encode())
-    uart.write(f"0, {left.error:.2f},{right.error:.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:
+    if robot.uart.in_waiting:
-      command = uart.readline().decode().strip()
+      command = robot.uart.readline().decode().strip()
       # PID settings
       if command.startswith("P"):
         left.pid.kp = float(command[1:])
@@ -74,24 +71,25 @@ async def command_handler():
       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:])
-      elif command.startswith("T"):
-        Settings.time_interval = 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("?"):
-        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"P{left.pid.kp:.2f}:I{left.pid.ki:.2f}:D{left.pid.kd:.2f}\n".encode())
-        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

@@ -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
@@ -22,7 +25,6 @@ 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)
@@ -42,11 +44,6 @@ def stop():
 
 
 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]