Update PID coefficients at any time

Cargo.lock

@@ -305,7 +305,7 @@ dependencies = [
 
 [[package]]
 name = "controller"
-version = "1.3.1"
+version = "1.3.2"
 dependencies = [
  "bootloader-params",
  "build-support",

README.md

@@ -240,10 +240,6 @@ then the right motor coefficients.
 - 0x21: K_I (4 bytes, or 2×4 bytes). Default value is 0x0000_0000 (0.01171875)
 - 0x22: K_D (4 bytes, or 2×4 bytes). Default value is 0x0000_0000 (0.0)
 
-Changing the coefficients will take effect only when the controlled
-mode is off, for example by setting the motors to standby mode. Even a
-speed of (0, 0) in controlled mode will not use the new coefficients.
-
 ## 0x26 PID I accumulator (R)
 
 In controlled mode, return the PID I accumulator for each wheel (left

controller/Cargo.toml

@@ -4,7 +4,7 @@ description = "Firmware for DC Motor Driver Hat DFR0592"
 authors.workspace = true
 edition.workspace = true
 license.workspace = true
-version = "1.3.1"  # Update in python/controller.py and tele0592/src/controller.rs as well
+version = "1.3.2"  # Update in python/controller.py and tele0592/src/controller.rs as well
 
 [[bin]]
 name = "controller-firmware"

controller/src/logic/mod.rs

@@ -134,10 +134,12 @@ fn process_command(
         &[CMD_PID_K_P, a, b, c, d] => {
             pid_control::LEFT_K_P.store(i32::from_le_bytes([a, b, c, d]), Ordering::Relaxed);
             pid_control::RIGHT_K_P.store(i32::from_le_bytes([a, b, c, d]), Ordering::Relaxed);
+            pid_control::mark_updated();
         }
         &[CMD_PID_K_P, a, b, c, d, e, f, g, h] => {
             pid_control::LEFT_K_P.store(i32::from_le_bytes([a, b, c, d]), Ordering::Relaxed);
             pid_control::RIGHT_K_P.store(i32::from_le_bytes([e, f, g, h]), Ordering::Relaxed);
+            pid_control::mark_updated();
         }
         [CMD_PID_K_P] => {
             response
@@ -150,10 +152,12 @@ fn process_command(
         &[CMD_PID_K_I, a, b, c, d] => {
             pid_control::LEFT_K_I.store(i32::from_le_bytes([a, b, c, d]), Ordering::Relaxed);
             pid_control::RIGHT_K_I.store(i32::from_le_bytes([a, b, c, d]), Ordering::Relaxed);
+            pid_control::mark_updated();
         }
         &[CMD_PID_K_I, a, b, c, d, e, f, g, h] => {
             pid_control::LEFT_K_I.store(i32::from_le_bytes([a, b, c, d]), Ordering::Relaxed);
             pid_control::RIGHT_K_I.store(i32::from_le_bytes([e, f, g, h]), Ordering::Relaxed);
+            pid_control::mark_updated();
         }
         [CMD_PID_K_I] => {
             response
@@ -166,10 +170,12 @@ fn process_command(
         &[CMD_PID_K_D, a, b, c, d] => {
             pid_control::LEFT_K_D.store(i32::from_le_bytes([a, b, c, d]), Ordering::Relaxed);
             pid_control::RIGHT_K_D.store(i32::from_le_bytes([a, b, c, d]), Ordering::Relaxed);
+            pid_control::mark_updated();
         }
         &[CMD_PID_K_D, a, b, c, d, e, f, g, h] => {
             pid_control::LEFT_K_D.store(i32::from_le_bytes([a, b, c, d]), Ordering::Relaxed);
             pid_control::RIGHT_K_D.store(i32::from_le_bytes([e, f, g, h]), Ordering::Relaxed);
+            pid_control::mark_updated();
         }
         [CMD_PID_K_D] => {
             response

controller/src/logic/pid_control.rs

@@ -18,10 +18,21 @@ pub static RIGHT_K_D: AtomicI32 = AtomicI32::new(0x00); // 0.0
 static LEFT_INTEGRATED_ERROR: AtomicI32 = AtomicI32::new(0x00);
 static RIGHT_INTEGRATED_ERROR: AtomicI32 = AtomicI32::new(0x00);
 
+// Set when the coefficients have been updated and the PID should
+// be reset when the robot is idle. Starting with a `true` value
+// ensure that the default coefficients will be loaded when the
+// PID process starts.
+static PID_UPDATED: AtomicBool = AtomicBool::new(true);
+
 static CONTROLLED_MODE: AtomicBool = AtomicBool::new(false);
 static LEFT_SPEED: AtomicI16 = AtomicI16::new(0);
 static RIGHT_SPEED: AtomicI16 = AtomicI16::new(0);
 
+/// Note that some PID coefficients have been updated
+pub fn mark_updated() {
+    PID_UPDATED.store(true, Ordering::Relaxed);
+}
+
 pub fn disable_controlled_mode() {
     CONTROLLED_MODE.store(false, Ordering::Relaxed);
 }
@@ -55,13 +66,8 @@ pub async fn controlled_mode() {
     let (mut left_pid, mut right_pid) = (Pid::<PidUnit>::default(), Pid::<PidUnit>::default());
     loop {
         ticker.next().await;
-        let new_controlled_mode = CONTROLLED_MODE.load(Ordering::Relaxed);
-        if !new_controlled_mode {
-            controlled_mode = false;
-            continue;
-        }
-        if !controlled_mode {
-            controlled_mode = true;
+        if PID_UPDATED.load(Ordering::Relaxed) {
+            PID_UPDATED.store(false, Ordering::Relaxed);
             left_pid = Pid::new(
                 PidUnit::from_raw(LEFT_K_P.load(Ordering::Relaxed)),
                 PidUnit::from_raw(LEFT_K_I.load(Ordering::Relaxed)),
@@ -73,10 +79,19 @@ pub async fn controlled_mode() {
                 PidUnit::from_raw(RIGHT_K_D.load(Ordering::Relaxed)),
             );
             relative = Relative::new(encoders);
-            // Wait for a full cycle when starting to avoid an
-            // insufficient number of ticks.
+        }
+        let old_controlled_mode = controlled_mode;
+        controlled_mode = CONTROLLED_MODE.load(Ordering::Relaxed);
+        if !controlled_mode {
             continue;
         }
+        if !old_controlled_mode {
+            left_pid.reset();
+            right_pid.reset();
+            LEFT_INTEGRATED_ERROR.store(0, Ordering::Relaxed);
+            RIGHT_INTEGRATED_ERROR.store(0, Ordering::Relaxed);
+            relative = Relative::new(encoders);
+        }
         let (left_ticks, right_ticks) = relative.ticks(encoders);
         let left_command = build_speed_command(&mut left_pid, &LEFT_SPEED, left_ticks);
         let right_command = build_speed_command(&mut right_pid, &RIGHT_SPEED, right_ticks);