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 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 54583ae..b58499d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -12,6 +12,10 @@ //! 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 @@ -19,12 +23,21 @@ //! 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 @@ -34,6 +47,9 @@ //! //! // 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 @@ -79,6 +95,9 @@ 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); /// ``` /// @@ -92,6 +111,9 @@ 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); /// ``` /// @@ -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,57 @@ where self } + #[cfg(feature = "dt")] + /// 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, dt: 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 * dt; + + // 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 + / dt; + 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, + } + } + + #[cfg(not(feature = "dt"))] /// Given a new measurement, calculates the next [control output](ControlOutput). /// /// # Panics @@ -282,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() { @@ -290,11 +391,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!(pid, 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!(pid, 0.0, 1.0).output, 10.0); } /// Derivative-only controller operation and limits @@ -304,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).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(5.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(10.0).output, -5.0); + assert_eq!(pid_next_control_output!(pid, 10.0).output, -5.0); } /// Integral-only controller operation and limits @@ -321,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).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!(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(5.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(15.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!(pid2.next_control_output(0.0).output, -20.0); - assert_eq!(pid2.next_control_output(0.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!(pid2.next_control_output(-5.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!(pid2.next_control_output(-15.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 @@ -349,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); + 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(20.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); } @@ -364,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); + 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(5.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(11.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(10.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) @@ -401,8 +502,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_next_control_output!(pid_f32, 0.0).output, + pid_next_control_output!(pid_f64, 0.0).output as f32 ); } } @@ -419,8 +520,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_next_control_output!(pid_i32, 0, 1).output, + pid_next_control_output!(pid_i8, 0i8, 1i8).output as i32 ); } } @@ -431,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); + 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) @@ -440,7 +541,7 @@ mod tests { pid.setpoint(0.0); assert_eq!( - pid.next_control_output(0.0), + pid_next_control_output!(pid, 0.0), ControlOutput { p: 0.0, i: 1.0, @@ -456,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); + 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);