diff --git a/Cargo.lock b/Cargo.lock
index d856251ef3f0eebd4ab6c2b13ce9d4cb2a56849f..2752c4dcb3a22131fc3f010d77865c764f992a0a 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -305,7 +305,7 @@ dependencies = [
[[package]]
name = "controller"
-version = "1.3.1"
+version = "1.3.2"
dependencies = [
"bootloader-params",
"build-support",
diff --git a/README.md b/README.md
index 7ea6953f1a42e61d6985752710e297a7e34e9095..adf602a59f507d8904f87e6e5493a8d051d00c96 100644
--- a/README.md
+++ b/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
diff --git a/controller/Cargo.toml b/controller/Cargo.toml
index a349615b2d6e0c795c07a89002fcdc84e4f3b0da..73e49798b7fd178bbcbe92f10f19fe4c60c2ed09 100644
--- a/controller/Cargo.toml
+++ b/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"
diff --git a/controller/src/logic/mod.rs b/controller/src/logic/mod.rs
index 720f3193065ed10ffa0e2450ac2cc3a25e499bdb..c4e666b013fecc8ae4b2dbb043cd5d2d8b2986bf 100644
--- a/controller/src/logic/mod.rs
+++ b/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
diff --git a/controller/src/logic/pid_control.rs b/controller/src/logic/pid_control.rs
index f2605e5ca367e7c58dc8ef565ff4482deb539f6f..babda74076dbeb8e18f582b3c92a7f9cf1d65481 100644
--- a/controller/src/logic/pid_control.rs
+++ b/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);