From e94f44af6b458c1a2be3e69af8529debc40c72bb Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 13 May 2024 14:56:53 -0300 Subject: [PATCH 01/58] Update lib.rs --- src/lib.rs | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/lib.rs b/src/lib.rs index db2f5c5..eaf1077 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -263,6 +263,14 @@ where pub fn reset_integral_term(&mut self) { self.integral_term = T::zero(); } + + /// Set integral term to a custom value. This might be useful to restore the + /// pid controller to a previous state after an interruption or crash. + pub fn set_integral_term(&mut self, integral_term: impl Into) -> &mut Self { + self.integral_term = integral_term.into(); + self.integral_term = apply_limit(self.i_limit, self.integral_term); + self + } } /// Saturating the input `value` according the absolute `limit` (`-abs(limit) <= output <= abs(limit)`). From b6b61ea7c8ad002c7eaf4af5e1153e91d572baa3 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 10:05:34 -0300 Subject: [PATCH 02/58] Change the API to be more flexible --- src/lib.rs | 305 ++++++++++++++++++++++++++++++++--------------------- 1 file changed, 186 insertions(+), 119 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index eaf1077..fc4167c 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -8,8 +8,10 @@ //! use pid::Pid; //! //! // Create a new proportional-only PID controller with a setpoint of 15 -//! let mut pid = Pid::new(15.0, 100.0); -//! pid.p(10.0, 100.0); +//! let mut pid = Pid::new() +//! .setpoint(15.0) +//! .clamp(-100.0, 100.0) +//! .p(10.0); //! //! // Input a measurement with an error of 5.0 from our setpoint //! let output = pid.next_control_output(10.0); @@ -24,7 +26,7 @@ //! assert_eq!(output.p, 50.0); //! //! // Add a new integral term to the controller and input again -//! pid.i(1.0, 100.0); +//! pid.i(1.0); //! let output = pid.next_control_output(10.0); //! //! // Now that the integral makes the controller stateful, it will change @@ -33,7 +35,7 @@ //! assert_eq!(output.i, 5.0); //! //! // Add our final derivative term and match our setpoint target -//! pid.d(2.0, 100.0); +//! pid.d(2.0); //! let output = pid.next_control_output(15.0); //! //! // The output will now say to go down due to the derivative @@ -44,26 +46,20 @@ //! ``` #![no_std] -#[cfg(feature = "serde")] -use serde::{Deserialize, Serialize}; - -/// A trait for any numeric type usable in the PID controller -/// -/// This trait is automatically implemented for all types that satisfy `PartialOrd + num_traits::Signed + Copy`. This includes all of the signed float types and builtin integer except for [isize]: -/// - [i8] -/// - [i16] -/// - [i32] -/// - [i64] -/// - [i128] -/// - [f32] -/// - [f64] -/// -/// As well as any user type that matches the requirements -pub trait Number: PartialOrd + num_traits::Signed + Copy {} +trait PartialEqClamp { + fn clamp(self, min: Self, max: Self) -> Self; +} -// Implement `Number` for all types that -// satisfy `PartialOrd + num_traits::Signed + Copy`. -impl Number for T {} +impl PartialEqClamp for T +where + T: core::cmp::PartialOrd, +{ + fn clamp(self, min: Self, max: Self) -> Self { + if self.lt(min) {min} else + if self.gt(max) {max} else + {self} + } +} /// Adjustable proportional-integral-derivative (PID) controller. /// @@ -75,8 +71,10 @@ impl Number for T {} /// use pid::Pid; /// /// // Create limited controller -/// let mut p_controller = Pid::new(15.0, 100.0); -/// p_controller.p(10.0, 100.0); +/// let mut p_controller = Pid::new() +/// .setpoint(15.0) +/// .clamp(-100.0, 100.0) +/// .p(10.0); /// /// // Get first output /// let p_output = p_controller.next_control_output(400.0); @@ -88,8 +86,12 @@ impl Number for T {} /// use pid::Pid; /// /// // Create full PID controller -/// let mut full_controller = Pid::new(15.0, 100.0); -/// full_controller.p(10.0, 100.0).i(4.5, 100.0).d(0.25, 100.0); +/// let mut full_controller = Pid::new() +/// .setpoint(15.0) +/// .clamp(-100.0, 100.0); +/// .p(10.0) +/// .i(4.5) +/// .d(0.25); /// /// // Get first output /// let full_output = full_controller.next_control_output(400.0); @@ -103,12 +105,10 @@ impl Number for T {} /// /// [Number] is abstract and can be used with anything from a [i32] to an [i128] (as well as user-defined types). Because of this, very small types might overflow during calculation in [`next_control_output`](Self::next_control_output). You probably don't want to use [i8] or user-defined types around that size so keep that in mind when designing your controller. #[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)] -#[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] -pub struct Pid { +#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] +pub struct Pid { /// Ideal setpoint to strive for. pub setpoint: T, - /// Defines the overall output filter limit. - pub output_limit: T, /// Proportional gain. pub kp: T, /// Integral gain. @@ -116,15 +116,25 @@ pub struct Pid { /// Derivative gain. pub kd: T, /// Limiter for the proportional term: `-p_limit <= P <= p_limit`. - pub p_limit: T, - /// Limiter for the integral term: `-i_limit <= I <= i_limit`. - pub i_limit: T, - /// Limiter for the derivative term: `-d_limit <= D <= d_limit`. - pub d_limit: T, + pub p_limit_high: T, + /// Limiter for the derivative term: `p_limit_low <= P <= p_limit`. + pub p_limit_low: T, + /// Limiter for the integral term: `i_limit_low <= I <= i_limit_high`. + pub i_limit_high: T, + /// Limiter for the derivative term: `i_limit_low <= I <= i_limit_high`. + pub i_limit_low: T, + /// Limiter for the derivative term: `d_limit_low <= D <= d_limit_high`. + pub d_limit_high: T, + /// Limiter for the derivative term: `d_limit_low <= D <= d_limit_high`. + pub d_limit_low: T, + /// Limiter for the derivative term: `o_limit_low <= O <= o_limit_high`. + pub o_limit_high: T, + /// Limiter for the derivative term: `o_limit_low <= O <= o_limit_high`. + pub o_limit_low: T, /// Last calculated integral value if [Pid::ki] is used. - integral_term: T, + pub i_term: T, /// Previously found measurement whilst using the [Pid::next_control_output] method. - prev_measurement: Option, + pub prev_measurement: Option, } /// Output of [controller iterations](Pid::next_control_output) with weights @@ -137,15 +147,19 @@ pub struct Pid { /// use pid::{Pid, ControlOutput}; /// /// // Setup controller -/// let mut pid = Pid::new(15.0, 100.0); -/// pid.p(10.0, 100.0).i(1.0, 100.0).d(2.0, 100.0); +/// let mut pid = Pid::new() +/// .setpoint(15.0) +/// .clamp(0.0, 100.0) +/// .p(10.0) +/// .i(1.0) +/// .d(2.0); /// /// // Input an example value and get a report for an output iteration /// 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)] -pub struct ControlOutput { +pub struct ControlOutput { /// Contribution of the P term to the output. pub p: T, /// Contribution of the I term to the output. @@ -160,47 +174,55 @@ pub struct ControlOutput { impl Pid where - T: Number, + T: core::ops::Sub + + core::ops::Add + + core::ops::Mul + + core::ops::Div + + core::cmp::PartialOrd + + Default + + core::ops::Neg + + core::marker::Copy, { /// Creates a new controller with the target setpoint and the output limit /// - /// To set your P, I, and D terms into this controller, please use the following builder methods: - /// - [Self::p()]: Proportional term setting - /// - [Self::i()]: Integral term setting - /// - [Self::d()]: Derivative term setting - pub fn new(setpoint: impl Into, output_limit: impl Into) -> Self { + /// To set your P, I, and D gains into this controller, please use the following builder methods: + /// - [Self::p()]: Proportional gain setting + /// - [Self::i()]: Integral gain setting + /// - [Self::d()]: Derivative gain setting + pub fn new() -> Self { Self { - setpoint: setpoint.into(), - output_limit: output_limit.into(), - kp: T::zero(), - ki: T::zero(), - kd: T::zero(), - p_limit: T::zero(), - i_limit: T::zero(), - d_limit: T::zero(), - integral_term: T::zero(), + setpoint: T::default(), + kp: T::default(), + ki: T::default(), + kd: T::default(), + p_limit_high: T::default(), + p_limit_low: T::default(), + i_limit_high: T::default(), + i_limit_low: T::default(), + d_limit_high: T::default(), + d_limit_low: T::default(), + o_limit_high: T::default(), + o_limit_low: T::default(), + integral_term: T::default(), prev_measurement: None, } } - /// Sets the [Self::p] term for this controller. - pub fn p(&mut self, gain: impl Into, limit: impl Into) -> &mut Self { + /// Sets the [Self::p] gain for this controller. + pub fn p(&mut self, gain: impl Into) -> &mut Self { self.kp = gain.into(); - self.p_limit = limit.into(); self } - /// Sets the [Self::i] term for this controller. - pub fn i(&mut self, gain: impl Into, limit: impl Into) -> &mut Self { + /// Sets the [Self::i] gain for this controller. + pub fn i(&mut self, gain: impl Into) -> &mut Self { self.ki = gain.into(); - self.i_limit = limit.into(); self } - /// Sets the [Self::d] term for this controller. - pub fn d(&mut self, gain: impl Into, limit: impl Into) -> &mut Self { + /// Sets the [Self::d] gain for this controller. + pub fn d(&mut self, gain: impl Into) -> &mut Self { self.kd = gain.into(); - self.d_limit = limit.into(); self } @@ -210,72 +232,82 @@ where self } + /// Sets the [Self::p] limit for this controller. + pub fn clamp(&mut self, low: impl Into, high: impl Into) -> &mut Self { + self.p_limit_low = low.into(); + self.p_limit_high = high.into(); + self.i_limit_low = low.into(); + self.i_limit_high = high.into(); + self.d_limit_low = low.into(); + self.d_limit_high = high.into(); + self.o_limit_low = low.into(); + self.o_limit_high = high.into(); + self + } + + /// Set integral term to a custom value. This might be useful to restore the + /// pid controller to a previous state after an interruption or crash. + pub fn set_integral_term(&mut self, i_term: impl Into) -> &mut Self { + let i_unbound: T = i_term.into(); + self.i_term = i_unbound.clamp(self.i_limit_low, self.i_limit_high); + self + } + + /// Resets the integral term back to zero, this may drastically change the + /// control output. + pub fn reset_integral_term(&mut self) -> &mut Self { + self.set_integral_term(T::default()) + } + /// 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 { + pub fn next_control_output(&mut self, measurement: impl Into) -> ControlOutput { + let measurement: T = measurement.into(); + // 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); + let p = p_unbounded.clamp(self.p_limit_low, self.p_limit_high); // 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; + let i_unbounded = self.i_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); + self.i_term = i_unbounded.clamp(self.i_limit_low, self.i_limit_high); // 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(), + None => T::default(), } * self.kd; self.prev_measurement = Some(measurement); - let d = apply_limit(self.d_limit, d_unbounded); + let d = d_unbounded.clamp(self.d_limit_low, self.d_limit_high); // 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); + let o_unbounded = p + self.i_term + d; + let output = o_unbounded.clamp(self.o_limit_low, self.o_limit_high); // Return the individual term's contributions and the final output ControlOutput { p, - i: self.integral_term, + i: self.i_term, d, - output: output, + output, } } - - /// Resets the integral term back to zero, this may drastically change the - /// control output. - pub fn reset_integral_term(&mut self) { - self.integral_term = T::zero(); - } - - /// Set integral term to a custom value. This might be useful to restore the - /// pid controller to a previous state after an interruption or crash. - pub fn set_integral_term(&mut self, integral_term: impl Into) -> &mut Self { - self.integral_term = integral_term.into(); - self.integral_term = apply_limit(self.i_limit, self.integral_term); - self - } -} - -/// Saturating the input `value` according the absolute `limit` (`-abs(limit) <= output <= abs(limit)`). -fn apply_limit(limit: T, value: T) -> T { - num_traits::clamp(value, -limit.abs(), limit.abs()) } #[cfg(test)] @@ -286,8 +318,11 @@ mod tests { /// Proportional-only controller operation and limits #[test] fn proportional() { - let mut pid = Pid::new(10.0, 100.0); - pid.p(2.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); + let mut pid = Pid::new() + .setpoint(10.0) + .clamp(-100.0, 100.0) + .p(2.0); + assert_eq!(pid.setpoint, 10.0); // Test simple proportional @@ -301,8 +336,10 @@ mod tests { /// Derivative-only controller operation and limits #[test] fn derivative() { - let mut pid = Pid::new(10.0, 100.0); - pid.p(0.0, 100.0).i(0.0, 100.0).d(2.0, 100.0); + let mut pid = Pid::new() + .setpoint(10.0) + .clamp(-100.0, 100.0) + .d(2.0); // Test that there's no derivative since it's the first measurement assert_eq!(pid.next_control_output(0.0).output, 0.0); @@ -318,8 +355,10 @@ mod tests { /// Integral-only controller operation and limits #[test] fn integral() { - let mut pid = Pid::new(10.0, 100.0); - pid.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); + let mut pid = Pid::new() + .setpoint(10.0) + .clamp(-100.0, 100.0) + .i(2.0); // Test basic integration assert_eq!(pid.next_control_output(0.0).output, 20.0); @@ -333,12 +372,16 @@ mod tests { assert_eq!(pid.next_control_output(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); + let mut pid2 = Pid::new() + .setpoint(-10.0) + .clamp(-100.0, 100.0) + .i(2.0); + assert_eq!(pid2.next_control_output(0.0).output, -20.0); assert_eq!(pid2.next_control_output(0.0).output, -40.0); - pid2.i_limit = 50.0; + pid2.i_limit_high = 50.0; + pid2.i_limit_low = -50.0; assert_eq!(pid2.next_control_output(-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); @@ -347,8 +390,13 @@ mod tests { /// Checks that a full PID controller's limits work properly through multiple output iterations #[test] fn output_limit() { - 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 mut pid = Pid::new() + .setpoint(10.0) + .clamp(-100.0, 100.0) + .p(1.0); + + pid.o_limit_high = 1.0; + pid.o_limit_low = -1.0; let out = pid.next_control_output(0.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 @@ -362,8 +410,12 @@ mod tests { /// Combined PID operation #[test] fn pid() { - 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 mut pid = Pid::new() + .setpoint(10.0) + .clamp(-100.0, 100.0) + .p(1.0) + .i(0.1) + .d(1.0); let out = pid.next_control_output(0.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 @@ -394,11 +446,13 @@ mod tests { /// PID operation with zero'd values, checking to see if different floats equal each other #[test] fn floats_zeros() { - let mut pid_f32 = Pid::new(10.0f32, 100.0); - pid_f32.p(0.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); + let mut pid_f32 = Pid::new() + .setpoint(10.0f32) + .clamp(-100.0, 100.0); - let mut pid_f64 = Pid::new(10.0, 100.0f64); - pid_f64.p(0.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); + let mut pid_f64 = Pid::new() + .setpoint(10.0) + .clamp(-100.0f64, 100.0f64); for _ in 0..5 { assert_eq!( @@ -412,11 +466,13 @@ mod tests { /// PID operation with zero'd values, checking to see if different floats equal each other #[test] fn signed_integers_zeros() { - let mut pid_i8 = Pid::new(10i8, 100); - pid_i8.p(0, 100).i(0, 100).d(0, 100); + let mut pid_i8 = Pid::new() + .setpoint(10i8) + .clamp(-100, 100); - let mut pid_i32 = Pid::new(10i32, 100); - pid_i32.p(0, 100).i(0, 100).d(0, 100); + let mut pid_i32 = Pid::new() + .setpoint(10i32) + .clamp(-100, 100); for _ in 0..5 { assert_eq!( @@ -429,8 +485,12 @@ mod tests { /// See if the controller can properly target to the setpoint after 2 output iterations #[test] fn setpoint() { - 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 mut pid = Pid::new() + .setpoint(10.0) + .clamp(-100.0, 100.0) + .p(1.0) + .i(0.1) + .d(1.0); let out = pid.next_control_output(0.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 @@ -454,8 +514,15 @@ mod tests { /// Make sure negative limits don't break the controller #[test] fn negative_limits() { - 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 mut pid = Pid::new() + .setpoint(10.0f32) + .clamp(50.0, -50.0) + .p(1.0) + .i(1.0) + .d(1.0); + + pid.o_limit_high = -10.0; + pid.o_limit_low = 10.0; let out = pid.next_control_output(0.0); assert_eq!(out.p, 10.0); From d79c459dfa37fc384ca64d17a4581ae2cb6d133e Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 10:09:43 -0300 Subject: [PATCH 03/58] remove num-traits dependency --- Cargo.toml | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index ab6f62f..53ce52e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,15 +13,8 @@ keywords = ["pid"] categories = ["no-std", "embedded", "algorithms"] readme = "README.md" -[dependencies.num-traits] -version = "0.2" -default-features = false - -[dependencies.serde] -version = "1.0" -optional = true -default-features = false -features = ["derive"] +[dependencies] +serde = { version = "1.0", default-features = false, features = ["derive"], optional = true } [badges] travis-ci = { repository = "braincore/pid-rs" } From 9d615120fa2f360fc021e844f0c799fa0b131667 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 10:28:10 -0300 Subject: [PATCH 04/58] Fix minor errors --- src/lib.rs | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index fc4167c..7e97d0a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -55,8 +55,8 @@ where T: core::cmp::PartialOrd, { fn clamp(self, min: Self, max: Self) -> Self { - if self.lt(min) {min} else - if self.gt(max) {max} else + if self < min {min} else + if self > max {max} else {self} } } @@ -203,7 +203,7 @@ where d_limit_low: T::default(), o_limit_high: T::default(), o_limit_low: T::default(), - integral_term: T::default(), + i_term: T::default(), prev_measurement: None, } } @@ -234,14 +234,16 @@ where /// Sets the [Self::p] limit for this controller. pub fn clamp(&mut self, low: impl Into, high: impl Into) -> &mut Self { - self.p_limit_low = low.into(); - self.p_limit_high = high.into(); - self.i_limit_low = low.into(); - self.i_limit_high = high.into(); - self.d_limit_low = low.into(); - self.d_limit_high = high.into(); - self.o_limit_low = low.into(); - self.o_limit_high = high.into(); + let low = low.into(); + let high = high.into(); + self.p_limit_low = low; + self.p_limit_high = high; + self.i_limit_low = low; + self.i_limit_high = high; + self.d_limit_low = low; + self.d_limit_high = high; + self.o_limit_low = low; + self.o_limit_high = high; self } From b839747faaee5c62b9f6e325bdf78ef98bc800f6 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 11:35:21 -0300 Subject: [PATCH 05/58] Implement 'Default' trait for Pid --- src/lib.rs | 45 +++++++++++++++++++++++++++------------------ 1 file changed, 27 insertions(+), 18 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 7e97d0a..45d4d61 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -172,24 +172,11 @@ pub struct ControlOutput { pub output: T, } -impl Pid +impl Default for Pid where - T: core::ops::Sub - + core::ops::Add - + core::ops::Mul - + core::ops::Div - + core::cmp::PartialOrd - + Default - + core::ops::Neg - + core::marker::Copy, + T: Default, { - /// Creates a new controller with the target setpoint and the output limit - /// - /// To set your P, I, and D gains into this controller, please use the following builder methods: - /// - [Self::p()]: Proportional gain setting - /// - [Self::i()]: Integral gain setting - /// - [Self::d()]: Derivative gain setting - pub fn new() -> Self { + fn default() -> Self { Self { setpoint: T::default(), kp: T::default(), @@ -207,6 +194,28 @@ where prev_measurement: None, } } +} + +impl Pid +where + T: core::ops::Sub + + core::ops::Add + + core::ops::Mul + + core::ops::Div + + core::cmp::PartialOrd + + Default + + core::ops::Neg + + core::marker::Copy, +{ + /// Creates a new controller with the target setpoint and the output limit + /// + /// To set your P, I, and D gains into this controller, please use the following builder methods: + /// - [Self::p()]: Proportional gain setting + /// - [Self::i()]: Integral gain setting + /// - [Self::d()]: Derivative gain setting + pub fn new() -> Self { + Self::default() + } /// Sets the [Self::p] gain for this controller. pub fn p(&mut self, gain: impl Into) -> &mut Self { @@ -234,8 +243,8 @@ where /// Sets the [Self::p] limit for this controller. pub fn clamp(&mut self, low: impl Into, high: impl Into) -> &mut Self { - let low = low.into(); - let high = high.into(); + let low: T = low.into(); + let high: T = high.into(); self.p_limit_low = low; self.p_limit_high = high; self.i_limit_low = low; From 91cd04ab2e5c38fc8053e532192fe3a543c979cd Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 12:01:52 -0300 Subject: [PATCH 06/58] Add 'Send + Sync' traits --- src/lib.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 45d4d61..d19c417 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -104,7 +104,7 @@ where /// # Type Warning /// /// [Number] is abstract and can be used with anything from a [i32] to an [i128] (as well as user-defined types). Because of this, very small types might overflow during calculation in [`next_control_output`](Self::next_control_output). You probably don't want to use [i8] or user-defined types around that size so keep that in mind when designing your controller. -#[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)] +#[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd, Send, Sync)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct Pid { /// Ideal setpoint to strive for. @@ -158,7 +158,7 @@ pub struct Pid { /// 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)] +#[derive(Debug, PartialEq, Eq, Send, Sync)] pub struct ControlOutput { /// Contribution of the P term to the output. pub p: T, From 9a8e9ad2fd1b811a9c437bac6aab8cb40dd5752c Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 12:08:23 -0300 Subject: [PATCH 07/58] Undo last commit because of 'no-std' --- src/lib.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index d19c417..45d4d61 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -104,7 +104,7 @@ where /// # Type Warning /// /// [Number] is abstract and can be used with anything from a [i32] to an [i128] (as well as user-defined types). Because of this, very small types might overflow during calculation in [`next_control_output`](Self::next_control_output). You probably don't want to use [i8] or user-defined types around that size so keep that in mind when designing your controller. -#[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd, Send, Sync)] +#[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct Pid { /// Ideal setpoint to strive for. @@ -158,7 +158,7 @@ pub struct Pid { /// 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, Send, Sync)] +#[derive(Debug, PartialEq, Eq)] pub struct ControlOutput { /// Contribution of the P term to the output. pub p: T, From 170f61c8c6509603a54c916869f572c77eb669e3 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 13:24:59 -0300 Subject: [PATCH 08/58] bring num traits dependency back --- Cargo.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/Cargo.toml b/Cargo.toml index 53ce52e..fc5a357 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -14,6 +14,7 @@ categories = ["no-std", "embedded", "algorithms"] readme = "README.md" [dependencies] +num-traits = { version = "0.2", default-features = false } serde = { version = "1.0", default-features = false, features = ["derive"], optional = true } [badges] From aa8066a7a0166b9fdd571d20c29de4408a5a9794 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 13:39:37 -0300 Subject: [PATCH 09/58] Implement constant constructor method --- src/lib.rs | 81 ++++++++++++++++++++++++++++++++++++------------------ 1 file changed, 55 insertions(+), 26 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 45d4d61..5eb3618 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -106,7 +106,7 @@ where /// [Number] is abstract and can be used with anything from a [i32] to an [i128] (as well as user-defined types). Because of this, very small types might overflow during calculation in [`next_control_output`](Self::next_control_output). You probably don't want to use [i8] or user-defined types around that size so keep that in mind when designing your controller. #[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] -pub struct Pid { +pub struct Pid { /// Ideal setpoint to strive for. pub setpoint: T, /// Proportional gain. @@ -159,7 +159,8 @@ pub struct Pid { /// println!("P: {}\nI: {}\nD: {}\nFinal Output: {}", output.p, output.i, output.d, output.output); /// ``` #[derive(Debug, PartialEq, Eq)] -pub struct ControlOutput { +#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] +pub struct ControlOutput { /// Contribution of the P term to the output. pub p: T, /// Contribution of the I term to the output. @@ -177,20 +178,35 @@ where T: Default, { fn default() -> Self { + Self::new() + } +} + +impl Pid +where + T: num_traits::PrimInt, +{ + /// Creates a new controller + /// + /// To set your P, I, and D gains into this controller, please use the following builder methods: + /// - [Self::p()]: Proportional gain setting + /// - [Self::i()]: Integral gain setting + /// - [Self::d()]: Derivative gain setting + pub const fn new() -> Self { Self { - setpoint: T::default(), - kp: T::default(), - ki: T::default(), - kd: T::default(), - p_limit_high: T::default(), - p_limit_low: T::default(), - i_limit_high: T::default(), - i_limit_low: T::default(), - d_limit_high: T::default(), - d_limit_low: T::default(), - o_limit_high: T::default(), - o_limit_low: T::default(), - i_term: T::default(), + setpoint: 0, + kp: 0, + ki: 0, + kd: 0, + p_limit_high: 0, + p_limit_low: 0, + i_limit_high: 0, + i_limit_low: 0, + d_limit_high: 0, + d_limit_low: 0, + o_limit_high: 0, + o_limit_low: 0, + i_term: 0, prev_measurement: None, } } @@ -198,25 +214,38 @@ where impl Pid where - T: core::ops::Sub - + core::ops::Add - + core::ops::Mul - + core::ops::Div - + core::cmp::PartialOrd - + Default - + core::ops::Neg - + core::marker::Copy, + T: num_traits::Num, { - /// Creates a new controller with the target setpoint and the output limit + /// Creates a new controller /// /// To set your P, I, and D gains into this controller, please use the following builder methods: /// - [Self::p()]: Proportional gain setting /// - [Self::i()]: Integral gain setting /// - [Self::d()]: Derivative gain setting - pub fn new() -> Self { - Self::default() + pub const fn new() -> Self { + Self { + setpoint: 0.0, + kp: 0.0, + ki: 0.0, + kd: 0.0, + p_limit_high: 0.0, + p_limit_low: 0.0, + i_limit_high: 0.0, + i_limit_low: 0.0, + d_limit_high: 0.0, + d_limit_low: 0.0, + o_limit_high: 0.0, + o_limit_low: 0.0, + i_term: 0.0, + prev_measurement: None, + } } +} +impl Pid +where + T: num_traits::Num, +{ /// Sets the [Self::p] gain for this controller. pub fn p(&mut self, gain: impl Into) -> &mut Self { self.kp = gain.into(); From 80135e9060168e5ec725caafa069201488806136 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 13:46:20 -0300 Subject: [PATCH 10/58] fix errors (1) --- src/lib.rs | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 5eb3618..84e0274 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -106,7 +106,7 @@ where /// [Number] is abstract and can be used with anything from a [i32] to an [i128] (as well as user-defined types). Because of this, very small types might overflow during calculation in [`next_control_output`](Self::next_control_output). You probably don't want to use [i8] or user-defined types around that size so keep that in mind when designing your controller. #[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] -pub struct Pid { +pub struct Pid { /// Ideal setpoint to strive for. pub setpoint: T, /// Proportional gain. @@ -160,7 +160,7 @@ pub struct Pid { /// ``` #[derive(Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] -pub struct ControlOutput { +pub struct ControlOutput { /// Contribution of the P term to the output. pub p: T, /// Contribution of the I term to the output. @@ -175,7 +175,7 @@ pub struct ControlOutput { impl Default for Pid where - T: Default, + T: core::default::Default, { fn default() -> Self { Self::new() @@ -184,7 +184,7 @@ where impl Pid where - T: num_traits::PrimInt, + T: num_traits::int::PrimInt, { /// Creates a new controller /// @@ -214,7 +214,7 @@ where impl Pid where - T: num_traits::Num, + T: num_traits::float::FloatCore, { /// Creates a new controller /// @@ -244,7 +244,8 @@ where impl Pid where - T: num_traits::Num, + T: num_traits::Num + + core::cmp::PartialOrd, { /// Sets the [Self::p] gain for this controller. pub fn p(&mut self, gain: impl Into) -> &mut Self { From 295cd89b425bd86823847f46aec60c6ebcccb0da Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 14:03:27 -0300 Subject: [PATCH 11/58] fix errors (2) --- src/lib.rs | 101 +++++++++++++++++++++++++++++------------------------ 1 file changed, 55 insertions(+), 46 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 84e0274..140625f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -46,15 +46,32 @@ //! ``` #![no_std] -trait PartialEqClamp { - fn clamp(self, min: Self, max: Self) -> Self; +/// A trait for any numeric type usable in the PID controller +/// +/// This trait is automatically implemented for all types that satisfy `PartialOrd + num_traits::Signed + Copy`. This includes all of the signed float types and builtin integer except for [isize]: +/// - [i8] +/// - [i16] +/// - [i32] +/// - [i64] +/// - [i128] +/// - [f32] +/// - [f64] +/// +/// As well as any user type that matches the requirements +pub trait Number: num_traits::sign::Signed + PartialOrd + Copy + Send { + pub fn clamp(self, min: Self, max: Self) -> Self; } -impl PartialEqClamp for T +// Implement `Number` for all types that +// satisfy `PartialOrd + num_traits::Signed + Copy`. +impl Number for T where - T: core::cmp::PartialOrd, + T: num_traits::sign::Signed + + PartialOrd + + Copy + + Send, { - fn clamp(self, min: Self, max: Self) -> Self { + pub fn clamp(self, min: Self, max: Self) -> Self { if self < min {min} else if self > max {max} else {self} @@ -106,7 +123,7 @@ where /// [Number] is abstract and can be used with anything from a [i32] to an [i128] (as well as user-defined types). Because of this, very small types might overflow during calculation in [`next_control_output`](Self::next_control_output). You probably don't want to use [i8] or user-defined types around that size so keep that in mind when designing your controller. #[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] -pub struct Pid { +pub struct Pid { /// Ideal setpoint to strive for. pub setpoint: T, /// Proportional gain. @@ -160,7 +177,7 @@ pub struct Pid { /// ``` #[derive(Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] -pub struct ControlOutput { +pub struct ControlOutput { /// Contribution of the P term to the output. pub p: T, /// Contribution of the I term to the output. @@ -173,18 +190,10 @@ pub struct ControlOutput { pub output: T, } -impl Default for Pid -where - T: core::default::Default, -{ - fn default() -> Self { - Self::new() - } -} - impl Pid where - T: num_traits::int::PrimInt, + T: Number + + num_traits::int::PrimInt, { /// Creates a new controller /// @@ -194,19 +203,19 @@ where /// - [Self::d()]: Derivative gain setting pub const fn new() -> Self { Self { - setpoint: 0, - kp: 0, - ki: 0, - kd: 0, - p_limit_high: 0, - p_limit_low: 0, - i_limit_high: 0, - i_limit_low: 0, - d_limit_high: 0, - d_limit_low: 0, - o_limit_high: 0, - o_limit_low: 0, - i_term: 0, + setpoint: 0 as T, + kp: 0 as T, + ki: 0 as T, + kd: 0 as T, + p_limit_high: 0 as T, + p_limit_low: 0 as T, + i_limit_high: 0 as T, + i_limit_low: 0 as T, + d_limit_high: 0 as T, + d_limit_low: 0 as T, + o_limit_high: 0 as T, + o_limit_low: 0 as T, + i_term: 0 as T, prev_measurement: None, } } @@ -214,7 +223,8 @@ where impl Pid where - T: num_traits::float::FloatCore, + T: Number + + num_traits::float::FloatCore, { /// Creates a new controller /// @@ -224,19 +234,19 @@ where /// - [Self::d()]: Derivative gain setting pub const fn new() -> Self { Self { - setpoint: 0.0, - kp: 0.0, - ki: 0.0, - kd: 0.0, - p_limit_high: 0.0, - p_limit_low: 0.0, - i_limit_high: 0.0, - i_limit_low: 0.0, - d_limit_high: 0.0, - d_limit_low: 0.0, - o_limit_high: 0.0, - o_limit_low: 0.0, - i_term: 0.0, + setpoint: 0.0 as T, + kp: 0.0 as T, + ki: 0.0 as T, + kd: 0.0 as T, + p_limit_high: 0.0 as T, + p_limit_low: 0.0 as T, + i_limit_high: 0.0 as T, + i_limit_low: 0.0 as T, + d_limit_high: 0.0 as T, + d_limit_low: 0.0 as T, + o_limit_high: 0.0 as T, + o_limit_low: 0.0 as T, + i_term: 0.0 as T, prev_measurement: None, } } @@ -244,8 +254,7 @@ where impl Pid where - T: num_traits::Num - + core::cmp::PartialOrd, + T: Number, { /// Sets the [Self::p] gain for this controller. pub fn p(&mut self, gain: impl Into) -> &mut Self { From 7e2c78fde4b5920a6fbf0449f9e3eabe06a38fe5 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 14:07:37 -0300 Subject: [PATCH 12/58] fix errors (3) --- src/lib.rs | 63 +++++++++++++++++++++++++++--------------------------- 1 file changed, 32 insertions(+), 31 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 140625f..321d74a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -58,8 +58,8 @@ /// - [f64] /// /// As well as any user type that matches the requirements -pub trait Number: num_traits::sign::Signed + PartialOrd + Copy + Send { - pub fn clamp(self, min: Self, max: Self) -> Self; +pub trait Number: num_traits::sign::Signed + PartialOrd + Default + Copy + Send { + fn clamp(self, min: Self, max: Self) -> Self; } // Implement `Number` for all types that @@ -67,11 +67,12 @@ pub trait Number: num_traits::sign::Signed + PartialOrd + Copy + Send { impl Number for T where T: num_traits::sign::Signed - + PartialOrd - + Copy + + PartialOrd + + Default + + Copy + Send, { - pub fn clamp(self, min: Self, max: Self) -> Self { + fn clamp(self, min: Self, max: Self) -> Self { if self < min {min} else if self > max {max} else {self} @@ -203,19 +204,19 @@ where /// - [Self::d()]: Derivative gain setting pub const fn new() -> Self { Self { - setpoint: 0 as T, - kp: 0 as T, - ki: 0 as T, - kd: 0 as T, - p_limit_high: 0 as T, - p_limit_low: 0 as T, - i_limit_high: 0 as T, - i_limit_low: 0 as T, - d_limit_high: 0 as T, - d_limit_low: 0 as T, - o_limit_high: 0 as T, - o_limit_low: 0 as T, - i_term: 0 as T, + setpoint: 0, + kp: 0, + ki: 0, + kd: 0, + p_limit_high: 0, + p_limit_low: 0, + i_limit_high: 0, + i_limit_low: 0, + d_limit_high: 0, + d_limit_low: 0, + o_limit_high: 0, + o_limit_low: 0, + i_term: 0, prev_measurement: None, } } @@ -234,19 +235,19 @@ where /// - [Self::d()]: Derivative gain setting pub const fn new() -> Self { Self { - setpoint: 0.0 as T, - kp: 0.0 as T, - ki: 0.0 as T, - kd: 0.0 as T, - p_limit_high: 0.0 as T, - p_limit_low: 0.0 as T, - i_limit_high: 0.0 as T, - i_limit_low: 0.0 as T, - d_limit_high: 0.0 as T, - d_limit_low: 0.0 as T, - o_limit_high: 0.0 as T, - o_limit_low: 0.0 as T, - i_term: 0.0 as T, + setpoint: 0.0, + kp: 0.0, + ki: 0.0, + kd: 0.0, + p_limit_high: 0.0, + p_limit_low: 0.0, + i_limit_high: 0.0, + i_limit_low: 0.0, + d_limit_high: 0.0, + d_limit_low: 0.0, + o_limit_high: 0.0, + o_limit_low: 0.0, + i_term: 0.0, prev_measurement: None, } } From fbbf6580fd9bae15da9e67708314e265be0f7b7f Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 14:10:08 -0300 Subject: [PATCH 13/58] fix errors (4) --- src/lib.rs | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 321d74a..87e520a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -191,10 +191,7 @@ pub struct ControlOutput { pub output: T, } -impl Pid -where - T: Number + - num_traits::int::PrimInt, +impl Pid { /// Creates a new controller /// @@ -202,7 +199,7 @@ where /// - [Self::p()]: Proportional gain setting /// - [Self::i()]: Integral gain setting /// - [Self::d()]: Derivative gain setting - pub const fn new() -> Self { + pub const fn new_int() -> Self { Self { setpoint: 0, kp: 0, @@ -222,10 +219,7 @@ where } } -impl Pid -where - T: Number - + num_traits::float::FloatCore, +impl Pid { /// Creates a new controller /// @@ -233,7 +227,7 @@ where /// - [Self::p()]: Proportional gain setting /// - [Self::i()]: Integral gain setting /// - [Self::d()]: Derivative gain setting - pub const fn new() -> Self { + pub const fn new_float() -> Self { Self { setpoint: 0.0, kp: 0.0, @@ -253,9 +247,7 @@ where } } -impl Pid -where - T: Number, +impl Pid { /// Sets the [Self::p] gain for this controller. pub fn p(&mut self, gain: impl Into) -> &mut Self { From e7d3dc637b902baa65d70fd49ade23dad4563b32 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 14:15:03 -0300 Subject: [PATCH 14/58] Implement const constructor --- src/lib.rs | 148 +++++++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 144 insertions(+), 4 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 87e520a..cb2b9c8 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -191,7 +191,7 @@ pub struct ControlOutput { pub output: T, } -impl Pid +impl Pid { /// Creates a new controller /// @@ -199,7 +199,7 @@ impl Pid /// - [Self::p()]: Proportional gain setting /// - [Self::i()]: Integral gain setting /// - [Self::d()]: Derivative gain setting - pub const fn new_int() -> Self { + pub const fn new() -> Self { Self { setpoint: 0, kp: 0, @@ -219,7 +219,7 @@ impl Pid } } -impl Pid +impl Pid { /// Creates a new controller /// @@ -227,7 +227,147 @@ impl Pid /// - [Self::p()]: Proportional gain setting /// - [Self::i()]: Integral gain setting /// - [Self::d()]: Derivative gain setting - pub const fn new_float() -> Self { + pub const fn new() -> Self { + Self { + setpoint: 0, + kp: 0, + ki: 0, + kd: 0, + p_limit_high: 0, + p_limit_low: 0, + i_limit_high: 0, + i_limit_low: 0, + d_limit_high: 0, + d_limit_low: 0, + o_limit_high: 0, + o_limit_low: 0, + i_term: 0, + prev_measurement: None, + } + } +} + +impl Pid +{ + /// Creates a new controller + /// + /// To set your P, I, and D gains into this controller, please use the following builder methods: + /// - [Self::p()]: Proportional gain setting + /// - [Self::i()]: Integral gain setting + /// - [Self::d()]: Derivative gain setting + pub const fn new() -> Self { + Self { + setpoint: 0, + kp: 0, + ki: 0, + kd: 0, + p_limit_high: 0, + p_limit_low: 0, + i_limit_high: 0, + i_limit_low: 0, + d_limit_high: 0, + d_limit_low: 0, + o_limit_high: 0, + o_limit_low: 0, + i_term: 0, + prev_measurement: None, + } + } +} + +impl Pid +{ + /// Creates a new controller + /// + /// To set your P, I, and D gains into this controller, please use the following builder methods: + /// - [Self::p()]: Proportional gain setting + /// - [Self::i()]: Integral gain setting + /// - [Self::d()]: Derivative gain setting + pub const fn new() -> Self { + Self { + setpoint: 0, + kp: 0, + ki: 0, + kd: 0, + p_limit_high: 0, + p_limit_low: 0, + i_limit_high: 0, + i_limit_low: 0, + d_limit_high: 0, + d_limit_low: 0, + o_limit_high: 0, + o_limit_low: 0, + i_term: 0, + prev_measurement: None, + } + } +} + +impl Pid +{ + /// Creates a new controller + /// + /// To set your P, I, and D gains into this controller, please use the following builder methods: + /// - [Self::p()]: Proportional gain setting + /// - [Self::i()]: Integral gain setting + /// - [Self::d()]: Derivative gain setting + pub const fn new() -> Self { + Self { + setpoint: 0, + kp: 0, + ki: 0, + kd: 0, + p_limit_high: 0, + p_limit_low: 0, + i_limit_high: 0, + i_limit_low: 0, + d_limit_high: 0, + d_limit_low: 0, + o_limit_high: 0, + o_limit_low: 0, + i_term: 0, + prev_measurement: None, + } + } +} + +impl Pid +{ + /// Creates a new controller + /// + /// To set your P, I, and D gains into this controller, please use the following builder methods: + /// - [Self::p()]: Proportional gain setting + /// - [Self::i()]: Integral gain setting + /// - [Self::d()]: Derivative gain setting + pub const fn new() -> Self { + Self { + setpoint: 0.0, + kp: 0.0, + ki: 0.0, + kd: 0.0, + p_limit_high: 0.0, + p_limit_low: 0.0, + i_limit_high: 0.0, + i_limit_low: 0.0, + d_limit_high: 0.0, + d_limit_low: 0.0, + o_limit_high: 0.0, + o_limit_low: 0.0, + i_term: 0.0, + prev_measurement: None, + } + } +} + +impl Pid +{ + /// Creates a new controller + /// + /// To set your P, I, and D gains into this controller, please use the following builder methods: + /// - [Self::p()]: Proportional gain setting + /// - [Self::i()]: Integral gain setting + /// - [Self::d()]: Derivative gain setting + pub const fn new() -> Self { Self { setpoint: 0.0, kp: 0.0, From 80d9bfde9a8951224d985df2ecce37481a4f7465 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 14:18:36 -0300 Subject: [PATCH 15/58] Implement core::Send --- src/lib.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index cb2b9c8..900cce7 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -122,7 +122,7 @@ where /// # Type Warning /// /// [Number] is abstract and can be used with anything from a [i32] to an [i128] (as well as user-defined types). Because of this, very small types might overflow during calculation in [`next_control_output`](Self::next_control_output). You probably don't want to use [i8] or user-defined types around that size so keep that in mind when designing your controller. -#[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)] +#[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd, Send)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct Pid { /// Ideal setpoint to strive for. @@ -176,7 +176,7 @@ pub struct Pid { /// 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)] +#[derive(Debug, PartialEq, Eq, Send)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct ControlOutput { /// Contribution of the P term to the output. From 211473930478d85fbd70fe88100b31397199a2ad Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 14:20:00 -0300 Subject: [PATCH 16/58] Implement Send explicit --- src/lib.rs | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 900cce7..a399451 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -122,7 +122,7 @@ where /// # Type Warning /// /// [Number] is abstract and can be used with anything from a [i32] to an [i128] (as well as user-defined types). Because of this, very small types might overflow during calculation in [`next_control_output`](Self::next_control_output). You probably don't want to use [i8] or user-defined types around that size so keep that in mind when designing your controller. -#[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd, Send)] +#[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct Pid { /// Ideal setpoint to strive for. @@ -176,7 +176,7 @@ pub struct Pid { /// 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, Send)] +#[derive(Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct ControlOutput { /// Contribution of the P term to the output. @@ -191,6 +191,9 @@ pub struct ControlOutput { pub output: T, } +unsafe impl Send for Pid where T: Send {} +unsafe impl Send for ControlOutput where T: Send {} + impl Pid { /// Creates a new controller From c886603e43013936a847edd0c3b5926cd0163a89 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 14:21:25 -0300 Subject: [PATCH 17/58] Fix implement send --- src/lib.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index a399451..96c4933 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -191,8 +191,8 @@ pub struct ControlOutput { pub output: T, } -unsafe impl Send for Pid where T: Send {} -unsafe impl Send for ControlOutput where T: Send {} +unsafe impl Send for Pid where T: Number {} +unsafe impl Send for ControlOutput where T: Number {} impl Pid { From b7caf0143ddcf00601c91648fb57577b274f0569 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 14 May 2024 14:32:59 -0300 Subject: [PATCH 18/58] Remove unnecessary unsafe --- src/lib.rs | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 96c4933..cb2b9c8 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -191,9 +191,6 @@ pub struct ControlOutput { pub output: T, } -unsafe impl Send for Pid where T: Number {} -unsafe impl Send for ControlOutput where T: Number {} - impl Pid { /// Creates a new controller From e76e126434a8305043d89c46d9cf175d7def0e73 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 15 May 2024 08:07:29 -0300 Subject: [PATCH 19/58] Try impl constructor --- src/lib.rs | 145 +++++++++++------------------------------------------ 1 file changed, 29 insertions(+), 116 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index cb2b9c8..0827671 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -191,6 +191,30 @@ pub struct ControlOutput { pub output: T, } +impl From> for Pid +where + T: From +{ + fn from(value: Pid) -> Self { + Self { + setpoint: T::from(value.setpoint), + kp: T::from(value.kp), + ki: T::from(value.ki), + kd: T::from(value.kd), + p_limit_high: T::from(value.p_limit_high), + p_limit_low: T::from(value.p_limit_low), + i_limit_high: T::from(value.i_limit_high), + i_limit_low: T::from(value.i_limit_low), + d_limit_high: T::from(value.d_limit_high), + d_limit_low: T::from(value.d_limit_low), + o_limit_high: T::from(value.o_limit_high), + o_limit_low: T::from(value.o_limit_low), + i_term: T::from(value.i_term), + prev_measurement: value.prev_measurement.map(|v| T::from(v)), + } + } +} + impl Pid { /// Creates a new controller @@ -221,124 +245,34 @@ impl Pid impl Pid { - /// Creates a new controller - /// - /// To set your P, I, and D gains into this controller, please use the following builder methods: - /// - [Self::p()]: Proportional gain setting - /// - [Self::i()]: Integral gain setting - /// - [Self::d()]: Derivative gain setting pub const fn new() -> Self { - Self { - setpoint: 0, - kp: 0, - ki: 0, - kd: 0, - p_limit_high: 0, - p_limit_low: 0, - i_limit_high: 0, - i_limit_low: 0, - d_limit_high: 0, - d_limit_low: 0, - o_limit_high: 0, - o_limit_low: 0, - i_term: 0, - prev_measurement: None, - } + Self { ..Pid::::new() } } } impl Pid { - /// Creates a new controller - /// - /// To set your P, I, and D gains into this controller, please use the following builder methods: - /// - [Self::p()]: Proportional gain setting - /// - [Self::i()]: Integral gain setting - /// - [Self::d()]: Derivative gain setting pub const fn new() -> Self { - Self { - setpoint: 0, - kp: 0, - ki: 0, - kd: 0, - p_limit_high: 0, - p_limit_low: 0, - i_limit_high: 0, - i_limit_low: 0, - d_limit_high: 0, - d_limit_low: 0, - o_limit_high: 0, - o_limit_low: 0, - i_term: 0, - prev_measurement: None, - } + Self { ..Pid::::new() } } } impl Pid { - /// Creates a new controller - /// - /// To set your P, I, and D gains into this controller, please use the following builder methods: - /// - [Self::p()]: Proportional gain setting - /// - [Self::i()]: Integral gain setting - /// - [Self::d()]: Derivative gain setting pub const fn new() -> Self { - Self { - setpoint: 0, - kp: 0, - ki: 0, - kd: 0, - p_limit_high: 0, - p_limit_low: 0, - i_limit_high: 0, - i_limit_low: 0, - d_limit_high: 0, - d_limit_low: 0, - o_limit_high: 0, - o_limit_low: 0, - i_term: 0, - prev_measurement: None, - } + Self { ..Pid::::new() } } } impl Pid { - /// Creates a new controller - /// - /// To set your P, I, and D gains into this controller, please use the following builder methods: - /// - [Self::p()]: Proportional gain setting - /// - [Self::i()]: Integral gain setting - /// - [Self::d()]: Derivative gain setting pub const fn new() -> Self { - Self { - setpoint: 0, - kp: 0, - ki: 0, - kd: 0, - p_limit_high: 0, - p_limit_low: 0, - i_limit_high: 0, - i_limit_low: 0, - d_limit_high: 0, - d_limit_low: 0, - o_limit_high: 0, - o_limit_low: 0, - i_term: 0, - prev_measurement: None, - } + Self { ..Pid::::new() } } } impl Pid { - /// Creates a new controller - /// - /// To set your P, I, and D gains into this controller, please use the following builder methods: - /// - [Self::p()]: Proportional gain setting - /// - [Self::i()]: Integral gain setting - /// - [Self::d()]: Derivative gain setting pub const fn new() -> Self { Self { setpoint: 0.0, @@ -361,29 +295,8 @@ impl Pid impl Pid { - /// Creates a new controller - /// - /// To set your P, I, and D gains into this controller, please use the following builder methods: - /// - [Self::p()]: Proportional gain setting - /// - [Self::i()]: Integral gain setting - /// - [Self::d()]: Derivative gain setting pub const fn new() -> Self { - Self { - setpoint: 0.0, - kp: 0.0, - ki: 0.0, - kd: 0.0, - p_limit_high: 0.0, - p_limit_low: 0.0, - i_limit_high: 0.0, - i_limit_low: 0.0, - d_limit_high: 0.0, - d_limit_low: 0.0, - o_limit_high: 0.0, - o_limit_low: 0.0, - i_term: 0.0, - prev_measurement: None, - } + Self { ..Pid::::new() } } } From d5c92f47583996e4bbb27241527811fbf07b4968 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 15 May 2024 08:10:14 -0300 Subject: [PATCH 20/58] try into --- src/lib.rs | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 0827671..9bca62e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -246,28 +246,28 @@ impl Pid impl Pid { pub const fn new() -> Self { - Self { ..Pid::::new() } + Self { ..Pid::::new().into() } } } impl Pid { pub const fn new() -> Self { - Self { ..Pid::::new() } + Self { ..Pid::::new().into() } } } impl Pid { pub const fn new() -> Self { - Self { ..Pid::::new() } + Self { ..Pid::::new().into() } } } impl Pid { pub const fn new() -> Self { - Self { ..Pid::::new() } + Self { ..Pid::::new().into() } } } @@ -296,7 +296,7 @@ impl Pid impl Pid { pub const fn new() -> Self { - Self { ..Pid::::new() } + Self { ..Pid::::new().into() } } } From a527d232c2e8e8916f6dc50a4baa296b243eee2a Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 15 May 2024 08:13:13 -0300 Subject: [PATCH 21/58] try const trait --- src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index 9bca62e..0bc0a28 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -191,7 +191,7 @@ pub struct ControlOutput { pub output: T, } -impl From> for Pid +impl const From> for Pid where T: From { From 0073c79992e77a48d98da729d156c5cce9624953 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 15 May 2024 08:16:06 -0300 Subject: [PATCH 22/58] fix const syntax --- src/lib.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/lib.rs b/src/lib.rs index 0bc0a28..7b2c0cb 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -191,6 +191,7 @@ pub struct ControlOutput { pub output: T, } +#![feature(const_trait_impl)] impl const From> for Pid where T: From From 52065f06fda21e8e43d863a07ceec7c9ea5db60f Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 15 May 2024 08:19:18 -0300 Subject: [PATCH 23/58] Remove const trait --- src/lib.rs | 119 +++++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 106 insertions(+), 13 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 7b2c0cb..e5425c4 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -191,8 +191,7 @@ pub struct ControlOutput { pub output: T, } -#![feature(const_trait_impl)] -impl const From> for Pid +impl From> for Pid where T: From { @@ -218,12 +217,6 @@ where impl Pid { - /// Creates a new controller - /// - /// To set your P, I, and D gains into this controller, please use the following builder methods: - /// - [Self::p()]: Proportional gain setting - /// - [Self::i()]: Integral gain setting - /// - [Self::d()]: Derivative gain setting pub const fn new() -> Self { Self { setpoint: 0, @@ -247,28 +240,88 @@ impl Pid impl Pid { pub const fn new() -> Self { - Self { ..Pid::::new().into() } + Self { + setpoint: 0, + kp: 0, + ki: 0, + kd: 0, + p_limit_high: 0, + p_limit_low: 0, + i_limit_high: 0, + i_limit_low: 0, + d_limit_high: 0, + d_limit_low: 0, + o_limit_high: 0, + o_limit_low: 0, + i_term: 0, + prev_measurement: None, + } } } impl Pid { pub const fn new() -> Self { - Self { ..Pid::::new().into() } + Self { + setpoint: 0, + kp: 0, + ki: 0, + kd: 0, + p_limit_high: 0, + p_limit_low: 0, + i_limit_high: 0, + i_limit_low: 0, + d_limit_high: 0, + d_limit_low: 0, + o_limit_high: 0, + o_limit_low: 0, + i_term: 0, + prev_measurement: None, + } } } impl Pid { pub const fn new() -> Self { - Self { ..Pid::::new().into() } + Self { + setpoint: 0, + kp: 0, + ki: 0, + kd: 0, + p_limit_high: 0, + p_limit_low: 0, + i_limit_high: 0, + i_limit_low: 0, + d_limit_high: 0, + d_limit_low: 0, + o_limit_high: 0, + o_limit_low: 0, + i_term: 0, + prev_measurement: None, + } } } impl Pid { pub const fn new() -> Self { - Self { ..Pid::::new().into() } + Self { + setpoint: 0, + kp: 0, + ki: 0, + kd: 0, + p_limit_high: 0, + p_limit_low: 0, + i_limit_high: 0, + i_limit_low: 0, + d_limit_high: 0, + d_limit_low: 0, + o_limit_high: 0, + o_limit_low: 0, + i_term: 0, + prev_measurement: None, + } } } @@ -297,12 +350,52 @@ impl Pid impl Pid { pub const fn new() -> Self { - Self { ..Pid::::new().into() } + Self { + setpoint: 0.0, + kp: 0.0, + ki: 0.0, + kd: 0.0, + p_limit_high: 0.0, + p_limit_low: 0.0, + i_limit_high: 0.0, + i_limit_low: 0.0, + d_limit_high: 0.0, + d_limit_low: 0.0, + o_limit_high: 0.0, + o_limit_low: 0.0, + i_term: 0.0, + prev_measurement: None, + } } } impl Pid { + /// Creates a new controller + /// + /// To set your P, I, and D gains into this controller, please use the following builder methods: + /// - [Self::p()]: Proportional gain setting + /// - [Self::i()]: Integral gain setting + /// - [Self::d()]: Derivative gain setting + pub fn new() -> Self { + Self { + setpoint: T::default(), + kp: T::default(), + ki: T::default(), + kd: T::default(), + p_limit_high: T::default(), + p_limit_low: T::default(), + i_limit_high: T::default(), + i_limit_low: T::default(), + d_limit_high: T::default(), + d_limit_low: T::default(), + o_limit_high: T::default(), + o_limit_low: T::default(), + i_term: T::default(), + prev_measurement: None, + } + } + /// Sets the [Self::p] gain for this controller. pub fn p(&mut self, gain: impl Into) -> &mut Self { self.kp = gain.into(); From bae75b6b07589a6e1b97f8f5d1f4b3aa85393e05 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 15 May 2024 08:23:32 -0300 Subject: [PATCH 24/58] try generic except types --- src/lib.rs | 53 ++++++++++++++++++++++++++++------------------------- 1 file changed, 28 insertions(+), 25 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index e5425c4..a137f30 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -215,6 +215,34 @@ where } } +impl Pid +{ + /// Creates a new controller + /// + /// To set your P, I, and D gains into this controller, please use the following builder methods: + /// - [Self::p()]: Proportional gain setting + /// - [Self::i()]: Integral gain setting + /// - [Self::d()]: Derivative gain setting + pub fn new() -> Self { + Self { + setpoint: T::default(), + kp: T::default(), + ki: T::default(), + kd: T::default(), + p_limit_high: T::default(), + p_limit_low: T::default(), + i_limit_high: T::default(), + i_limit_low: T::default(), + d_limit_high: T::default(), + d_limit_low: T::default(), + o_limit_high: T::default(), + o_limit_low: T::default(), + i_term: T::default(), + prev_measurement: None, + } + } +} + impl Pid { pub const fn new() -> Self { @@ -371,31 +399,6 @@ impl Pid impl Pid { - /// Creates a new controller - /// - /// To set your P, I, and D gains into this controller, please use the following builder methods: - /// - [Self::p()]: Proportional gain setting - /// - [Self::i()]: Integral gain setting - /// - [Self::d()]: Derivative gain setting - pub fn new() -> Self { - Self { - setpoint: T::default(), - kp: T::default(), - ki: T::default(), - kd: T::default(), - p_limit_high: T::default(), - p_limit_low: T::default(), - i_limit_high: T::default(), - i_limit_low: T::default(), - d_limit_high: T::default(), - d_limit_low: T::default(), - o_limit_high: T::default(), - o_limit_low: T::default(), - i_term: T::default(), - prev_measurement: None, - } - } - /// Sets the [Self::p] gain for this controller. pub fn p(&mut self, gain: impl Into) -> &mut Self { self.kp = gain.into(); From b83262c5123dba53f7569cd28ab05fcfda9062a2 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 15 May 2024 08:26:50 -0300 Subject: [PATCH 25/58] impl const new --- src/lib.rs | 67 ++++++++++++++++++++++++++---------------------------- 1 file changed, 32 insertions(+), 35 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index a137f30..3ba66a4 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -215,37 +215,9 @@ where } } -impl Pid -{ - /// Creates a new controller - /// - /// To set your P, I, and D gains into this controller, please use the following builder methods: - /// - [Self::p()]: Proportional gain setting - /// - [Self::i()]: Integral gain setting - /// - [Self::d()]: Derivative gain setting - pub fn new() -> Self { - Self { - setpoint: T::default(), - kp: T::default(), - ki: T::default(), - kd: T::default(), - p_limit_high: T::default(), - p_limit_low: T::default(), - i_limit_high: T::default(), - i_limit_low: T::default(), - d_limit_high: T::default(), - d_limit_low: T::default(), - o_limit_high: T::default(), - o_limit_low: T::default(), - i_term: T::default(), - prev_measurement: None, - } - } -} - impl Pid { - pub const fn new() -> Self { + pub const fn new_const() -> Self { Self { setpoint: 0, kp: 0, @@ -267,7 +239,7 @@ impl Pid impl Pid { - pub const fn new() -> Self { + pub const fn new_const() -> Self { Self { setpoint: 0, kp: 0, @@ -289,7 +261,7 @@ impl Pid impl Pid { - pub const fn new() -> Self { + pub const fn new_const() -> Self { Self { setpoint: 0, kp: 0, @@ -311,7 +283,7 @@ impl Pid impl Pid { - pub const fn new() -> Self { + pub const fn new_const() -> Self { Self { setpoint: 0, kp: 0, @@ -333,7 +305,7 @@ impl Pid impl Pid { - pub const fn new() -> Self { + pub const fn new_const() -> Self { Self { setpoint: 0, kp: 0, @@ -355,7 +327,7 @@ impl Pid impl Pid { - pub const fn new() -> Self { + pub const fn new_const() -> Self { Self { setpoint: 0.0, kp: 0.0, @@ -377,7 +349,7 @@ impl Pid impl Pid { - pub const fn new() -> Self { + pub const fn new_const() -> Self { Self { setpoint: 0.0, kp: 0.0, @@ -399,6 +371,31 @@ impl Pid impl Pid { + /// Creates a new controller + /// + /// To set your P, I, and D gains into this controller, please use the following builder methods: + /// - [Self::p()]: Proportional gain setting + /// - [Self::i()]: Integral gain setting + /// - [Self::d()]: Derivative gain setting + pub fn new() -> Self { + Self { + setpoint: T::default(), + kp: T::default(), + ki: T::default(), + kd: T::default(), + p_limit_high: T::default(), + p_limit_low: T::default(), + i_limit_high: T::default(), + i_limit_low: T::default(), + d_limit_high: T::default(), + d_limit_low: T::default(), + o_limit_high: T::default(), + o_limit_low: T::default(), + i_term: T::default(), + prev_measurement: None, + } + } + /// Sets the [Self::p] gain for this controller. pub fn p(&mut self, gain: impl Into) -> &mut Self { self.kp = gain.into(); From 1e6d66fd5f749554604ce79d0020eb50238edf0a Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 15 May 2024 08:28:29 -0300 Subject: [PATCH 26/58] remove conflicting trait --- src/lib.rs | 24 ------------------------ 1 file changed, 24 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 3ba66a4..8e01d3b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -191,30 +191,6 @@ pub struct ControlOutput { pub output: T, } -impl From> for Pid -where - T: From -{ - fn from(value: Pid) -> Self { - Self { - setpoint: T::from(value.setpoint), - kp: T::from(value.kp), - ki: T::from(value.ki), - kd: T::from(value.kd), - p_limit_high: T::from(value.p_limit_high), - p_limit_low: T::from(value.p_limit_low), - i_limit_high: T::from(value.i_limit_high), - i_limit_low: T::from(value.i_limit_low), - d_limit_high: T::from(value.d_limit_high), - d_limit_low: T::from(value.d_limit_low), - o_limit_high: T::from(value.o_limit_high), - o_limit_low: T::from(value.o_limit_low), - i_term: T::from(value.i_term), - prev_measurement: value.prev_measurement.map(|v| T::from(v)), - } - } -} - impl Pid { pub const fn new_const() -> Self { From 172cdb5cb41bb028af3fa3751cf1f00acc613587 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 20 May 2024 13:26:23 -0300 Subject: [PATCH 27/58] implement error type --- src/lib.rs | 477 +++++++++++++++++++++-------------------------------- 1 file changed, 190 insertions(+), 287 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 8e01d3b..fd6b9cf 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -14,20 +14,20 @@ //! .p(10.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).unwrap(); //! //! // 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).unwrap(); //! 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); -//! let output = pid.next_control_output(10.0); +//! let output = pid.next_control_output(10.0).unwrap(); //! //! // Now that the integral makes the controller stateful, it will change //! assert_eq!(output.output, 55.0); // <-- @@ -36,7 +36,7 @@ //! //! // Add our final derivative term and match our setpoint target //! pid.d(2.0); -//! let output = pid.next_control_output(15.0); +//! let output = pid.next_control_output(15.0).unwrap(); //! //! // The output will now say to go down due to the derivative //! assert_eq!(output.output, -5.0); // <-- @@ -46,6 +46,10 @@ //! ``` #![no_std] +use core::num; + +use num_traits::CheckedMul; + /// A trait for any numeric type usable in the PID controller /// /// This trait is automatically implemented for all types that satisfy `PartialOrd + num_traits::Signed + Copy`. This includes all of the signed float types and builtin integer except for [isize]: @@ -58,25 +62,22 @@ /// - [f64] /// /// As well as any user type that matches the requirements -pub trait Number: num_traits::sign::Signed + PartialOrd + Default + Copy + Send { - fn clamp(self, min: Self, max: Self) -> Self; -} - -// Implement `Number` for all types that -// satisfy `PartialOrd + num_traits::Signed + Copy`. -impl Number for T -where - T: num_traits::sign::Signed - + PartialOrd - + Default - + Copy - + Send, -{ - fn clamp(self, min: Self, max: Self) -> Self { - if self < min {min} else - if self > max {max} else - {self} - } +pub trait Number: num_traits::sign::Signed + + num_traits::ops::checked::CheckedAdd + + num_traits::ops::checked::CheckedSub + + num_traits::ops::checked::CheckedMul + + num_traits::ops::checked::CheckedDiv + + num_traits::ops::checked::CheckedNeg + + PartialOrd + + Copy + + Send +{} + +/// An error emitted due to problems with the PID controller. +#[derive(Debug)] +pub enum PidError { + NoSetpoint, + OpOverflow, } /// Adjustable proportional-integral-derivative (PID) controller. @@ -95,7 +96,7 @@ where /// .p(10.0); /// /// // Get first output -/// let p_output = p_controller.next_control_output(400.0); +/// let p_output = p_controller.next_control_output(400.0).unwrap(); /// ``` /// /// 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: @@ -112,7 +113,7 @@ where /// .d(0.25); /// /// // Get first output -/// let full_output = full_controller.next_control_output(400.0); +/// let full_output = full_controller.next_control_output(400.0).unwrap(); /// ``` /// /// 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. @@ -126,33 +127,25 @@ where #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct Pid { /// Ideal setpoint to strive for. - pub setpoint: T, + pub setpoint: Option, /// Proportional gain. - pub kp: T, + pub kp: Option, /// Integral gain. - pub ki: T, + pub ki: Option, /// Derivative gain. - pub kd: T, + pub kd: Option, + /// Last calculated integral value if [Pid::ki] is used. + pub i_term: Option, + /// Previously found measurement whilst using the [Pid::next_control_output] method. + pub prev_measurement: Option, /// Limiter for the proportional term: `-p_limit <= P <= p_limit`. - pub p_limit_high: T, - /// Limiter for the derivative term: `p_limit_low <= P <= p_limit`. - pub p_limit_low: T, + pub p_limit: PidLimit, /// Limiter for the integral term: `i_limit_low <= I <= i_limit_high`. - pub i_limit_high: T, - /// Limiter for the derivative term: `i_limit_low <= I <= i_limit_high`. - pub i_limit_low: T, - /// Limiter for the derivative term: `d_limit_low <= D <= d_limit_high`. - pub d_limit_high: T, + pub i_limit: PidLimit, /// Limiter for the derivative term: `d_limit_low <= D <= d_limit_high`. - pub d_limit_low: T, + pub d_limit: PidLimit, /// Limiter for the derivative term: `o_limit_low <= O <= o_limit_high`. - pub o_limit_high: T, - /// Limiter for the derivative term: `o_limit_low <= O <= o_limit_high`. - pub o_limit_low: T, - /// Last calculated integral value if [Pid::ki] is used. - pub i_term: T, - /// Previously found measurement whilst using the [Pid::next_control_output] method. - pub prev_measurement: Option, + pub out_limit: PidLimit, } /// Output of [controller iterations](Pid::next_control_output) with weights @@ -173,10 +166,10 @@ pub struct Pid { /// .d(2.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).unwrap(); /// println!("P: {}\nI: {}\nD: {}\nFinal Output: {}", output.p, output.i, output.d, output.output); /// ``` -#[derive(Debug, PartialEq, Eq)] +#[derive(Debug, PartialEq, Eq, Clone, Copy)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct ControlOutput { /// Contribution of the P term to the output. @@ -191,161 +184,39 @@ pub struct ControlOutput { pub output: T, } -impl Pid -{ - pub const fn new_const() -> Self { - Self { - setpoint: 0, - kp: 0, - ki: 0, - kd: 0, - p_limit_high: 0, - p_limit_low: 0, - i_limit_high: 0, - i_limit_low: 0, - d_limit_high: 0, - d_limit_low: 0, - o_limit_high: 0, - o_limit_low: 0, - i_term: 0, - prev_measurement: None, - } - } -} - -impl Pid -{ - pub const fn new_const() -> Self { - Self { - setpoint: 0, - kp: 0, - ki: 0, - kd: 0, - p_limit_high: 0, - p_limit_low: 0, - i_limit_high: 0, - i_limit_low: 0, - d_limit_high: 0, - d_limit_low: 0, - o_limit_high: 0, - o_limit_low: 0, - i_term: 0, - prev_measurement: None, - } - } -} - -impl Pid -{ - pub const fn new_const() -> Self { - Self { - setpoint: 0, - kp: 0, - ki: 0, - kd: 0, - p_limit_high: 0, - p_limit_low: 0, - i_limit_high: 0, - i_limit_low: 0, - d_limit_high: 0, - d_limit_low: 0, - o_limit_high: 0, - o_limit_low: 0, - i_term: 0, - prev_measurement: None, - } - } -} - -impl Pid -{ - pub const fn new_const() -> Self { - Self { - setpoint: 0, - kp: 0, - ki: 0, - kd: 0, - p_limit_high: 0, - p_limit_low: 0, - i_limit_high: 0, - i_limit_low: 0, - d_limit_high: 0, - d_limit_low: 0, - o_limit_high: 0, - o_limit_low: 0, - i_term: 0, - prev_measurement: None, - } - } +#[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)] +#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] +pub struct PidLimit { + min: Option, + max: Option, } -impl Pid +impl PidLimit +where + T: Number { - pub const fn new_const() -> Self { + pub const fn new() -> Self { Self { - setpoint: 0, - kp: 0, - ki: 0, - kd: 0, - p_limit_high: 0, - p_limit_low: 0, - i_limit_high: 0, - i_limit_low: 0, - d_limit_high: 0, - d_limit_low: 0, - o_limit_high: 0, - o_limit_low: 0, - i_term: 0, - prev_measurement: None, + min: None, + max: None, } } -} -impl Pid -{ - pub const fn new_const() -> Self { - Self { - setpoint: 0.0, - kp: 0.0, - ki: 0.0, - kd: 0.0, - p_limit_high: 0.0, - p_limit_low: 0.0, - i_limit_high: 0.0, - i_limit_low: 0.0, - d_limit_high: 0.0, - d_limit_low: 0.0, - o_limit_high: 0.0, - o_limit_low: 0.0, - i_term: 0.0, - prev_measurement: None, - } + pub fn clamp(self, value: impl Into) -> T { + let mut value: T = value.into(); + value = if let Some(min) = self.min { + if value < min {min} else {value} + } else {value}; + value = if let Some(max) = self.max { + if value > max {max} else {value} + } else {value}; + value } } -impl Pid -{ - pub const fn new_const() -> Self { - Self { - setpoint: 0.0, - kp: 0.0, - ki: 0.0, - kd: 0.0, - p_limit_high: 0.0, - p_limit_low: 0.0, - i_limit_high: 0.0, - i_limit_low: 0.0, - d_limit_high: 0.0, - d_limit_low: 0.0, - o_limit_high: 0.0, - o_limit_low: 0.0, - i_term: 0.0, - prev_measurement: None, - } - } -} - -impl Pid +impl Pid +where + T: Number { /// Creates a new controller /// @@ -353,61 +224,68 @@ impl Pid /// - [Self::p()]: Proportional gain setting /// - [Self::i()]: Integral gain setting /// - [Self::d()]: Derivative gain setting - pub fn new() -> Self { + pub const fn new() -> Self { Self { - setpoint: T::default(), - kp: T::default(), - ki: T::default(), - kd: T::default(), - p_limit_high: T::default(), - p_limit_low: T::default(), - i_limit_high: T::default(), - i_limit_low: T::default(), - d_limit_high: T::default(), - d_limit_low: T::default(), - o_limit_high: T::default(), - o_limit_low: T::default(), - i_term: T::default(), + setpoint: None, + kp: None, + ki: None, + kd: None, + i_term: None, prev_measurement: None, + p_limit: PidLimit::::new(), + i_limit: PidLimit::::new(), + d_limit: PidLimit::::new(), + out_limit: PidLimit::::new(), } } /// Sets the [Self::p] gain for this controller. pub fn p(&mut self, gain: impl Into) -> &mut Self { - self.kp = gain.into(); + self.kp = Some(gain.into()); self } /// Sets the [Self::i] gain for this controller. pub fn i(&mut self, gain: impl Into) -> &mut Self { - self.ki = gain.into(); + self.ki = Some(gain.into()); self } /// Sets the [Self::d] gain for this controller. pub fn d(&mut self, gain: impl Into) -> &mut Self { - self.kd = gain.into(); + self.kd = Some(gain.into()); self } /// Sets the [Pid::setpoint] to target for this controller. pub fn setpoint(&mut self, setpoint: impl Into) -> &mut Self { - self.setpoint = setpoint.into(); + self.setpoint = Some(setpoint.into()); + self + } + + /// Sets the min and max limits for this controller. + pub fn clamp(&mut self, min: impl Into, max: impl Into) -> &mut Self { + self.clamp_min(min) + .clamp_max(max) + } + + /// Sets the min limits for this controller. + pub fn clamp_max(&mut self, max: impl Into) -> &mut Self { + let max: T = max.into(); + self.p_limit.max = Some(max); + self.i_limit.max = Some(max); + self.d_limit.max = Some(max); + self.out_limit.max = Some(max); self } - /// Sets the [Self::p] limit for this controller. - pub fn clamp(&mut self, low: impl Into, high: impl Into) -> &mut Self { - let low: T = low.into(); - let high: T = high.into(); - self.p_limit_low = low; - self.p_limit_high = high; - self.i_limit_low = low; - self.i_limit_high = high; - self.d_limit_low = low; - self.d_limit_high = high; - self.o_limit_low = low; - self.o_limit_high = high; + /// Sets the max limits for this controller. + pub fn clamp_min(&mut self, min: impl Into) -> &mut Self { + let min: T = min.into(); + self.p_limit.min = Some(min); + self.i_limit.min = Some(min); + self.d_limit.min = Some(min); + self.out_limit.min = Some(min); self } @@ -415,64 +293,89 @@ impl Pid /// pid controller to a previous state after an interruption or crash. pub fn set_integral_term(&mut self, i_term: impl Into) -> &mut Self { let i_unbound: T = i_term.into(); - self.i_term = i_unbound.clamp(self.i_limit_low, self.i_limit_high); + self.i_term = Some(self.i_limit.clamp(i_unbound)); self } /// Resets the integral term back to zero, this may drastically change the /// control output. pub fn reset_integral_term(&mut self) -> &mut Self { - self.set_integral_term(T::default()) + self.set_integral_term(T::zero()) } /// 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: impl Into) -> ControlOutput { + /// # Returns Error + /// + /// - If a setpoint has not been set via `setpoint()`. + /// - If an overflow occured in one of the calculations. + pub fn next_control_output(&mut self, measurement: impl Into) -> Result, PidError> { let measurement: T = measurement.into(); // 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 = p_unbounded.clamp(self.p_limit_low, self.p_limit_high); - - // 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. - let i_unbounded = self.i_term + error * self.ki; - - // Mitigate integral windup: Don't want to keep building up error - // beyond what i_limit will allow. - self.i_term = i_unbounded.clamp(self.i_limit_low, self.i_limit_high); - - // 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::default(), - } * self.kd; - self.prev_measurement = Some(measurement); - let d = d_unbounded.clamp(self.d_limit_low, self.d_limit_high); + let setpoint = self.setpoint.ok_or(PidError::NoSetpoint)?; + let error = setpoint.checked_sub(&measurement) + .ok_or(PidError::OpOverflow)?; + + let p = if let Some(kp) = self.kp { + // Calculate the proportional term and limit to it's individual limit + let p_unbounded = kp.checked_mul(&error) + .ok_or(PidError::OpOverflow)?; + self.p_limit.clamp(p_unbounded) + } else {T::zero()}; + + self.i_term = if let Some(ki) = self.ki { + let i_term = if let Some(i_term) = self.i_term {i_term} + else {T::zero()}; + // 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. + let i_unbounded = ki.checked_mul(&error) + .ok_or(PidError::OpOverflow)? + .checked_add(&i_term) + .ok_or(PidError::OpOverflow)?; + // Mitigate integral windup: Don't want to keep building up error + // beyond what i_limit will allow. + Some(self.i_limit.clamp(i_unbounded)) + } else {self.i_term}; + + let i = if let Some(i) = self.i_term {i} + else {T::zero()}; + + let d = if let Some(kd) = self.kd { + // Mitigate derivative kick: Use the derivative of the measurement + // rather than the derivative of the error. + if let Some(prev_measurement) = self.prev_measurement { + let d_unbounded = measurement.checked_sub(&prev_measurement) + .ok_or(PidError::OpOverflow)? + .checked_mul(&kd) + .ok_or(PidError::OpOverflow)?; + self.d_limit.clamp(d_unbounded) + } else {T::zero()} + } else {T::zero()}; + + let output = { + // Calculate the final output by adding together the PID terms, then + // apply the final defined output limit + let o_unbounded = p.checked_add(&i) + .ok_or(PidError::OpOverflow)? + .checked_add(&d) + .ok_or(PidError::OpOverflow)?; + self.out_limit.clamp(o_unbounded) + }; - // Calculate the final output by adding together the PID terms, then - // apply the final defined output limit - let o_unbounded = p + self.i_term + d; - let output = o_unbounded.clamp(self.o_limit_low, self.o_limit_high); + self.prev_measurement = Some(measurement); // Return the individual term's contributions and the final output - ControlOutput { + Ok(ControlOutput { p, - i: self.i_term, + i, d, output, - } + }) } } @@ -492,11 +395,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).unwrap().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).unwrap().output, 10.0); } /// Derivative-only controller operation and limits @@ -508,14 +411,14 @@ mod tests { .d(2.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).unwrap().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).unwrap().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).unwrap().output, -5.0); } /// Integral-only controller operation and limits @@ -527,15 +430,15 @@ mod tests { .i(2.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).unwrap().output, 20.0); + assert_eq!(pid.next_control_output(0.0).unwrap().output, 40.0); + assert_eq!(pid.next_control_output(5.0).unwrap().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).unwrap().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).unwrap().output, 40.0); // Test that error integral accumulates negative values let mut pid2 = Pid::new() @@ -543,14 +446,14 @@ mod tests { .clamp(-100.0, 100.0) .i(2.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).unwrap().output, -20.0); + assert_eq!(pid2.next_control_output(0.0).unwrap().output, -40.0); pid2.i_limit_high = 50.0; pid2.i_limit_low = -50.0; - assert_eq!(pid2.next_control_output(-5.0).output, -50.0); + assert_eq!(pid2.next_control_output(-5.0).unwrap().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).unwrap().output, -40.0); } /// Checks that a full PID controller's limits work properly through multiple output iterations @@ -564,11 +467,11 @@ mod tests { pid.o_limit_high = 1.0; pid.o_limit_low = -1.0; - let out = pid.next_control_output(0.0); + let out = pid.next_control_output(0.0).unwrap(); 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).unwrap(); assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0) assert_eq!(out.output, -1.0); } @@ -583,25 +486,25 @@ mod tests { .i(0.1) .d(1.0); - let out = pid.next_control_output(0.0); + let out = pid.next_control_output(0.0).unwrap(); 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).unwrap(); 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).unwrap(); 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).unwrap(); 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) @@ -622,8 +525,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).unwrap().output, + pid_f64.next_control_output(0.0).unwrap().output as f32 ); } } @@ -642,8 +545,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).unwrap().output, + pid_i8.next_control_output(0i8).unwrap().output as i32 ); } } @@ -658,7 +561,7 @@ mod tests { .i(0.1) .d(1.0); - let out = pid.next_control_output(0.0); + let out = pid.next_control_output(0.0).unwrap(); 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) @@ -667,7 +570,7 @@ mod tests { pid.setpoint(0.0); assert_eq!( - pid.next_control_output(0.0), + pid.next_control_output(0.0).unwrap(), ControlOutput { p: 0.0, i: 1.0, @@ -690,7 +593,7 @@ mod tests { pid.o_limit_high = -10.0; pid.o_limit_low = 10.0; - let out = pid.next_control_output(0.0); + let out = pid.next_control_output(0.0).unwrap(); assert_eq!(out.p, 10.0); assert_eq!(out.i, 10.0); assert_eq!(out.d, 0.0); From 47bdeb31da4e2253a58f0c742fadf43546e44b70 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 20 May 2024 13:38:41 -0300 Subject: [PATCH 28/58] fix errors --- src/lib.rs | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index fd6b9cf..506f0e4 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -63,14 +63,20 @@ use num_traits::CheckedMul; /// /// As well as any user type that matches the requirements pub trait Number: num_traits::sign::Signed - + num_traits::ops::checked::CheckedAdd + + PartialOrd + + Copy +{} + +pub trait CheckedBasic: num_traits::ops::checked::CheckedAdd + num_traits::ops::checked::CheckedSub + num_traits::ops::checked::CheckedMul + num_traits::ops::checked::CheckedDiv + num_traits::ops::checked::CheckedNeg - + PartialOrd - + Copy - + Send +{} + +impl CheckedBasic for T +where + T: Number {} /// An error emitted due to problems with the PID controller. @@ -216,7 +222,7 @@ where impl Pid where - T: Number + T: Number + CheckedBasic { /// Creates a new controller /// From f8e8651ffc2ec9e4b9a82804f34d269e0c1eeb6a Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 20 May 2024 13:40:51 -0300 Subject: [PATCH 29/58] Update lib.rs --- src/lib.rs | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 506f0e4..7fb3ccc 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -65,6 +65,7 @@ use num_traits::CheckedMul; pub trait Number: num_traits::sign::Signed + PartialOrd + Copy + + CheckedBasic {} pub trait CheckedBasic: num_traits::ops::checked::CheckedAdd @@ -74,11 +75,6 @@ pub trait CheckedBasic: num_traits::ops::checked::CheckedAdd + num_traits::ops::checked::CheckedNeg {} -impl CheckedBasic for T -where - T: Number -{} - /// An error emitted due to problems with the PID controller. #[derive(Debug)] pub enum PidError { @@ -222,7 +218,7 @@ where impl Pid where - T: Number + CheckedBasic + T: Number { /// Creates a new controller /// From ed9361f4b90d0412faa727c1220a7c7351381d99 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 20 May 2024 13:48:56 -0300 Subject: [PATCH 30/58] Update lib.rs --- src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index 7fb3ccc..8e40787 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -76,7 +76,7 @@ pub trait CheckedBasic: num_traits::ops::checked::CheckedAdd {} /// An error emitted due to problems with the PID controller. -#[derive(Debug)] +#[derive(Debug, core::error::Error)] pub enum PidError { NoSetpoint, OpOverflow, From ffa9be64cb090e9de024a25a8b8678a72db69a7b Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 20 May 2024 13:50:41 -0300 Subject: [PATCH 31/58] Update lib.rs --- src/lib.rs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index 8e40787..ae9dea8 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -76,12 +76,14 @@ pub trait CheckedBasic: num_traits::ops::checked::CheckedAdd {} /// An error emitted due to problems with the PID controller. -#[derive(Debug, core::error::Error)] +#[derive(Debug)] pub enum PidError { NoSetpoint, OpOverflow, } +impl core::error::Error for PidError {} + /// Adjustable proportional-integral-derivative (PID) controller. /// /// # Examples From ba95af6ade97beadef1a2d2628672c214a05222b Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 20 May 2024 13:51:40 -0300 Subject: [PATCH 32/58] Update lib.rs --- src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index ae9dea8..c1a80eb 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -76,7 +76,7 @@ pub trait CheckedBasic: num_traits::ops::checked::CheckedAdd {} /// An error emitted due to problems with the PID controller. -#[derive(Debug)] +#[derive(Debug, Display)] pub enum PidError { NoSetpoint, OpOverflow, From 8389e50a705753ccaa00e47cf8cde23ec766f999 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 20 May 2024 13:52:46 -0300 Subject: [PATCH 33/58] fix errors --- src/lib.rs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index c1a80eb..7fb3ccc 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -76,14 +76,12 @@ pub trait CheckedBasic: num_traits::ops::checked::CheckedAdd {} /// An error emitted due to problems with the PID controller. -#[derive(Debug, Display)] +#[derive(Debug)] pub enum PidError { NoSetpoint, OpOverflow, } -impl core::error::Error for PidError {} - /// Adjustable proportional-integral-derivative (PID) controller. /// /// # Examples From ee99e4bf378ed7080c8ca041083349047f0d41ca Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 20 May 2024 14:52:05 -0300 Subject: [PATCH 34/58] remove checked --- src/lib.rs | 30 ++++++------------------------ 1 file changed, 6 insertions(+), 24 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 7fb3ccc..c5a5eef 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -68,18 +68,11 @@ pub trait Number: num_traits::sign::Signed + CheckedBasic {} -pub trait CheckedBasic: num_traits::ops::checked::CheckedAdd - + num_traits::ops::checked::CheckedSub - + num_traits::ops::checked::CheckedMul - + num_traits::ops::checked::CheckedDiv - + num_traits::ops::checked::CheckedNeg -{} - /// An error emitted due to problems with the PID controller. #[derive(Debug)] pub enum PidError { NoSetpoint, - OpOverflow, + OpsOverflow, } /// Adjustable proportional-integral-derivative (PID) controller. @@ -317,13 +310,11 @@ where // Calculate the error between the ideal setpoint and the current // measurement to compare against let setpoint = self.setpoint.ok_or(PidError::NoSetpoint)?; - let error = setpoint.checked_sub(&measurement) - .ok_or(PidError::OpOverflow)?; + let error = setpoint - measurement; let p = if let Some(kp) = self.kp { // Calculate the proportional term and limit to it's individual limit - let p_unbounded = kp.checked_mul(&error) - .ok_or(PidError::OpOverflow)?; + let p_unbounded = kp * error; self.p_limit.clamp(p_unbounded) } else {T::zero()}; @@ -335,10 +326,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. - let i_unbounded = ki.checked_mul(&error) - .ok_or(PidError::OpOverflow)? - .checked_add(&i_term) - .ok_or(PidError::OpOverflow)?; + let i_unbounded = (ki * error) + i_term; // Mitigate integral windup: Don't want to keep building up error // beyond what i_limit will allow. Some(self.i_limit.clamp(i_unbounded)) @@ -351,10 +339,7 @@ where // Mitigate derivative kick: Use the derivative of the measurement // rather than the derivative of the error. if let Some(prev_measurement) = self.prev_measurement { - let d_unbounded = measurement.checked_sub(&prev_measurement) - .ok_or(PidError::OpOverflow)? - .checked_mul(&kd) - .ok_or(PidError::OpOverflow)?; + let d_unbounded = kd * (measurement - prev_measurement); self.d_limit.clamp(d_unbounded) } else {T::zero()} } else {T::zero()}; @@ -362,10 +347,7 @@ where let output = { // Calculate the final output by adding together the PID terms, then // apply the final defined output limit - let o_unbounded = p.checked_add(&i) - .ok_or(PidError::OpOverflow)? - .checked_add(&d) - .ok_or(PidError::OpOverflow)?; + let o_unbounded = p + i + d; self.out_limit.clamp(o_unbounded) }; From 7a6715fb56540aff345f9d4fdf2e00c70ab83cca Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 20 May 2024 14:57:56 -0300 Subject: [PATCH 35/58] Update lib.rs --- src/lib.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index c5a5eef..37f1777 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -65,7 +65,6 @@ use num_traits::CheckedMul; pub trait Number: num_traits::sign::Signed + PartialOrd + Copy - + CheckedBasic {} /// An error emitted due to problems with the PID controller. From eb1a9b6ca73f906a056795541a7def5a6cf8f695 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 20 May 2024 15:04:00 -0300 Subject: [PATCH 36/58] Update lib.rs --- src/lib.rs | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 37f1777..5c9dba1 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -46,10 +46,6 @@ //! ``` #![no_std] -use core::num; - -use num_traits::CheckedMul; - /// A trait for any numeric type usable in the PID controller /// /// This trait is automatically implemented for all types that satisfy `PartialOrd + num_traits::Signed + Copy`. This includes all of the signed float types and builtin integer except for [isize]: @@ -117,7 +113,7 @@ pub enum PidError { /// # Type Warning /// /// [Number] is abstract and can be used with anything from a [i32] to an [i128] (as well as user-defined types). Because of this, very small types might overflow during calculation in [`next_control_output`](Self::next_control_output). You probably don't want to use [i8] or user-defined types around that size so keep that in mind when designing your controller. -#[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)] +#[derive(Clone, Copy, Eq, PartialEq, PartialOrd)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct Pid { /// Ideal setpoint to strive for. @@ -178,7 +174,7 @@ pub struct ControlOutput { pub output: T, } -#[derive(Clone, Copy, Eq, PartialEq, Ord, PartialOrd)] +#[derive(Clone, Copy, Eq, PartialEq, PartialOrd)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct PidLimit { min: Option, From 4799974464bc90666808cd82e983d3fbf9f4f37d Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 20 May 2024 15:06:01 -0300 Subject: [PATCH 37/58] Update lib.rs --- src/lib.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 5c9dba1..56cf457 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -115,7 +115,7 @@ pub enum PidError { /// [Number] is abstract and can be used with anything from a [i32] to an [i128] (as well as user-defined types). Because of this, very small types might overflow during calculation in [`next_control_output`](Self::next_control_output). You probably don't want to use [i8] or user-defined types around that size so keep that in mind when designing your controller. #[derive(Clone, Copy, Eq, PartialEq, PartialOrd)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] -pub struct Pid { +pub struct Pid { /// Ideal setpoint to strive for. pub setpoint: Option, /// Proportional gain. @@ -161,7 +161,7 @@ pub struct Pid { /// ``` #[derive(Debug, PartialEq, Eq, Clone, Copy)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] -pub struct ControlOutput { +pub struct ControlOutput { /// Contribution of the P term to the output. pub p: T, /// Contribution of the I term to the output. @@ -176,7 +176,7 @@ pub struct ControlOutput { #[derive(Clone, Copy, Eq, PartialEq, PartialOrd)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] -pub struct PidLimit { +pub struct PidLimit { min: Option, max: Option, } From edf59aacdd2c2a1d43b37019cf9149568cdd2a94 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 21 May 2024 13:59:20 -0300 Subject: [PATCH 38/58] Update lib.rs --- src/lib.rs | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 56cf457..a3e1c67 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -58,10 +58,11 @@ /// - [f64] /// /// As well as any user type that matches the requirements -pub trait Number: num_traits::sign::Signed - + PartialOrd - + Copy -{} +pub trait Number: PartialOrd + num_traits::Signed + Copy {} + +// Implement `Number` for all types that +// satisfy `PartialOrd + num_traits::Signed + Copy`. +impl Number for T {} /// An error emitted due to problems with the PID controller. #[derive(Debug)] From 36b8304c30510892a07572269133f9fb19c6646f Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 21 May 2024 14:07:58 -0300 Subject: [PATCH 39/58] change error to option --- src/lib.rs | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index a3e1c67..e0bfd22 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -64,13 +64,6 @@ pub trait Number: PartialOrd + num_traits::Signed + Copy {} // satisfy `PartialOrd + num_traits::Signed + Copy`. impl Number for T {} -/// An error emitted due to problems with the PID controller. -#[derive(Debug)] -pub enum PidError { - NoSetpoint, - OpsOverflow, -} - /// Adjustable proportional-integral-derivative (PID) controller. /// /// # Examples @@ -300,12 +293,15 @@ where /// /// - If a setpoint has not been set via `setpoint()`. /// - If an overflow occured in one of the calculations. - pub fn next_control_output(&mut self, measurement: impl Into) -> Result, PidError> { + pub fn next_control_output(&mut self, measurement: impl Into) -> Option> { let measurement: T = measurement.into(); // Calculate the error between the ideal setpoint and the current // measurement to compare against - let setpoint = self.setpoint.ok_or(PidError::NoSetpoint)?; + let setpoint = match self.setpoint { + Some(value) => value, + None => return None, + }; let error = setpoint - measurement; let p = if let Some(kp) = self.kp { @@ -350,7 +346,7 @@ where self.prev_measurement = Some(measurement); // Return the individual term's contributions and the final output - Ok(ControlOutput { + Some(ControlOutput { p, i, d, From 9324cab5688e099050582ba1972ac2ebbcf9e5dc Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 21 May 2024 18:58:26 -0300 Subject: [PATCH 40/58] Update lib.rs --- src/lib.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index e0bfd22..150361a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -284,7 +284,8 @@ where /// Resets the integral term back to zero, this may drastically change the /// control output. pub fn reset_integral_term(&mut self) -> &mut Self { - self.set_integral_term(T::zero()) + self.i_term = None; + self } /// Given a new measurement, calculates the next [control output](ControlOutput). From 142754c72f5a858d4b342bf95d06ae47bd09adfe Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 21 May 2024 19:02:30 -0300 Subject: [PATCH 41/58] Update lib.rs --- src/lib.rs | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 150361a..fd1a76d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -254,16 +254,6 @@ where } /// Sets the min limits for this controller. - pub fn clamp_max(&mut self, max: impl Into) -> &mut Self { - let max: T = max.into(); - self.p_limit.max = Some(max); - self.i_limit.max = Some(max); - self.d_limit.max = Some(max); - self.out_limit.max = Some(max); - self - } - - /// Sets the max limits for this controller. pub fn clamp_min(&mut self, min: impl Into) -> &mut Self { let min: T = min.into(); self.p_limit.min = Some(min); @@ -273,6 +263,16 @@ where self } + /// Sets the max limits for this controller. + pub fn clamp_max(&mut self, max: impl Into) -> &mut Self { + let max: T = max.into(); + self.p_limit.max = Some(max); + self.i_limit.max = Some(max); + self.d_limit.max = Some(max); + self.out_limit.max = Some(max); + self + } + /// Set integral term to a custom value. This might be useful to restore the /// pid controller to a previous state after an interruption or crash. pub fn set_integral_term(&mut self, i_term: impl Into) -> &mut Self { From 10e5c8d412c4432c7bb45f30d9af23dcd507f205 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 21 May 2024 20:48:17 -0300 Subject: [PATCH 42/58] implement dt and functional style --- src/lib.rs | 235 +++++++++++++++++++++++++++++++++-------------------- 1 file changed, 146 insertions(+), 89 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index fd1a76d..f24005c 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -14,7 +14,7 @@ //! .p(10.0); //! //! // Input a measurement with an error of 5.0 from our setpoint -//! let output = pid.next_control_output(10.0).unwrap(); +//! let output = pid.update(10.0).unwrap(); //! //! // Show that the error is correct by multiplying by our kp //! assert_eq!(output.output, 50.0); // <-- @@ -27,7 +27,7 @@ //! //! // Add a new integral term to the controller and input again //! pid.i(1.0); -//! let output = pid.next_control_output(10.0).unwrap(); +//! let output = pid.update(10.0).unwrap(); //! //! // Now that the integral makes the controller stateful, it will change //! assert_eq!(output.output, 55.0); // <-- @@ -36,7 +36,7 @@ //! //! // Add our final derivative term and match our setpoint target //! pid.d(2.0); -//! let output = pid.next_control_output(15.0).unwrap(); +//! let output = pid.update(15.0).unwrap(); //! //! // The output will now say to go down due to the derivative //! assert_eq!(output.output, -5.0); // <-- @@ -80,7 +80,7 @@ impl Number for T {} /// .p(10.0); /// /// // Get first output -/// let p_output = p_controller.next_control_output(400.0).unwrap(); +/// let p_output = p_controller.update(400.0).unwrap(); /// ``` /// /// 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: @@ -97,10 +97,10 @@ impl Number for T {} /// .d(0.25); /// /// // Get first output -/// let full_output = full_controller.next_control_output(400.0).unwrap(); +/// let full_output = full_controller.update(400.0).unwrap(); /// ``` /// -/// 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. +/// This [`update`](Self::update) 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. /// /// The last item of note is that these [`p`](Self::p()), [`i`](Self::i()), and [`d`](Self::d()) methods can be used *during* operation which lets you add and/or modify these controller values if need be. /// @@ -121,7 +121,7 @@ pub struct Pid { /// Last calculated integral value if [Pid::ki] is used. pub i_term: Option, /// Previously found measurement whilst using the [Pid::next_control_output] method. - pub prev_measurement: Option, + pub prev: Option>, /// Limiter for the proportional term: `-p_limit <= P <= p_limit`. pub p_limit: PidLimit, /// Limiter for the integral term: `i_limit_low <= I <= i_limit_high`. @@ -150,12 +150,16 @@ pub struct Pid { /// .d(2.0); /// /// // Input an example value and get a report for an output iteration -/// let output = pid.next_control_output(26.2456).unwrap(); +/// let output = pid.update(26.2456).unwrap(); /// println!("P: {}\nI: {}\nD: {}\nFinal Output: {}", output.p, output.i, output.d, output.output); /// ``` #[derive(Debug, PartialEq, Eq, Clone, Copy)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct ControlOutput { + /// The input value to the controller. + pub input: T, + /// The error value based on input. + pub error: T, /// Contribution of the P term to the output. pub p: T, /// Contribution of the I term to the output. @@ -186,6 +190,7 @@ where } } + /// Clamp a given value using pre-defined limits pub fn clamp(self, value: impl Into) -> T { let mut value: T = value.into(); value = if let Some(min) = self.min { @@ -215,7 +220,7 @@ where ki: None, kd: None, i_term: None, - prev_measurement: None, + prev: None, p_limit: PidLimit::::new(), i_limit: PidLimit::::new(), d_limit: PidLimit::::new(), @@ -290,52 +295,71 @@ where /// Given a new measurement, calculates the next [control output](ControlOutput). /// - /// # Returns Error + /// # Returns None /// /// - If a setpoint has not been set via `setpoint()`. - /// - If an overflow occured in one of the calculations. - pub fn next_control_output(&mut self, measurement: impl Into) -> Option> { - let measurement: T = measurement.into(); + /// - If no gain is set via `p()`, `i()` or `d()`. + pub fn update(&mut self, input: impl Into) -> Option> { + // Convert parameters to number type + let input: T = input.into(); - // Calculate the error between the ideal setpoint and the current - // measurement to compare against + // Return early if no setpoint is defined let setpoint = match self.setpoint { Some(value) => value, None => return None, }; - let error = setpoint - measurement; - - let p = if let Some(kp) = self.kp { - // Calculate the proportional term and limit to it's individual limit - let p_unbounded = kp * error; - self.p_limit.clamp(p_unbounded) - } else {T::zero()}; - - self.i_term = if let Some(ki) = self.ki { - let i_term = if let Some(i_term) = self.i_term {i_term} - else {T::zero()}; - // 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. - let i_unbounded = (ki * error) + i_term; - // Mitigate integral windup: Don't want to keep building up error - // beyond what i_limit will allow. - Some(self.i_limit.clamp(i_unbounded)) - } else {self.i_term}; - - let i = if let Some(i) = self.i_term {i} - else {T::zero()}; - - let d = if let Some(kd) = self.kd { - // Mitigate derivative kick: Use the derivative of the measurement - // rather than the derivative of the error. - if let Some(prev_measurement) = self.prev_measurement { - let d_unbounded = kd * (measurement - prev_measurement); - self.d_limit.clamp(d_unbounded) - } else {T::zero()} - } else {T::zero()}; + + // Return early if no gains are defined + if (self.kp == None) && + (self.ki == None) && + (self.kd == None) + { return None }; + + // Calculate the error between the ideal setpoint and the current + // measurement to compare against + let error = setpoint - input; + + // Calculate proportional if it exists + let p = self.kp.map_or( + T::zero(), + |kp| { + // Calculate the proportional term and limit to it's individual limit + let p_unbounded = kp * error; + self.p_limit.clamp(p_unbounded) + } + ); + + // Calculate integral if it exists + self.i_term = self.ki.map_or( + self.i_term, + |ki| { + let i_term = self.i_term.map_or(T::zero(), |i| i); + // 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. + let i_unbounded = (ki * error) + i_term; + // Mitigate integral windup: Don't want to keep building up error + // beyond what i_limit will allow. + Some(self.i_limit.clamp(i_unbounded)) + } + ); + // Get stored integral term if it exists + let i = self.i_term.map_or(T::zero(), |i| i); + + // Calculate derivative if it exists + let d = self.kd.map_or( + T::zero(), + |kd| self.prev.map_or( + T::zero(), + |prev| { + // Mitigate derivative kick: Use the derivative of the measurement + // rather than the derivative of the error. + let d_unbounded = kd * (input - prev.input); + self.d_limit.clamp(d_unbounded) + } + )); let output = { // Calculate the final output by adding together the PID terms, then @@ -344,15 +368,43 @@ where self.out_limit.clamp(o_unbounded) }; - self.prev_measurement = Some(measurement); - - // Return the individual term's contributions and the final output - Some(ControlOutput { + // Register the individual term's contributions and the final output + self.prev = Some(ControlOutput { + input, + error, p, i, d, output, - }) + }); + + // return output + self.prev.clone() + } + + /// Given a new measurement and a delta time, calculates the next [control output](ControlOutput). + /// + /// # Returns None + /// + /// - If a setpoint has not been set via `setpoint()`. + /// - If no gain is set via `p()`, `i()` or `d()`. + pub fn update_with_dt(&mut self, input: impl Into, dt: impl Into) -> Option> { + // Store previous integral sum + let i_term = self.i_term.map_or(T::zero(), |i| i); + // Call normal update + self.update(input) + .map(|out| self.ki.map_or( + Some(out), + |ki| { + // Convert parameters to number type + let dt: T = dt.into(); + // Calculate new integral term with delta time + out.i = (ki * out.error * dt) + i_term; + out.output = out.p + out.i + out.d; + self.prev = Some(out); + out + } + )) } } @@ -372,11 +424,12 @@ mod tests { assert_eq!(pid.setpoint, 10.0); // Test simple proportional - assert_eq!(pid.next_control_output(0.0).unwrap().output, 20.0); + assert_eq!(pid.update(0.0).unwrap().output, 20.0); // Test proportional limit - pid.p_limit = 10.0; - assert_eq!(pid.next_control_output(0.0).unwrap().output, 10.0); + pid.p_limit.max = Some(10.0); + pid.p_limit.min = Some(-10.0); + assert_eq!(pid.update(0.0).unwrap().output, 10.0); } /// Derivative-only controller operation and limits @@ -388,14 +441,15 @@ mod tests { .d(2.0); // Test that there's no derivative since it's the first measurement - assert_eq!(pid.next_control_output(0.0).unwrap().output, 0.0); + assert_eq!(pid.update(0.0).unwrap().output, 0.0); // Test that there's now a derivative - assert_eq!(pid.next_control_output(5.0).unwrap().output, -10.0); + assert_eq!(pid.update(5.0).unwrap().output, -10.0); // Test derivative limit - pid.d_limit = 5.0; - assert_eq!(pid.next_control_output(10.0).unwrap().output, -5.0); + pid.d_limit.max = Some(5.0); + pid.d_limit.min = Some(-5.0); + assert_eq!(pid.update(10.0).unwrap().output, -5.0); } /// Integral-only controller operation and limits @@ -407,15 +461,16 @@ mod tests { .i(2.0); // Test basic integration - assert_eq!(pid.next_control_output(0.0).unwrap().output, 20.0); - assert_eq!(pid.next_control_output(0.0).unwrap().output, 40.0); - assert_eq!(pid.next_control_output(5.0).unwrap().output, 50.0); + assert_eq!(pid.update(0.0).unwrap().output, 20.0); + assert_eq!(pid.update(0.0).unwrap().output, 40.0); + assert_eq!(pid.update(5.0).unwrap().output, 50.0); // Test limit - pid.i_limit = 50.0; - assert_eq!(pid.next_control_output(5.0).unwrap().output, 50.0); + pid.i_limit.max = Some(50.0); + pid.i_limit.min = Some(-50.0); + assert_eq!(pid.update(5.0).unwrap().output, 50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid.next_control_output(15.0).unwrap().output, 40.0); + assert_eq!(pid.update(15.0).unwrap().output, 40.0); // Test that error integral accumulates negative values let mut pid2 = Pid::new() @@ -423,14 +478,14 @@ mod tests { .clamp(-100.0, 100.0) .i(2.0); - assert_eq!(pid2.next_control_output(0.0).unwrap().output, -20.0); - assert_eq!(pid2.next_control_output(0.0).unwrap().output, -40.0); + assert_eq!(pid2.update(0.0).unwrap().output, -20.0); + assert_eq!(pid2.update(0.0).unwrap().output, -40.0); - pid2.i_limit_high = 50.0; - pid2.i_limit_low = -50.0; - assert_eq!(pid2.next_control_output(-5.0).unwrap().output, -50.0); + pid2.i_limit.max = Some(50.0); + pid2.i_limit.min = Some(-50.0); + assert_eq!(pid2.update(-5.0).unwrap().output, -50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid2.next_control_output(-15.0).unwrap().output, -40.0); + assert_eq!(pid2.update(-15.0).unwrap().output, -40.0); } /// Checks that a full PID controller's limits work properly through multiple output iterations @@ -441,14 +496,14 @@ mod tests { .clamp(-100.0, 100.0) .p(1.0); - pid.o_limit_high = 1.0; - pid.o_limit_low = -1.0; + pid.out_limit.max = Some(1.0); + pid.out_limit.min = Some(-1.0); - let out = pid.next_control_output(0.0).unwrap(); + let out = pid.update(0.0).unwrap(); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.output, 1.0); - let out = pid.next_control_output(20.0).unwrap(); + let out = pid.update(20.0).unwrap(); assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0) assert_eq!(out.output, -1.0); } @@ -463,25 +518,25 @@ mod tests { .i(0.1) .d(1.0); - let out = pid.next_control_output(0.0).unwrap(); + let out = pid.update(0.0).unwrap(); 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).unwrap(); + let out = pid.update(5.0).unwrap(); 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).unwrap(); + let out = pid.update(11.0).unwrap(); 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).unwrap(); + let out = pid.update(10.0).unwrap(); 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 +557,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_f32.next_control_output(0.0).unwrap().output, - pid_f64.next_control_output(0.0).unwrap().output as f32 + pid_f32.update(0.0).unwrap().output, + pid_f64.update(0.0).unwrap().output as f32 ); } } @@ -522,8 +577,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_i32.next_control_output(0).unwrap().output, - pid_i8.next_control_output(0i8).unwrap().output as i32 + pid_i32.update(0).unwrap().output, + pid_i8.update(0i8).unwrap().output as i32 ); } } @@ -538,7 +593,7 @@ mod tests { .i(0.1) .d(1.0); - let out = pid.next_control_output(0.0).unwrap(); + let out = pid.update(0.0).unwrap(); 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) @@ -547,8 +602,10 @@ mod tests { pid.setpoint(0.0); assert_eq!( - pid.next_control_output(0.0).unwrap(), + pid.update(0.0).unwrap(), ControlOutput { + input: 0.0, + error: 10.0, p: 0.0, i: 1.0, d: -0.0, @@ -567,10 +624,10 @@ mod tests { .i(1.0) .d(1.0); - pid.o_limit_high = -10.0; - pid.o_limit_low = 10.0; + pid.out_limit.max = Some(-10.0); + pid.out_limit.min = Some(10.0); - let out = pid.next_control_output(0.0).unwrap(); + let out = pid.update(0.0).unwrap(); assert_eq!(out.p, 10.0); assert_eq!(out.i, 10.0); assert_eq!(out.d, 0.0); From c4a0cb3d13c9fc6f8c99b5d1884f513142288b7c Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 21 May 2024 20:48:46 -0300 Subject: [PATCH 43/58] Update lib.rs --- src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index f24005c..b9c1b78 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -21,7 +21,7 @@ //! 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).unwrap(); +//! let output = pid.update(10.0).unwrap(); //! assert_eq!(output.output, 50.0); // <-- //! assert_eq!(output.p, 50.0); //! From c9fd304436cb1ad224f219a46723cf0abe9c6088 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 21 May 2024 21:09:13 -0300 Subject: [PATCH 44/58] Update lib.rs --- src/lib.rs | 40 ++++++++++++++++------------------------ 1 file changed, 16 insertions(+), 24 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index b9c1b78..e16dea5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -183,6 +183,7 @@ impl PidLimit where T: Number { + /// Creates a new limit struct pub const fn new() -> Self { Self { min: None, @@ -190,8 +191,17 @@ where } } + /// Sets the min and max limits + pub fn set(&mut self, min: impl Into, max: impl Into) -> &mut Self { + let min: T = min.into(); + let max: T = max.into(); + self.min = Some(min); + self.max = Some(max); + self + } + /// Clamp a given value using pre-defined limits - pub fn clamp(self, value: impl Into) -> T { + pub fn clamp(&self, value: impl Into) -> T { let mut value: T = value.into(); value = if let Some(min) = self.min { if value < min {min} else {value} @@ -252,29 +262,11 @@ where self } - /// Sets the min and max limits for this controller. - pub fn clamp(&mut self, min: impl Into, max: impl Into) -> &mut Self { - self.clamp_min(min) - .clamp_max(max) - } - - /// Sets the min limits for this controller. - pub fn clamp_min(&mut self, min: impl Into) -> &mut Self { - let min: T = min.into(); - self.p_limit.min = Some(min); - self.i_limit.min = Some(min); - self.d_limit.min = Some(min); - self.out_limit.min = Some(min); - self - } - - /// Sets the max limits for this controller. - pub fn clamp_max(&mut self, max: impl Into) -> &mut Self { - let max: T = max.into(); - self.p_limit.max = Some(max); - self.i_limit.max = Some(max); - self.d_limit.max = Some(max); - self.out_limit.max = Some(max); + /// Sets the min and max limits for the controller. + pub fn limit(&mut self, min: impl Into, max: impl Into) -> &mut Self { + self.p_limit.set(min, max); + self.i_limit.set(min, max); + self.out_limit.set(min, max); self } From b343ceacc97331cb830cf5fa649dfd87878ef092 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 21 May 2024 21:22:33 -0300 Subject: [PATCH 45/58] Update lib.rs --- src/lib.rs | 68 ++++++++++++++++++++++++++---------------------------- 1 file changed, 33 insertions(+), 35 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index e16dea5..161b685 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -10,7 +10,7 @@ //! // Create a new proportional-only PID controller with a setpoint of 15 //! let mut pid = Pid::new() //! .setpoint(15.0) -//! .clamp(-100.0, 100.0) +//! .limit(-100.0, 100.0) //! .p(10.0); //! //! // Input a measurement with an error of 5.0 from our setpoint @@ -76,7 +76,7 @@ impl Number for T {} /// // Create limited controller /// let mut p_controller = Pid::new() /// .setpoint(15.0) -/// .clamp(-100.0, 100.0) +/// .limit(-100.0, 100.0) /// .p(10.0); /// /// // Get first output @@ -91,7 +91,7 @@ impl Number for T {} /// // Create full PID controller /// let mut full_controller = Pid::new() /// .setpoint(15.0) -/// .clamp(-100.0, 100.0); +/// .limit(-100.0, 100.0); /// .p(10.0) /// .i(4.5) /// .d(0.25); @@ -144,7 +144,7 @@ pub struct Pid { /// // Setup controller /// let mut pid = Pid::new() /// .setpoint(15.0) -/// .clamp(0.0, 100.0) +/// .limit(0.0, 100.0) /// .p(10.0) /// .i(1.0) /// .d(2.0); @@ -193,10 +193,8 @@ where /// Sets the min and max limits pub fn set(&mut self, min: impl Into, max: impl Into) -> &mut Self { - let min: T = min.into(); - let max: T = max.into(); - self.min = Some(min); - self.max = Some(max); + self.min = Some(min.into()); + self.max = Some(max.into()); self } @@ -391,8 +389,10 @@ where // Convert parameters to number type let dt: T = dt.into(); // Calculate new integral term with delta time - out.i = (ki * out.error * dt) + i_term; - out.output = out.p + out.i + out.d; + let i_unbounded = (ki * out.error * dt) + i_term; + out.i = self.i_limit.clamp(i_unbounded); + let o_unbounded = out.p + out.i + out.d; + out.output = self.out_limit.clamp(o_unbounded); self.prev = Some(out); out } @@ -410,7 +410,7 @@ mod tests { fn proportional() { let mut pid = Pid::new() .setpoint(10.0) - .clamp(-100.0, 100.0) + .limit(-100.0, 100.0) .p(2.0); assert_eq!(pid.setpoint, 10.0); @@ -419,9 +419,8 @@ mod tests { assert_eq!(pid.update(0.0).unwrap().output, 20.0); // Test proportional limit - pid.p_limit.max = Some(10.0); - pid.p_limit.min = Some(-10.0); - assert_eq!(pid.update(0.0).unwrap().output, 10.0); + pid.p_limit.set(-10.0, 10.0); + assert_eq!(pid.update(0.0).unwrap().output,10.0); } /// Derivative-only controller operation and limits @@ -429,7 +428,7 @@ mod tests { fn derivative() { let mut pid = Pid::new() .setpoint(10.0) - .clamp(-100.0, 100.0) + .limit(-100.0, 100.0) .d(2.0); // Test that there's no derivative since it's the first measurement @@ -439,8 +438,7 @@ mod tests { assert_eq!(pid.update(5.0).unwrap().output, -10.0); // Test derivative limit - pid.d_limit.max = Some(5.0); - pid.d_limit.min = Some(-5.0); + pid.d_limit.set(-5.0, 5.0); assert_eq!(pid.update(10.0).unwrap().output, -5.0); } @@ -449,7 +447,7 @@ mod tests { fn integral() { let mut pid = Pid::new() .setpoint(10.0) - .clamp(-100.0, 100.0) + .limit(-100.0, 100.0) .i(2.0); // Test basic integration @@ -458,8 +456,7 @@ mod tests { assert_eq!(pid.update(5.0).unwrap().output, 50.0); // Test limit - pid.i_limit.max = Some(50.0); - pid.i_limit.min = Some(-50.0); + pid.i_limit.set(-50.0, 50.0); assert_eq!(pid.update(5.0).unwrap().output, 50.0); // Test that limit doesn't impede reversal of error integral assert_eq!(pid.update(15.0).unwrap().output, 40.0); @@ -467,14 +464,13 @@ mod tests { // Test that error integral accumulates negative values let mut pid2 = Pid::new() .setpoint(-10.0) - .clamp(-100.0, 100.0) + .limit(-100.0, 100.0) .i(2.0); assert_eq!(pid2.update(0.0).unwrap().output, -20.0); assert_eq!(pid2.update(0.0).unwrap().output, -40.0); - pid2.i_limit.max = Some(50.0); - pid2.i_limit.min = Some(-50.0); + pid2.i_limit.set(-50.0, 50.0); assert_eq!(pid2.update(-5.0).unwrap().output, -50.0); // Test that limit doesn't impede reversal of error integral assert_eq!(pid2.update(-15.0).unwrap().output, -40.0); @@ -485,11 +481,10 @@ mod tests { fn output_limit() { let mut pid = Pid::new() .setpoint(10.0) - .clamp(-100.0, 100.0) + .limit(-100.0, 100.0) .p(1.0); - pid.out_limit.max = Some(1.0); - pid.out_limit.min = Some(-1.0); + pid.out_limit.set(-1.0, 1.0); let out = pid.update(0.0).unwrap(); assert_eq!(out.p, 10.0); // 1.0 * 10.0 @@ -505,7 +500,7 @@ mod tests { fn pid() { let mut pid = Pid::new() .setpoint(10.0) - .clamp(-100.0, 100.0) + .limit(-100.0, 100.0) .p(1.0) .i(0.1) .d(1.0); @@ -541,11 +536,13 @@ mod tests { fn floats_zeros() { let mut pid_f32 = Pid::new() .setpoint(10.0f32) - .clamp(-100.0, 100.0); + .limit(-100.0, 100.0) + .p(0.0); let mut pid_f64 = Pid::new() .setpoint(10.0) - .clamp(-100.0f64, 100.0f64); + .limit(-100.0f64, 100.0f64) + .p(0.0); for _ in 0..5 { assert_eq!( @@ -561,11 +558,13 @@ mod tests { fn signed_integers_zeros() { let mut pid_i8 = Pid::new() .setpoint(10i8) - .clamp(-100, 100); + .limit(-100, 100) + .p(0.0); let mut pid_i32 = Pid::new() .setpoint(10i32) - .clamp(-100, 100); + .limit(-100, 100) + .p(0.0); for _ in 0..5 { assert_eq!( @@ -580,7 +579,7 @@ mod tests { fn setpoint() { let mut pid = Pid::new() .setpoint(10.0) - .clamp(-100.0, 100.0) + .limit(-100.0, 100.0) .p(1.0) .i(0.1) .d(1.0); @@ -611,13 +610,12 @@ mod tests { fn negative_limits() { let mut pid = Pid::new() .setpoint(10.0f32) - .clamp(50.0, -50.0) + .limit(50.0, -50.0) .p(1.0) .i(1.0) .d(1.0); - pid.out_limit.max = Some(-10.0); - pid.out_limit.min = Some(10.0); + pid.out_limit.set(10.0, -10.0); let out = pid.update(0.0).unwrap(); assert_eq!(out.p, 10.0); From b3094d028c8faf8b37323e3fc746983d88a55a13 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 21 May 2024 21:31:58 -0300 Subject: [PATCH 46/58] fix bugs --- src/lib.rs | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 161b685..6536a6e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -221,6 +221,8 @@ where /// - [Self::p()]: Proportional gain setting /// - [Self::i()]: Integral gain setting /// - [Self::d()]: Derivative gain setting + /// + /// To set output limits please use the [Self::limit()] method. pub const fn new() -> Self { Self { setpoint: None, @@ -236,19 +238,19 @@ where } } - /// Sets the [Self::p] gain for this controller. + /// Sets the [Self::kp] gain for this controller. pub fn p(&mut self, gain: impl Into) -> &mut Self { self.kp = Some(gain.into()); self } - /// Sets the [Self::i] gain for this controller. + /// Sets the [Self::ki] gain for this controller. pub fn i(&mut self, gain: impl Into) -> &mut Self { self.ki = Some(gain.into()); self } - /// Sets the [Self::d] gain for this controller. + /// Sets the [Self::kd] gain for this controller. pub fn d(&mut self, gain: impl Into) -> &mut Self { self.kd = Some(gain.into()); self @@ -261,6 +263,13 @@ where } /// Sets the min and max limits for the controller. + /// + /// This method sets the limits of proportional term, + /// integral term and total output, but does not set + /// the limits for derivative term. + /// + /// To set derivative term limits one must call + /// [`self.d_limit.set`](PidLimit::set()). pub fn limit(&mut self, min: impl Into, max: impl Into) -> &mut Self { self.p_limit.set(min, max); self.i_limit.set(min, max); @@ -346,7 +355,7 @@ where |prev| { // Mitigate derivative kick: Use the derivative of the measurement // rather than the derivative of the error. - let d_unbounded = kd * (input - prev.input); + let d_unbounded = -kd * (input - prev.input); self.d_limit.clamp(d_unbounded) } )); From 9357d6a34216738b572c8f51ac2fe1e83c3ee568 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 21 May 2024 21:34:41 -0300 Subject: [PATCH 47/58] fix error --- src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index 6536a6e..387507c 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -393,7 +393,7 @@ where // Call normal update self.update(input) .map(|out| self.ki.map_or( - Some(out), + out, |ki| { // Convert parameters to number type let dt: T = dt.into(); From 0a4d39f7bfcea9f15e1898d8320924a2a050e987 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Tue, 21 May 2024 23:11:07 -0300 Subject: [PATCH 48/58] add kd dt adjustment --- src/lib.rs | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 387507c..3d14d49 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -383,11 +383,21 @@ where /// Given a new measurement and a delta time, calculates the next [control output](ControlOutput). /// + /// Param `dt`: Delta Time is a unitary duration value used to adjust integral and derivative terms for any sampling frequency. + /// If `dt` is assumed to be in seconds, `ki` and `kd` will be represented in 1:1 ratio after 1 second of delay between samples. + /// Integral term is directly proportional to dt and derivative term is inversely proportional. This means that if dt=2 then integral + /// term will be ki*2 and derivative term will be kd/2. + /// /// # Returns None /// /// - If a setpoint has not been set via `setpoint()`. /// - If no gain is set via `p()`, `i()` or `d()`. + /// - If `dt` is zero. pub fn update_with_dt(&mut self, input: impl Into, dt: impl Into) -> Option> { + // Convert parameters to number type + let dt: T = dt.into(); + // Verify delta time value + if dt == T::zero() { return None }; // Store previous integral sum let i_term = self.i_term.map_or(T::zero(), |i| i); // Call normal update @@ -395,11 +405,13 @@ where .map(|out| self.ki.map_or( out, |ki| { - // Convert parameters to number type - let dt: T = dt.into(); // Calculate new integral term with delta time let i_unbounded = (ki * out.error * dt) + i_term; out.i = self.i_limit.clamp(i_unbounded); + // Calculate new derivative term with delta time + let d_unbounded = out.d / dt; + out.d = self.d_limit.clamp(d_unbounded); + // Sum new terms into output let o_unbounded = out.p + out.i + out.d; out.output = self.out_limit.clamp(o_unbounded); self.prev = Some(out); From 31b5ca24fa80d9bd61ebe8a0df2cd6359a5defed Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 22 May 2024 05:56:57 -0300 Subject: [PATCH 49/58] make dt guard better --- src/lib.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 3d14d49..69712b7 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -392,12 +392,12 @@ where /// /// - If a setpoint has not been set via `setpoint()`. /// - If no gain is set via `p()`, `i()` or `d()`. - /// - If `dt` is zero. + /// - If `dt` <= zero. pub fn update_with_dt(&mut self, input: impl Into, dt: impl Into) -> Option> { // Convert parameters to number type let dt: T = dt.into(); // Verify delta time value - if dt == T::zero() { return None }; + if dt <= T::zero() { return None }; // Store previous integral sum let i_term = self.i_term.map_or(T::zero(), |i| i); // Call normal update From 94b78f91bfe004fc2a953cedb3a9797d10b320e6 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 22 May 2024 06:59:17 -0300 Subject: [PATCH 50/58] Update lib.rs --- src/lib.rs | 62 ++++++++++++++++++++++++++---------------------------- 1 file changed, 30 insertions(+), 32 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 69712b7..c75da5d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -118,9 +118,7 @@ pub struct Pid { pub ki: Option, /// Derivative gain. pub kd: Option, - /// Last calculated integral value if [Pid::ki] is used. - pub i_term: Option, - /// Previously found measurement whilst using the [Pid::next_control_output] method. + /// Previous control output calculated unsing the [Pid::update()] method. pub prev: Option>, /// Limiter for the proportional term: `-p_limit <= P <= p_limit`. pub p_limit: PidLimit, @@ -279,16 +277,19 @@ where /// Set integral term to a custom value. This might be useful to restore the /// pid controller to a previous state after an interruption or crash. - pub fn set_integral_term(&mut self, i_term: impl Into) -> &mut Self { - let i_unbound: T = i_term.into(); - self.i_term = Some(self.i_limit.clamp(i_unbound)); + pub fn set_integral_term(&mut self, term: impl Into) -> &mut Self { + let i_unbound: T = term.into(); + self.prev.i = self.i_limit.clamp(i_unbound); self } /// Resets the integral term back to zero, this may drastically change the /// control output. pub fn reset_integral_term(&mut self) -> &mut Self { - self.i_term = None; + self.prev = self.prev.map(|out| { + out.i = T::zero(); + out + }); self } @@ -329,24 +330,22 @@ where ); // Calculate integral if it exists - self.i_term = self.ki.map_or( - self.i_term, + let i = self.ki.map_or( + T::zero(), |ki| { - let i_term = self.i_term.map_or(T::zero(), |i| i); + let i_prev = self.prev.map_or(T::zero(), |out| out.i); // 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. - let i_unbounded = (ki * error) + i_term; + let i_unbounded = (ki * error) + i_prev; // Mitigate integral windup: Don't want to keep building up error // beyond what i_limit will allow. Some(self.i_limit.clamp(i_unbounded)) } ); - // Get stored integral term if it exists - let i = self.i_term.map_or(T::zero(), |i| i); - + // Calculate derivative if it exists let d = self.kd.map_or( T::zero(), @@ -398,26 +397,25 @@ where let dt: T = dt.into(); // Verify delta time value if dt <= T::zero() { return None }; - // Store previous integral sum - let i_term = self.i_term.map_or(T::zero(), |i| i); + // Store previous output + let prev = self.prev.clone(); // Call normal update self.update(input) - .map(|out| self.ki.map_or( - out, - |ki| { - // Calculate new integral term with delta time - let i_unbounded = (ki * out.error * dt) + i_term; - out.i = self.i_limit.clamp(i_unbounded); - // Calculate new derivative term with delta time - let d_unbounded = out.d / dt; - out.d = self.d_limit.clamp(d_unbounded); - // Sum new terms into output - let o_unbounded = out.p + out.i + out.d; - out.output = self.out_limit.clamp(o_unbounded); - self.prev = Some(out); - out - } - )) + .map(|out| { + // Calculate new integral term with delta time + let ki = self.ki.map_or(T::zero(), |ki| ki); + let i_prev = prev.map_or(T::zero(), |out| out.i) + let i_unbounded = (ki * out.error * dt) + i_prev; + out.i = self.i_limit.clamp(i_unbounded); + // Calculate new derivative term with delta time + let d_unbounded = out.d / dt; + out.d = self.d_limit.clamp(d_unbounded); + // Sum new terms into output + let o_unbounded = out.p + out.i + out.d; + out.output = self.out_limit.clamp(o_unbounded); + self.prev = Some(out); + out + }) } } From 896898058c17750b5224ebe7f870dc0d519228bc Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 22 May 2024 08:11:18 -0300 Subject: [PATCH 51/58] fix bugs --- src/lib.rs | 30 ++++++++++++++++++++++-------- 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index c75da5d..0d6ebab 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -199,12 +199,12 @@ where /// Clamp a given value using pre-defined limits pub fn clamp(&self, value: impl Into) -> T { let mut value: T = value.into(); - value = if let Some(min) = self.min { - if value < min {min} else {value} - } else {value}; - value = if let Some(max) = self.max { - if value > max {max} else {value} - } else {value}; + value = self.min.map(|min| min.gt(&value).then_some(min)) + .flatten() + .map_or(value, |x| x); + value = self.max.map(|max| max.lt(&value).then_some(max)) + .flatten() + .map_or(value, |x| x); value } } @@ -227,7 +227,6 @@ where kp: None, ki: None, kd: None, - i_term: None, prev: None, p_limit: PidLimit::::new(), i_limit: PidLimit::::new(), @@ -279,7 +278,22 @@ where /// pid controller to a previous state after an interruption or crash. pub fn set_integral_term(&mut self, term: impl Into) -> &mut Self { let i_unbound: T = term.into(); - self.prev.i = self.i_limit.clamp(i_unbound); + let i = self.i_limit.clamp(i_unbound); + let out = self.prev.map_or( + ControlOutput { + input: T::zero(), + error: T::zero(), + p: T::zero(), + i, + d: T::zero(), + output: T::zero(), + }, + |out| { + out.i = i; + out + } + ); + self.prev = Some(out); self } From 10a186c8f41dcf7774d21e8af03736d1247d5d0a Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 22 May 2024 08:20:53 -0300 Subject: [PATCH 52/58] fix bugs --- src/lib.rs | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 0d6ebab..d95f85d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -268,6 +268,8 @@ where /// To set derivative term limits one must call /// [`self.d_limit.set`](PidLimit::set()). pub fn limit(&mut self, min: impl Into, max: impl Into) -> &mut Self { + let min: T = min.into(); + let max: T = max.into(); self.p_limit.set(min, max); self.i_limit.set(min, max); self.out_limit.set(min, max); @@ -288,7 +290,7 @@ where d: T::zero(), output: T::zero(), }, - |out| { + |mut out| { out.i = i; out } @@ -300,7 +302,7 @@ where /// Resets the integral term back to zero, this may drastically change the /// control output. pub fn reset_integral_term(&mut self) -> &mut Self { - self.prev = self.prev.map(|out| { + self.prev = self.prev.map(|mut out| { out.i = T::zero(); out }); @@ -356,7 +358,7 @@ where let i_unbounded = (ki * error) + i_prev; // Mitigate integral windup: Don't want to keep building up error // beyond what i_limit will allow. - Some(self.i_limit.clamp(i_unbounded)) + self.i_limit.clamp(i_unbounded) } ); @@ -415,10 +417,10 @@ where let prev = self.prev.clone(); // Call normal update self.update(input) - .map(|out| { + .map(|mut out| { // Calculate new integral term with delta time let ki = self.ki.map_or(T::zero(), |ki| ki); - let i_prev = prev.map_or(T::zero(), |out| out.i) + let i_prev = prev.map_or(T::zero(), |out| out.i); let i_unbounded = (ki * out.error * dt) + i_prev; out.i = self.i_limit.clamp(i_unbounded); // Calculate new derivative term with delta time From 7da4f7ffd6a31d54c566b4a247d6e0ac96940f2e Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 22 May 2024 08:22:55 -0300 Subject: [PATCH 53/58] fix bugs --- src/lib.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index d95f85d..47cb2e7 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -107,7 +107,7 @@ impl Number for T {} /// # Type Warning /// /// [Number] is abstract and can be used with anything from a [i32] to an [i128] (as well as user-defined types). Because of this, very small types might overflow during calculation in [`next_control_output`](Self::next_control_output). You probably don't want to use [i8] or user-defined types around that size so keep that in mind when designing your controller. -#[derive(Clone, Copy, Eq, PartialEq, PartialOrd)] +#[derive(Clone, Copy, Eq, PartialEq)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct Pid { /// Ideal setpoint to strive for. @@ -151,7 +151,7 @@ pub struct Pid { /// let output = pid.update(26.2456).unwrap(); /// println!("P: {}\nI: {}\nD: {}\nFinal Output: {}", output.p, output.i, output.d, output.output); /// ``` -#[derive(Debug, PartialEq, Eq, Clone, Copy)] +#[derive(Clone, Copy, Eq, PartialEq)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct ControlOutput { /// The input value to the controller. @@ -170,7 +170,7 @@ pub struct ControlOutput { pub output: T, } -#[derive(Clone, Copy, Eq, PartialEq, PartialOrd)] +#[derive(Clone, Copy, Eq, PartialEqa)] #[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] pub struct PidLimit { min: Option, From 102b5636367e4c1d0ce8ee2deeec97f1223e3a04 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Wed, 22 May 2024 08:26:26 -0300 Subject: [PATCH 54/58] fix typo --- src/lib.rs | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 47cb2e7..724630e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -130,6 +130,14 @@ pub struct Pid { pub out_limit: PidLimit, } +/// Limits of controller terms +#[derive(Clone, Copy, Eq, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] +pub struct PidLimit { + min: Option, + max: Option, +} + /// Output of [controller iterations](Pid::next_control_output) with weights /// /// # Example @@ -170,13 +178,7 @@ pub struct ControlOutput { pub output: T, } -#[derive(Clone, Copy, Eq, PartialEqa)] -#[cfg_attr(feature = "serde", derive(serde::Deserialize, serde::Serialize))] -pub struct PidLimit { - min: Option, - max: Option, -} - +// PidLimit methods impl PidLimit where T: Number @@ -209,6 +211,7 @@ where } } +// Pid methods impl Pid where T: Number From 8472585839db4232cb611a2c3ae820234a725dd4 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Sat, 25 May 2024 15:04:34 -0300 Subject: [PATCH 55/58] Improve control output limits --- src/lib.rs | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 724630e..a1b2040 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -262,20 +262,27 @@ where self } - /// Sets the min and max limits for the controller. + /// Sets the `min` and `max` limits for the controller output. /// - /// This method sets the limits of proportional term, - /// integral term and total output, but does not set - /// the limits for derivative term. + /// Calling this method sets asymmetric limits in the output, + /// that is, in the final sum of the `p`, `i` and `d` terms. /// - /// To set derivative term limits one must call - /// [`self.d_limit.set`](PidLimit::set()). + /// This method also sets symmetric limits to each individual term. + /// The symmetric limits are `-sym` and `+sym`, where `sym` is the + /// maximum between the absolute values of `min` and `max`. + /// + /// When these limits are set they will prevent integral term windup. pub fn limit(&mut self, min: impl Into, max: impl Into) -> &mut Self { let min: T = min.into(); let max: T = max.into(); - self.p_limit.set(min, max); - self.i_limit.set(min, max); + // Set asymmetric limits self.out_limit.set(min, max); + // Get maximum absolute value + let sym = min.abs().max(max.abs()); + // Set symmetric limits + self.p_limit.set(-sym, sym); + self.i_limit.set(-sym, sym); + self.d_limit.set(-sym, sym); self } @@ -304,7 +311,7 @@ where /// Resets the integral term back to zero, this may drastically change the /// control output. - pub fn reset_integral_term(&mut self) -> &mut Self { + pub fn reset(&mut self) -> &mut Self { self.prev = self.prev.map(|mut out| { out.i = T::zero(); out From 2b32fbf71113ebf13722a7158721d505901aed1d Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Sat, 25 May 2024 15:16:09 -0300 Subject: [PATCH 56/58] do not rely on `Eq` impl --- src/lib.rs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index a1b2040..9ed847b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -278,7 +278,9 @@ where // Set asymmetric limits self.out_limit.set(min, max); // Get maximum absolute value - let sym = min.abs().max(max.abs()); + let sym = if min.abs().ge(max.abs()) + {min.abs()} else + {max.abs()}; // Set symmetric limits self.p_limit.set(-sym, sym); self.i_limit.set(-sym, sym); From 29b8e52a2aa39d51c24e8a6f0c2b5545acb462a7 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 27 May 2024 15:30:31 -0300 Subject: [PATCH 57/58] fix typo --- src/lib.rs | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 9ed847b..07fc56d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -278,9 +278,8 @@ where // Set asymmetric limits self.out_limit.set(min, max); // Get maximum absolute value - let sym = if min.abs().ge(max.abs()) - {min.abs()} else - {max.abs()}; + let sym = if min.abs() > max.abs()) {min.abs()} + else {max.abs()}; // Set symmetric limits self.p_limit.set(-sym, sym); self.i_limit.set(-sym, sym); From b4433dbd3bcbe1b0b7309f9b2b936ce02eed86a4 Mon Sep 17 00:00:00 2001 From: un3481 <78743473+un3481@users.noreply.github.com> Date: Mon, 27 May 2024 15:31:38 -0300 Subject: [PATCH 58/58] fix typo --- src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index 07fc56d..3c61c99 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -278,7 +278,7 @@ where // Set asymmetric limits self.out_limit.set(min, max); // Get maximum absolute value - let sym = if min.abs() > max.abs()) {min.abs()} + let sym = if min.abs() > max.abs() {min.abs()} else {max.abs()}; // Set symmetric limits self.p_limit.set(-sym, sym);