Use a new speed scale from -100 to 100

controller/python/controller.py

@@ -5,7 +5,7 @@ from typing import Optional
 
 class Controller:
 
-    I2C_BUS = 1
+    I2C_BUS = 8
     I2C_ADDR = 0x57
 
     WHO_AM_I = 0x0F
@@ -14,7 +14,7 @@ class Controller:
     ENCODER_TICKS = 0x32
 
     def __init__(self):
-        self.i2c = smbus.SMBus(1)
+        self.i2c = smbus.SMBus(self.I2C_BUS)
 
     def who_am_i(self) -> int:
         """Check that the motors controller board is present. This should return
@@ -31,16 +31,16 @@ class Controller:
         )
 
     def set_motor_speed(self, left: Optional[int], right: Optional[int]):
-        """Set the motor speed between -127 and 127. None means not to change the
+        """Set the motor speed between -100 and 100. None means not to change the
         motor value. Using None for both motors will put the controller board in standby
         mode and motors will stop."""
 
         def convert(v, arg):
             if v is None:
                 return -128
-            if not isinstance(v, int) or v < -127 or v > 127:
+            if not isinstance(v, int) or v < -100 or v > 100:
                 raise ValueError(
-                    f"{arg} must be an integer between -127 and 127 or None"
+                    f"{arg} must be an integer between -100 and 100 or None"
                 )
             return v
 

controller/python/localpid.py

+from controller import Controller
+import time
+
+k_p = 0.3
+k_i = 0.005
+k_d = 0
+
+min_speed = 0
+precision = 10
+
+# distance and max_speed are in ticks
+def advance(c, distance, max_speed):
+    # Configure motor speed
+    c.set_max_percentage(100)
+    
+    # Current positions
+    pos_left = 0
+    pos_right = 0
+
+    # Error
+    prev_err_left, prev_err_right = 0, 0
+    acc_err_left, acc_err_right = 0, 0
+
+    # Reset ticks
+    c.get_encoder_ticks()
+
+    # Go until distance is reached
+    final_target = distance
+    target = 0
+    stable_for = 0
+    ramp_up_speed = 0
+    while True:
+        # Ramp-up the speed on four second
+        ramp_up_speed = min(ramp_up_speed + max_speed / 400, max_speed)
+        acceptable_speed = round(ramp_up_speed)
+        if final_target > 0:
+            target = min(target + acceptable_speed, final_target)
+        else:
+            target = max(target - acceptable_speed, final_target)
+        time.sleep(0.01)  # Every 1/100th of seconds
+        enc_left, enc_right = c.get_encoder_ticks()
+        print((enc_left, enc_right))
+        pos_left, pos_right = pos_left + enc_left, pos_right + enc_right
+
+        if abs(pos_left - final_target) <= precision and abs(pos_right - final_target) <= precision:
+            stable_for += 1
+            if stable_for >= 10:
+                c.set_motor_speed(None, None)
+                break
+        else:
+            stable_for = 0
+        
+        err_left, err_right = target - pos_left, target - pos_right
+        der_err_left, der_err_right = err_left - prev_err_left, err_right - prev_err_right
+        acc_err_left, acc_err_right = acc_err_left + err_left, acc_err_right + err_right
+        prev_err_left, prev_err_right = err_left, err_right
+        command_left = command(err_left, acc_err_left, der_err_left)
+        command_right = command(err_right, acc_err_right, der_err_right)
+        c.set_motor_speed(command_left, command_right)
+        print(f"target = {target}, pos = {(pos_left, pos_right)}, speed = {(command_left, command_right)}")
+        
+def command(err, acc, der):
+    speed = round(err * k_p + acc * k_i + der * k_d)
+    if speed < -100:
+        speed = -100
+    elif speed > -min_speed and speed < 0:
+        speed = -min_speed
+    elif speed > 0 and speed < min_speed:
+        speed = min_speed
+    elif speed > 100:
+        speed = 100
+    return speed
+
+c = Controller()
+assert(c.who_am_i() == 0x57)
+try:
+    advance(c, 20000, 50)
+    #advance(c, -10000, 50)
+except KeyboardInterrupt:
+    print("Stopping engine")
+    c.set_motor_speed(None, None)
+

controller/python/showencoders.py

+#! /usr/bin/env python
+#
+# Show encoders value as fast as possible
+
+from controller import Controller
+import time
+
+c = Controller()
+
+left, right = 0, 0
+while True:
+    dl, dr = c.get_encoder_ticks()
+    left, right = left + dl, right + dr
+    print((left, right))
+    

controller/src/logic.rs

@@ -72,8 +72,13 @@ fn handle_command(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
                 state.motors.max_pwm() as i32,
                 i32::from(state.max_motor_percentage),
             );
-            let scaled_speed = |speed| {
-                (speed != 0x80).then(|| i32::from(speed as i8) * max_pwm * mmp / (127 * 100))
+            let scaled_speed = |speed| match speed as i8 {
+                -128 => None,
+                speed @ (-100..=100) => Some(i32::from(speed) * max_pwm * mmp / (100 * 100)),
+                speed => {
+                    defmt::warn!("speed {} out of range [-100, 100]", speed);
+                    None
+                }
             };
             state.motor_speed = [ms[0], ms[1]];
             let (speed_left, speed_right) = (scaled_speed(ms[0]), scaled_speed(ms[1]));