From e9ab0c46827f84b07b319932189e12429ec92be5 Mon Sep 17 00:00:00 2001 From: Ruben Nitsche Date: Thu, 26 Oct 2023 12:24:36 +0200 Subject: [PATCH 1/6] Added dt handover --- src/lib.rs | 62 +++++++++++++++++++++++++++--------------------------- 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 54583ae..3573b32 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -210,12 +210,12 @@ where self } - /// Given a new measurement, calculates the next [control output](ControlOutput). + /// Given a new measurement and dt, calculates the next [control output](ControlOutput). /// /// # Panics /// /// - If a setpoint has not been set via `update_setpoint()`. - pub fn next_control_output(&mut self, measurement: T) -> ControlOutput { + pub fn next_control_output(&mut self, measurement: T, dt: T) -> ControlOutput { // Calculate the error between the ideal setpoint and the current // measurement to compare against let error = self.setpoint - measurement; @@ -229,7 +229,7 @@ where // just the error (no ki), because we support ki changing dynamically, // we store the entire term so that we don't need to remember previous // ki values. - self.integral_term = self.integral_term + error * self.ki; + self.integral_term = self.integral_term + error * self.ki * dt; // Mitigate integral windup: Don't want to keep building up error // beyond what i_limit will allow. @@ -240,7 +240,7 @@ where let d_unbounded = -match self.prev_measurement.as_ref() { Some(prev_measurement) => measurement - *prev_measurement, None => T::zero(), - } * self.kd; + } * self.kd / dt; self.prev_measurement = Some(measurement); let d = apply_limit(self.d_limit, d_unbounded); @@ -290,11 +290,11 @@ mod tests { assert_eq!(pid.setpoint, 10.0); // Test simple proportional - assert_eq!(pid.next_control_output(0.0).output, 20.0); + assert_eq!(pid.next_control_output(0.0,1.0).output, 20.0); // Test proportional limit pid.p_limit = 10.0; - assert_eq!(pid.next_control_output(0.0).output, 10.0); + assert_eq!(pid.next_control_output(0.0,1.0).output, 10.0); } /// Derivative-only controller operation and limits @@ -304,14 +304,14 @@ mod tests { pid.p(0.0, 100.0).i(0.0, 100.0).d(2.0, 100.0); // Test that there's no derivative since it's the first measurement - assert_eq!(pid.next_control_output(0.0).output, 0.0); + assert_eq!(pid.next_control_output(0.0,1.0).output, 0.0); // Test that there's now a derivative - assert_eq!(pid.next_control_output(5.0).output, -10.0); + assert_eq!(pid.next_control_output(5.0,1.0).output, -10.0); // Test derivative limit pid.d_limit = 5.0; - assert_eq!(pid.next_control_output(10.0).output, -5.0); + assert_eq!(pid.next_control_output(10.0,1.0).output, -5.0); } /// Integral-only controller operation and limits @@ -321,26 +321,26 @@ mod tests { pid.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); // Test basic integration - assert_eq!(pid.next_control_output(0.0).output, 20.0); - assert_eq!(pid.next_control_output(0.0).output, 40.0); - assert_eq!(pid.next_control_output(5.0).output, 50.0); + assert_eq!(pid.next_control_output(0.0,1.0).output, 20.0); + assert_eq!(pid.next_control_output(0.0,1.0).output, 40.0); + assert_eq!(pid.next_control_output(5.0,1.0).output, 50.0); // Test limit pid.i_limit = 50.0; - assert_eq!(pid.next_control_output(5.0).output, 50.0); + assert_eq!(pid.next_control_output(5.0,1.0).output, 50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid.next_control_output(15.0).output, 40.0); + assert_eq!(pid.next_control_output(15.0,1.0).output, 40.0); // Test that error integral accumulates negative values let mut pid2 = Pid::new(-10.0, 100.0); pid2.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); - assert_eq!(pid2.next_control_output(0.0).output, -20.0); - assert_eq!(pid2.next_control_output(0.0).output, -40.0); + assert_eq!(pid2.next_control_output(0.0,1.0).output, -20.0); + assert_eq!(pid2.next_control_output(0.0,1.0).output, -40.0); pid2.i_limit = 50.0; - assert_eq!(pid2.next_control_output(-5.0).output, -50.0); + assert_eq!(pid2.next_control_output(-5.0,1.0).output, -50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid2.next_control_output(-15.0).output, -40.0); + assert_eq!(pid2.next_control_output(-15.0,1.0).output, -40.0); } /// Checks that a full PID controller's limits work properly through multiple output iterations @@ -349,11 +349,11 @@ mod tests { let mut pid = Pid::new(10.0, 1.0); pid.p(1.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); - let out = pid.next_control_output(0.0); + let out = pid.next_control_output(0.0,1.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.output, 1.0); - let out = pid.next_control_output(20.0); + let out = pid.next_control_output(20.0,1.0); assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0) assert_eq!(out.output, -1.0); } @@ -364,25 +364,25 @@ mod tests { let mut pid = Pid::new(10.0, 100.0); pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); - let out = pid.next_control_output(0.0); + let out = pid.next_control_output(0.0,1.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.i, 1.0); // 0.1 * 10.0 assert_eq!(out.d, 0.0); // -(1.0 * 0.0) assert_eq!(out.output, 11.0); - let out = pid.next_control_output(5.0); + let out = pid.next_control_output(5.0,1.0); assert_eq!(out.p, 5.0); // 1.0 * 5.0 assert_eq!(out.i, 1.5); // 0.1 * (10.0 + 5.0) assert_eq!(out.d, -5.0); // -(1.0 * 5.0) assert_eq!(out.output, 1.5); - let out = pid.next_control_output(11.0); + let out = pid.next_control_output(11.0,1.0); assert_eq!(out.p, -1.0); // 1.0 * -1.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1) assert_eq!(out.d, -6.0); // -(1.0 * 6.0) assert_eq!(out.output, -5.6); - let out = pid.next_control_output(10.0); + let out = pid.next_control_output(10.0,1.0); assert_eq!(out.p, 0.0); // 1.0 * 0.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1.0 + 0.0) assert_eq!(out.d, 1.0); // -(1.0 * -1.0) @@ -401,8 +401,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_f32.next_control_output(0.0).output, - pid_f64.next_control_output(0.0).output as f32 + pid_f32.next_control_output(0.0,1.0).output, + pid_f64.next_control_output(0.0,1.0).output as f32 ); } } @@ -419,8 +419,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_i32.next_control_output(0).output, - pid_i8.next_control_output(0i8).output as i32 + pid_i32.next_control_output(0,1).output, + pid_i8.next_control_output(0i8,1i8).output as i32 ); } } @@ -431,7 +431,7 @@ mod tests { let mut pid = Pid::new(10.0, 100.0); pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); - let out = pid.next_control_output(0.0); + let out = pid.next_control_output(0.0, 1.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.i, 1.0); // 0.1 * 10.0 assert_eq!(out.d, 0.0); // -(1.0 * 0.0) @@ -440,7 +440,7 @@ mod tests { pid.setpoint(0.0); assert_eq!( - pid.next_control_output(0.0), + pid.next_control_output(0.0, 1.0), ControlOutput { p: 0.0, i: 1.0, @@ -456,7 +456,7 @@ mod tests { let mut pid = Pid::new(10.0f32, -10.0); pid.p(1.0, -50.0).i(1.0, -50.0).d(1.0, -50.0); - let out = pid.next_control_output(0.0); + let out = pid.next_control_output(0.0, 1.0); assert_eq!(out.p, 10.0); assert_eq!(out.i, 10.0); assert_eq!(out.d, 0.0); From 796814add0f3baee80333036fd171d6a5e7bd742 Mon Sep 17 00:00:00 2001 From: Mathilda Grace Date: Thu, 27 Mar 2025 10:25:52 -0400 Subject: [PATCH 2/6] Format code --- src/lib.rs | 51 ++++++++++++++++++++++++++------------------------- 1 file changed, 26 insertions(+), 25 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 3573b32..50a2040 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -240,7 +240,8 @@ where let d_unbounded = -match self.prev_measurement.as_ref() { Some(prev_measurement) => measurement - *prev_measurement, None => T::zero(), - } * self.kd / dt; + } * self.kd + / dt; self.prev_measurement = Some(measurement); let d = apply_limit(self.d_limit, d_unbounded); @@ -290,11 +291,11 @@ mod tests { assert_eq!(pid.setpoint, 10.0); // Test simple proportional - assert_eq!(pid.next_control_output(0.0,1.0).output, 20.0); + assert_eq!(pid.next_control_output(0.0, 1.0).output, 20.0); // Test proportional limit pid.p_limit = 10.0; - assert_eq!(pid.next_control_output(0.0,1.0).output, 10.0); + assert_eq!(pid.next_control_output(0.0, 1.0).output, 10.0); } /// Derivative-only controller operation and limits @@ -304,14 +305,14 @@ mod tests { pid.p(0.0, 100.0).i(0.0, 100.0).d(2.0, 100.0); // Test that there's no derivative since it's the first measurement - assert_eq!(pid.next_control_output(0.0,1.0).output, 0.0); + assert_eq!(pid.next_control_output(0.0, 1.0).output, 0.0); // Test that there's now a derivative - assert_eq!(pid.next_control_output(5.0,1.0).output, -10.0); + assert_eq!(pid.next_control_output(5.0, 1.0).output, -10.0); // Test derivative limit pid.d_limit = 5.0; - assert_eq!(pid.next_control_output(10.0,1.0).output, -5.0); + assert_eq!(pid.next_control_output(10.0, 1.0).output, -5.0); } /// Integral-only controller operation and limits @@ -321,26 +322,26 @@ mod tests { pid.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); // Test basic integration - assert_eq!(pid.next_control_output(0.0,1.0).output, 20.0); - assert_eq!(pid.next_control_output(0.0,1.0).output, 40.0); - assert_eq!(pid.next_control_output(5.0,1.0).output, 50.0); + assert_eq!(pid.next_control_output(0.0, 1.0).output, 20.0); + assert_eq!(pid.next_control_output(0.0, 1.0).output, 40.0); + assert_eq!(pid.next_control_output(5.0, 1.0).output, 50.0); // Test limit pid.i_limit = 50.0; - assert_eq!(pid.next_control_output(5.0,1.0).output, 50.0); + assert_eq!(pid.next_control_output(5.0, 1.0).output, 50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid.next_control_output(15.0,1.0).output, 40.0); + assert_eq!(pid.next_control_output(15.0, 1.0).output, 40.0); // Test that error integral accumulates negative values let mut pid2 = Pid::new(-10.0, 100.0); pid2.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); - assert_eq!(pid2.next_control_output(0.0,1.0).output, -20.0); - assert_eq!(pid2.next_control_output(0.0,1.0).output, -40.0); + assert_eq!(pid2.next_control_output(0.0, 1.0).output, -20.0); + assert_eq!(pid2.next_control_output(0.0, 1.0).output, -40.0); pid2.i_limit = 50.0; - assert_eq!(pid2.next_control_output(-5.0,1.0).output, -50.0); + assert_eq!(pid2.next_control_output(-5.0, 1.0).output, -50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid2.next_control_output(-15.0,1.0).output, -40.0); + assert_eq!(pid2.next_control_output(-15.0, 1.0).output, -40.0); } /// Checks that a full PID controller's limits work properly through multiple output iterations @@ -349,11 +350,11 @@ mod tests { let mut pid = Pid::new(10.0, 1.0); pid.p(1.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); - let out = pid.next_control_output(0.0,1.0); + let out = pid.next_control_output(0.0, 1.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.output, 1.0); - let out = pid.next_control_output(20.0,1.0); + let out = pid.next_control_output(20.0, 1.0); assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0) assert_eq!(out.output, -1.0); } @@ -364,25 +365,25 @@ mod tests { let mut pid = Pid::new(10.0, 100.0); pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); - let out = pid.next_control_output(0.0,1.0); + let out = pid.next_control_output(0.0, 1.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.i, 1.0); // 0.1 * 10.0 assert_eq!(out.d, 0.0); // -(1.0 * 0.0) assert_eq!(out.output, 11.0); - let out = pid.next_control_output(5.0,1.0); + let out = pid.next_control_output(5.0, 1.0); assert_eq!(out.p, 5.0); // 1.0 * 5.0 assert_eq!(out.i, 1.5); // 0.1 * (10.0 + 5.0) assert_eq!(out.d, -5.0); // -(1.0 * 5.0) assert_eq!(out.output, 1.5); - let out = pid.next_control_output(11.0,1.0); + let out = pid.next_control_output(11.0, 1.0); assert_eq!(out.p, -1.0); // 1.0 * -1.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1) assert_eq!(out.d, -6.0); // -(1.0 * 6.0) assert_eq!(out.output, -5.6); - let out = pid.next_control_output(10.0,1.0); + let out = pid.next_control_output(10.0, 1.0); assert_eq!(out.p, 0.0); // 1.0 * 0.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1.0 + 0.0) assert_eq!(out.d, 1.0); // -(1.0 * -1.0) @@ -401,8 +402,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_f32.next_control_output(0.0,1.0).output, - pid_f64.next_control_output(0.0,1.0).output as f32 + pid_f32.next_control_output(0.0, 1.0).output, + pid_f64.next_control_output(0.0, 1.0).output as f32 ); } } @@ -419,8 +420,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_i32.next_control_output(0,1).output, - pid_i8.next_control_output(0i8,1i8).output as i32 + pid_i32.next_control_output(0, 1).output, + pid_i8.next_control_output(0i8, 1i8).output as i32 ); } } From c8b43eb57cf506407f8aae4833ab2584e584b2b3 Mon Sep 17 00:00:00 2001 From: Mathilda Grace Date: Thu, 27 Mar 2025 11:16:07 -0400 Subject: [PATCH 3/6] Add missing argument to tests --- src/lib.rs | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 50a2040..e692b5f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -12,20 +12,20 @@ //! pid.p(10.0, 100.0); //! //! // Input a measurement with an error of 5.0 from our setpoint -//! let output = pid.next_control_output(10.0); +//! let output = pid.next_control_output(10.0, 1.0); //! //! // Show that the error is correct by multiplying by our kp //! assert_eq!(output.output, 50.0); // <-- //! assert_eq!(output.p, 50.0); //! //! // It won't change on repeat; the controller is proportional-only -//! let output = pid.next_control_output(10.0); +//! let output = pid.next_control_output(10.0, 1.0); //! assert_eq!(output.output, 50.0); // <-- //! assert_eq!(output.p, 50.0); //! //! // Add a new integral term to the controller and input again //! pid.i(1.0, 100.0); -//! let output = pid.next_control_output(10.0); +//! let output = pid.next_control_output(10.0, 1.0); //! //! // Now that the integral makes the controller stateful, it will change //! assert_eq!(output.output, 55.0); // <-- @@ -34,7 +34,7 @@ //! //! // Add our final derivative term and match our setpoint target //! pid.d(2.0, 100.0); -//! let output = pid.next_control_output(15.0); +//! let output = pid.next_control_output(15.0, 1.0); //! //! // The output will now say to go down due to the derivative //! assert_eq!(output.output, -5.0); // <-- @@ -79,7 +79,7 @@ impl Number for T {} /// p_controller.p(10.0, 100.0); /// /// // Get first output -/// let p_output = p_controller.next_control_output(400.0); +/// let p_output = p_controller.next_control_output(400.0, 1.0); /// ``` /// /// This controller would give you set a proportional controller to `10.0` with a target of `15.0` and an output limit of `100.0` per [output](Self::next_control_output) iteration. The same controller with a full PID system built in looks like: @@ -92,7 +92,7 @@ impl Number for T {} /// full_controller.p(10.0, 100.0).i(4.5, 100.0).d(0.25, 100.0); /// /// // Get first output -/// let full_output = full_controller.next_control_output(400.0); +/// let full_output = full_controller.next_control_output(400.0, 1.0); /// ``` /// /// This [`next_control_output`](Self::next_control_output) method is what's used to input new values into the controller to tell it what the current state of the system is. In the examples above it's only being used once, but realistically this will be a hot method. Please see [ControlOutput] for examples of how to handle these outputs; it's quite straight forward and mirrors the values of this structure in some ways. @@ -141,7 +141,7 @@ pub struct Pid { /// pid.p(10.0, 100.0).i(1.0, 100.0).d(2.0, 100.0); /// /// // Input an example value and get a report for an output iteration -/// let output = pid.next_control_output(26.2456); +/// let output = pid.next_control_output(26.2456, 1.0); /// println!("P: {}\nI: {}\nD: {}\nFinal Output: {}", output.p, output.i, output.d, output.output); /// ``` #[derive(Debug, PartialEq, Eq)] From 6a22a4739b709e6790184921b4853c05a40e1869 Mon Sep 17 00:00:00 2001 From: Mathilda Grace Date: Thu, 27 Mar 2025 12:51:52 -0400 Subject: [PATCH 4/6] Add `dt` feature flag --- Cargo.toml | 3 ++ src/lib.rs | 154 +++++++++++++++++++++++++++++++++++++++++++---------- 2 files changed, 130 insertions(+), 27 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index ab6f62f..351dcdb 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,6 +13,9 @@ keywords = ["pid"] categories = ["no-std", "embedded", "algorithms"] readme = "README.md" +[features] +dt = [] + [dependencies.num-traits] version = "0.2" default-features = false diff --git a/src/lib.rs b/src/lib.rs index e692b5f..38fd662 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -12,20 +12,33 @@ //! pid.p(10.0, 100.0); //! //! // Input a measurement with an error of 5.0 from our setpoint +//! +//! #[cfg(feature = "dt")] //! let output = pid.next_control_output(10.0, 1.0); +//! #[cfg(not(feature = "dt"))] +//! let output = pid.next_control_output(10.0); //! //! // Show that the error is correct by multiplying by our kp //! assert_eq!(output.output, 50.0); // <-- //! assert_eq!(output.p, 50.0); //! //! // It won't change on repeat; the controller is proportional-only +//! +//! #[cfg(feature = "dt")] //! let output = pid.next_control_output(10.0, 1.0); +//! #[cfg(not(feature = "dt"))] +//! let output = pid.next_control_output(10.0); +//! //! assert_eq!(output.output, 50.0); // <-- //! assert_eq!(output.p, 50.0); //! //! // Add a new integral term to the controller and input again //! pid.i(1.0, 100.0); +//! +//! #[cfg(feature = "dt")] //! let output = pid.next_control_output(10.0, 1.0); +//! #[cfg(not(feature = "dt"))] +//! let output = pid.next_control_output(10.0); //! //! // Now that the integral makes the controller stateful, it will change //! assert_eq!(output.output, 55.0); // <-- @@ -34,7 +47,10 @@ //! //! // Add our final derivative term and match our setpoint target //! pid.d(2.0, 100.0); +//! #[cfg(feature = "dt")] //! let output = pid.next_control_output(15.0, 1.0); +//! #[cfg(not(feature = "dt"))] +//! let output = pid.next_control_output(15.0); //! //! // The output will now say to go down due to the derivative //! assert_eq!(output.output, -5.0); // <-- @@ -79,7 +95,10 @@ impl Number for T {} /// p_controller.p(10.0, 100.0); /// /// // Get first output +/// #[cfg(feature = "dt")] /// let p_output = p_controller.next_control_output(400.0, 1.0); +/// #[cfg(not(feature = "dt"))] +/// let p_output = p_controller.next_control_output(400.0); /// ``` /// /// This controller would give you set a proportional controller to `10.0` with a target of `15.0` and an output limit of `100.0` per [output](Self::next_control_output) iteration. The same controller with a full PID system built in looks like: @@ -92,7 +111,10 @@ impl Number for T {} /// full_controller.p(10.0, 100.0).i(4.5, 100.0).d(0.25, 100.0); /// /// // Get first output +/// #[cfg(feature = "dt")] /// let full_output = full_controller.next_control_output(400.0, 1.0); +/// #[cfg(not(feature = "dt"))] +/// let full_output = full_controller.next_control_output(400.0); /// ``` /// /// This [`next_control_output`](Self::next_control_output) method is what's used to input new values into the controller to tell it what the current state of the system is. In the examples above it's only being used once, but realistically this will be a hot method. Please see [ControlOutput] for examples of how to handle these outputs; it's quite straight forward and mirrors the values of this structure in some ways. @@ -141,7 +163,11 @@ pub struct Pid { /// pid.p(10.0, 100.0).i(1.0, 100.0).d(2.0, 100.0); /// /// // Input an example value and get a report for an output iteration +/// #[cfg(feature = "dt")] /// let output = pid.next_control_output(26.2456, 1.0); +/// #[cfg(not(feature = "dt"))] +/// let output = pid.next_control_output(26.2456); +/// /// println!("P: {}\nI: {}\nD: {}\nFinal Output: {}", output.p, output.i, output.d, output.output); /// ``` #[derive(Debug, PartialEq, Eq)] @@ -210,6 +236,7 @@ where self } + #[cfg(feature = "dt")] /// Given a new measurement and dt, calculates the next [control output](ControlOutput). /// /// # Panics @@ -259,6 +286,55 @@ where } } + #[cfg(not(feature = "dt"))] + /// Given a new measurement, calculates the next [control output](ControlOutput). + /// + /// # Panics + /// + /// - If a setpoint has not been set via `update_setpoint()`. + pub fn next_control_output(&mut self, measurement: T) -> ControlOutput { + // Calculate the error between the ideal setpoint and the current + // measurement to compare against + let error = self.setpoint - measurement; + + // Calculate the proportional term and limit to it's individual limit + let p_unbounded = error * self.kp; + let p = apply_limit(self.p_limit, p_unbounded); + + // Mitigate output jumps when ki(t) != ki(t-1). + // While it's standard to use an error_integral that's a running sum of + // just the error (no ki), because we support ki changing dynamically, + // we store the entire term so that we don't need to remember previous + // ki values. + self.integral_term = self.integral_term + error * self.ki; + + // Mitigate integral windup: Don't want to keep building up error + // beyond what i_limit will allow. + self.integral_term = apply_limit(self.i_limit, self.integral_term); + + // Mitigate derivative kick: Use the derivative of the measurement + // rather than the derivative of the error. + let d_unbounded = -match self.prev_measurement.as_ref() { + Some(prev_measurement) => measurement - *prev_measurement, + None => T::zero(), + } * self.kd; + self.prev_measurement = Some(measurement); + let d = apply_limit(self.d_limit, d_unbounded); + + // Calculate the final output by adding together the PID terms, then + // apply the final defined output limit + let output = p + self.integral_term + d; + let output = apply_limit(self.output_limit, output); + + // Return the individual term's contributions and the final output + ControlOutput { + p, + i: self.integral_term, + d, + output, + } + } + /// Resets the integral term back to zero, this may drastically change the /// control output. pub fn reset_integral_term(&mut self) { @@ -283,6 +359,30 @@ mod tests { use super::Pid; use crate::ControlOutput; + /// Macro to call Pid::next_control_output() with the correct signature + #[cfg(feature = "dt")] + macro_rules! pid_next_control_output { + ($self:ident, $measurement:expr, $dt:expr) => { + $self.next_control_output($measurement, $dt) + }; + ($self:ident, $measurement:expr) => { + // If the dt feature is enabled, we call the function with dt = 1.0 + $self.next_control_output($measurement, 1.0) + }; + } + + /// Macro to call Pid::next_control_output() with the correct signature + #[cfg(not(feature = "dt"))] + macro_rules! pid_next_control_output { + ($self:ident, $measurement:expr, $dt:expr) => { + // If the dt feature is not enabled, we ignore the dt argument + $self.next_control_output($measurement) + }; + ($self:ident, $measurement:expr) => { + $self.next_control_output($measurement) + }; + } + /// Proportional-only controller operation and limits #[test] fn proportional() { @@ -291,11 +391,11 @@ mod tests { assert_eq!(pid.setpoint, 10.0); // Test simple proportional - assert_eq!(pid.next_control_output(0.0, 1.0).output, 20.0); + assert_eq!(pid_next_control_output!(pid, 0.0, 1.0).output, 20.0); // Test proportional limit pid.p_limit = 10.0; - assert_eq!(pid.next_control_output(0.0, 1.0).output, 10.0); + assert_eq!(pid_next_control_output!(pid, 0.0, 1.0).output, 10.0); } /// Derivative-only controller operation and limits @@ -305,14 +405,14 @@ mod tests { pid.p(0.0, 100.0).i(0.0, 100.0).d(2.0, 100.0); // Test that there's no derivative since it's the first measurement - assert_eq!(pid.next_control_output(0.0, 1.0).output, 0.0); + assert_eq!(pid_next_control_output!(pid, 0.0, 1.0).output, 0.0); // Test that there's now a derivative - assert_eq!(pid.next_control_output(5.0, 1.0).output, -10.0); + assert_eq!(pid_next_control_output!(pid, 5.0, 1.0).output, -10.0); // Test derivative limit pid.d_limit = 5.0; - assert_eq!(pid.next_control_output(10.0, 1.0).output, -5.0); + assert_eq!(pid_next_control_output!(pid, 10.0, 1.0).output, -5.0); } /// Integral-only controller operation and limits @@ -322,26 +422,26 @@ mod tests { pid.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); // Test basic integration - assert_eq!(pid.next_control_output(0.0, 1.0).output, 20.0); - assert_eq!(pid.next_control_output(0.0, 1.0).output, 40.0); - assert_eq!(pid.next_control_output(5.0, 1.0).output, 50.0); + assert_eq!(pid_next_control_output!(pid, 0.0, 1.0).output, 20.0); + assert_eq!(pid_next_control_output!(pid, 0.0, 1.0).output, 40.0); + assert_eq!(pid_next_control_output!(pid, 5.0, 1.0).output, 50.0); // Test limit pid.i_limit = 50.0; - assert_eq!(pid.next_control_output(5.0, 1.0).output, 50.0); + assert_eq!(pid_next_control_output!(pid, 5.0, 1.0).output, 50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid.next_control_output(15.0, 1.0).output, 40.0); + assert_eq!(pid_next_control_output!(pid, 15.0, 1.0).output, 40.0); // Test that error integral accumulates negative values let mut pid2 = Pid::new(-10.0, 100.0); pid2.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); - assert_eq!(pid2.next_control_output(0.0, 1.0).output, -20.0); - assert_eq!(pid2.next_control_output(0.0, 1.0).output, -40.0); + assert_eq!(pid_next_control_output!(pid2, 0.0, 1.0).output, -20.0); + assert_eq!(pid_next_control_output!(pid2, 0.0, 1.0).output, -40.0); pid2.i_limit = 50.0; - assert_eq!(pid2.next_control_output(-5.0, 1.0).output, -50.0); + assert_eq!(pid_next_control_output!(pid2, -5.0, 1.0).output, -50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid2.next_control_output(-15.0, 1.0).output, -40.0); + assert_eq!(pid_next_control_output!(pid2, -15.0, 1.0).output, -40.0); } /// Checks that a full PID controller's limits work properly through multiple output iterations @@ -350,11 +450,11 @@ mod tests { let mut pid = Pid::new(10.0, 1.0); pid.p(1.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); - let out = pid.next_control_output(0.0, 1.0); + let out = pid_next_control_output!(pid, 0.0, 1.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.output, 1.0); - let out = pid.next_control_output(20.0, 1.0); + let out = pid_next_control_output!(pid, 20.0, 1.0); assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0) assert_eq!(out.output, -1.0); } @@ -365,25 +465,25 @@ mod tests { let mut pid = Pid::new(10.0, 100.0); pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); - let out = pid.next_control_output(0.0, 1.0); + let out = pid_next_control_output!(pid, 0.0, 1.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.i, 1.0); // 0.1 * 10.0 assert_eq!(out.d, 0.0); // -(1.0 * 0.0) assert_eq!(out.output, 11.0); - let out = pid.next_control_output(5.0, 1.0); + let out = pid_next_control_output!(pid, 5.0, 1.0); assert_eq!(out.p, 5.0); // 1.0 * 5.0 assert_eq!(out.i, 1.5); // 0.1 * (10.0 + 5.0) assert_eq!(out.d, -5.0); // -(1.0 * 5.0) assert_eq!(out.output, 1.5); - let out = pid.next_control_output(11.0, 1.0); + let out = pid_next_control_output!(pid, 11.0, 1.0); assert_eq!(out.p, -1.0); // 1.0 * -1.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1) assert_eq!(out.d, -6.0); // -(1.0 * 6.0) assert_eq!(out.output, -5.6); - let out = pid.next_control_output(10.0, 1.0); + let out = pid_next_control_output!(pid, 10.0, 1.0); assert_eq!(out.p, 0.0); // 1.0 * 0.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1.0 + 0.0) assert_eq!(out.d, 1.0); // -(1.0 * -1.0) @@ -402,8 +502,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_f32.next_control_output(0.0, 1.0).output, - pid_f64.next_control_output(0.0, 1.0).output as f32 + pid_next_control_output!(pid_f32, 0.0, 1.0).output, + pid_next_control_output!(pid_f64, 0.0, 1.0).output as f32 ); } } @@ -420,8 +520,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_i32.next_control_output(0, 1).output, - pid_i8.next_control_output(0i8, 1i8).output as i32 + pid_next_control_output!(pid_i32, 0, 1).output, + pid_next_control_output!(pid_i8, 0i8, 1i8).output as i32 ); } } @@ -432,7 +532,7 @@ mod tests { let mut pid = Pid::new(10.0, 100.0); pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); - let out = pid.next_control_output(0.0, 1.0); + let out = pid_next_control_output!(pid, 0.0, 1.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.i, 1.0); // 0.1 * 10.0 assert_eq!(out.d, 0.0); // -(1.0 * 0.0) @@ -441,7 +541,7 @@ mod tests { pid.setpoint(0.0); assert_eq!( - pid.next_control_output(0.0, 1.0), + pid_next_control_output!(pid, 0.0, 1.0), ControlOutput { p: 0.0, i: 1.0, @@ -457,7 +557,7 @@ mod tests { let mut pid = Pid::new(10.0f32, -10.0); pid.p(1.0, -50.0).i(1.0, -50.0).d(1.0, -50.0); - let out = pid.next_control_output(0.0, 1.0); + let out = pid_next_control_output!(pid, 0.0, 1.0); assert_eq!(out.p, 10.0); assert_eq!(out.i, 10.0); assert_eq!(out.d, 0.0); From 5396d5a57b74c5101f08d196e1b5ef9aaa41daaf Mon Sep 17 00:00:00 2001 From: Mathilda Grace Date: Thu, 27 Mar 2025 12:53:05 -0400 Subject: [PATCH 5/6] Update build job to test with all features --- .github/workflows/rust.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/rust.yml b/.github/workflows/rust.yml index 000bb2c..853311f 100644 --- a/.github/workflows/rust.yml +++ b/.github/workflows/rust.yml @@ -19,4 +19,6 @@ jobs: - name: Build run: cargo build --verbose - name: Run tests - run: cargo test --verbose + run: | + cargo install cargo-all-features && + cargo test-all-features --verbose From 16f13cd251322501c538812efaa5540a792e5c4a Mon Sep 17 00:00:00 2001 From: Mathilda Grace Date: Thu, 27 Mar 2025 13:06:09 -0400 Subject: [PATCH 6/6] Remove redundant arguments in macro calls where `dt` is `1.0` --- src/lib.rs | 46 +++++++++++++++++++++++----------------------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 38fd662..b58499d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -405,14 +405,14 @@ mod tests { pid.p(0.0, 100.0).i(0.0, 100.0).d(2.0, 100.0); // Test that there's no derivative since it's the first measurement - assert_eq!(pid_next_control_output!(pid, 0.0, 1.0).output, 0.0); + assert_eq!(pid_next_control_output!(pid, 0.0).output, 0.0); // Test that there's now a derivative - assert_eq!(pid_next_control_output!(pid, 5.0, 1.0).output, -10.0); + assert_eq!(pid_next_control_output!(pid, 5.0).output, -10.0); // Test derivative limit pid.d_limit = 5.0; - assert_eq!(pid_next_control_output!(pid, 10.0, 1.0).output, -5.0); + assert_eq!(pid_next_control_output!(pid, 10.0).output, -5.0); } /// Integral-only controller operation and limits @@ -422,26 +422,26 @@ mod tests { pid.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); // Test basic integration - assert_eq!(pid_next_control_output!(pid, 0.0, 1.0).output, 20.0); - assert_eq!(pid_next_control_output!(pid, 0.0, 1.0).output, 40.0); - assert_eq!(pid_next_control_output!(pid, 5.0, 1.0).output, 50.0); + assert_eq!(pid_next_control_output!(pid, 0.0).output, 20.0); + assert_eq!(pid_next_control_output!(pid, 0.0).output, 40.0); + assert_eq!(pid_next_control_output!(pid, 5.0).output, 50.0); // Test limit pid.i_limit = 50.0; - assert_eq!(pid_next_control_output!(pid, 5.0, 1.0).output, 50.0); + assert_eq!(pid_next_control_output!(pid, 5.0).output, 50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid_next_control_output!(pid, 15.0, 1.0).output, 40.0); + assert_eq!(pid_next_control_output!(pid, 15.0).output, 40.0); // Test that error integral accumulates negative values let mut pid2 = Pid::new(-10.0, 100.0); pid2.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); - assert_eq!(pid_next_control_output!(pid2, 0.0, 1.0).output, -20.0); - assert_eq!(pid_next_control_output!(pid2, 0.0, 1.0).output, -40.0); + assert_eq!(pid_next_control_output!(pid2, 0.0).output, -20.0); + assert_eq!(pid_next_control_output!(pid2, 0.0).output, -40.0); pid2.i_limit = 50.0; - assert_eq!(pid_next_control_output!(pid2, -5.0, 1.0).output, -50.0); + assert_eq!(pid_next_control_output!(pid2, -5.0).output, -50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid_next_control_output!(pid2, -15.0, 1.0).output, -40.0); + assert_eq!(pid_next_control_output!(pid2, -15.0).output, -40.0); } /// Checks that a full PID controller's limits work properly through multiple output iterations @@ -450,11 +450,11 @@ mod tests { let mut pid = Pid::new(10.0, 1.0); pid.p(1.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); - let out = pid_next_control_output!(pid, 0.0, 1.0); + let out = pid_next_control_output!(pid, 0.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.output, 1.0); - let out = pid_next_control_output!(pid, 20.0, 1.0); + let out = pid_next_control_output!(pid, 20.0); assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0) assert_eq!(out.output, -1.0); } @@ -465,25 +465,25 @@ mod tests { let mut pid = Pid::new(10.0, 100.0); pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); - let out = pid_next_control_output!(pid, 0.0, 1.0); + let out = pid_next_control_output!(pid, 0.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.i, 1.0); // 0.1 * 10.0 assert_eq!(out.d, 0.0); // -(1.0 * 0.0) assert_eq!(out.output, 11.0); - let out = pid_next_control_output!(pid, 5.0, 1.0); + let out = pid_next_control_output!(pid, 5.0); assert_eq!(out.p, 5.0); // 1.0 * 5.0 assert_eq!(out.i, 1.5); // 0.1 * (10.0 + 5.0) assert_eq!(out.d, -5.0); // -(1.0 * 5.0) assert_eq!(out.output, 1.5); - let out = pid_next_control_output!(pid, 11.0, 1.0); + let out = pid_next_control_output!(pid, 11.0); assert_eq!(out.p, -1.0); // 1.0 * -1.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1) assert_eq!(out.d, -6.0); // -(1.0 * 6.0) assert_eq!(out.output, -5.6); - let out = pid_next_control_output!(pid, 10.0, 1.0); + let out = pid_next_control_output!(pid, 10.0); assert_eq!(out.p, 0.0); // 1.0 * 0.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1.0 + 0.0) assert_eq!(out.d, 1.0); // -(1.0 * -1.0) @@ -502,8 +502,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_next_control_output!(pid_f32, 0.0, 1.0).output, - pid_next_control_output!(pid_f64, 0.0, 1.0).output as f32 + pid_next_control_output!(pid_f32, 0.0).output, + pid_next_control_output!(pid_f64, 0.0).output as f32 ); } } @@ -532,7 +532,7 @@ mod tests { let mut pid = Pid::new(10.0, 100.0); pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); - let out = pid_next_control_output!(pid, 0.0, 1.0); + let out = pid_next_control_output!(pid, 0.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.i, 1.0); // 0.1 * 10.0 assert_eq!(out.d, 0.0); // -(1.0 * 0.0) @@ -541,7 +541,7 @@ mod tests { pid.setpoint(0.0); assert_eq!( - pid_next_control_output!(pid, 0.0, 1.0), + pid_next_control_output!(pid, 0.0), ControlOutput { p: 0.0, i: 1.0, @@ -557,7 +557,7 @@ mod tests { let mut pid = Pid::new(10.0f32, -10.0); pid.p(1.0, -50.0).i(1.0, -50.0).d(1.0, -50.0); - let out = pid_next_control_output!(pid, 0.0, 1.0); + let out = pid_next_control_output!(pid, 0.0); assert_eq!(out.p, 10.0); assert_eq!(out.i, 10.0); assert_eq!(out.d, 0.0);