Current state of experiments

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.control_signal = 0
+    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
-    speed_in_m_per_s = robot.ticks_to_mm(speed_in_ticks) / 1000
-    # calculate the error  
-    self.error = (Settings.speed * Settings.motors_enabled) - speed_in_m_per_s
+    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
-    self.control_signal = self.pid.calculate(self.error, dt)
-    self.motor_fn(self.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)
@@ -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("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:])
+      if command.startswith("M"):
+        Settings.speed = 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