From 753aa6c56ef1d4f93efc16ef6cae3b849734ee70 Mon Sep 17 00:00:00 2001 From: Zachary Sunforge Date: Tue, 4 Jun 2024 19:30:01 -0700 Subject: [PATCH 1/6] Dependency version bump --- Cargo.toml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 208c689..217abf3 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -29,7 +29,7 @@ license = "MIT" version = "0.2.*" # Units of measurement [workspace.dependencies.uom] -version = "0.35.*" +version = "0.36.*" default-features = false features = ["f32", "si"] # Logging @@ -50,7 +50,7 @@ version = "1.0.*" version = "1.0.*" # Memory [workspace.dependencies.static_cell] -version = "2.0.*" +version = "2.1.*" [workspace.dependencies.heapless] version = "0.8.*" # Other embedded utilities @@ -75,7 +75,7 @@ version = "0.1.*" version = "0.3.*" features = ["defmt", "defmt-timestamp-uptime"] [workspace.dependencies.embassy-sync] -version = "0.5.*" +version = "0.6.*" features = ["defmt"] [workspace.dependencies.embassy-embedded-hal] version = "0.1.*" -- 2.43.0 From e0757847cb8dfb060a61ffae2b164239c69b694d Mon Sep 17 00:00:00 2001 From: Zachary Sunforge Date: Tue, 4 Jun 2024 19:33:51 -0700 Subject: [PATCH 2/6] Removed SCALE and Heapless dependencies --- Cargo.toml | 7 ------- 1 file changed, 7 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 217abf3..c4c725b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -39,10 +39,6 @@ version = "0.1.*" version = "0.3.*" [workspace.dependencies.defmt-rtt] version = "0.4.*" -# Serialization -[workspace.dependencies.parity-scale-codec] -version = "3.6.*" -default-features = false # Embedded-HAL [workspace.dependencies.embedded-hal] version = "1.0.*" @@ -51,8 +47,6 @@ version = "1.0.*" # Memory [workspace.dependencies.static_cell] version = "2.1.*" -[workspace.dependencies.heapless] -version = "0.8.*" # Other embedded utilities [workspace.dependencies.cortex-m] version = "0.7.*" @@ -115,7 +109,6 @@ lm35 = [] [dependencies] uom = { workspace = true } -parity-scale-codec = { workspace = true } libm = { workspace = true } #--------------------------------------------------------------------------------------------------------------------- -- 2.43.0 From a3d03ce29b4d2b1cdce3e4eae16c01b978fa3df5 Mon Sep 17 00:00:00 2001 From: Zachary Sunforge Date: Tue, 4 Jun 2024 20:26:59 -0700 Subject: [PATCH 3/6] Initial setup for PID control. Copied file from https://github.com/braincore/pid-rs as it's very small and simple, and we'll want to make some modifications. --- Cargo.toml | 13 +- src/control/mod.rs | 2 + src/control/pid.rs | 457 +++++++++++++++++++++++++++++++++++++++++++++ src/lib.rs | 1 + 4 files changed, 472 insertions(+), 1 deletion(-) create mode 100644 src/control/mod.rs create mode 100644 src/control/pid.rs diff --git a/Cargo.toml b/Cargo.toml index c4c725b..27bb1f0 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -24,7 +24,10 @@ readme = "README.md" license = "MIT" #----- no-std ---------------------------------- -# Math +# Numbers +[workspace.dependencies.num-traits] +version = "0.2.*" +default-features = false [workspace.dependencies.libm] version = "0.2.*" # Units of measurement @@ -47,6 +50,11 @@ version = "1.0.*" # Memory [workspace.dependencies.static_cell] version = "2.1.*" +# Serioalization +[workspace.dependencies.serde] +version = "1.0.*" +default-features = false +features = ["derive"] # Other embedded utilities [workspace.dependencies.cortex-m] version = "0.7.*" @@ -106,10 +114,13 @@ license.workspace = true [features] thermocouple_k = [] lm35 = [] +pid = [] [dependencies] uom = { workspace = true } +num-traits = { workspace = true } libm = { workspace = true } +serde = { workspace = true, optional = true } #--------------------------------------------------------------------------------------------------------------------- #----- Profiles ------------------------ diff --git a/src/control/mod.rs b/src/control/mod.rs new file mode 100644 index 0000000..2d4adbe --- /dev/null +++ b/src/control/mod.rs @@ -0,0 +1,2 @@ +#[cfg(feature = "pid")] +pub mod pid; \ No newline at end of file diff --git a/src/control/pid.rs b/src/control/pid.rs new file mode 100644 index 0000000..ab17db2 --- /dev/null +++ b/src/control/pid.rs @@ -0,0 +1,457 @@ +//! A proportional-integral-derivative (PID) controller library. +//! +//! See [Pid] for the adjustable controller itself, as well as [ControlOutput] for the outputs and weights which you can use after setting up your controller. Follow the complete example below to setup your first controller! +//! +//! # Example +//! +//! ```rust +//! use physical::control::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); +//! +//! // Input a measurement with an error of 5.0 from our setpoint +//! let output = pid.next_control_output(10.0); +//! +//! // Show that the error is correct by multiplying by our kp +//! assert_eq!(output.output, 50.0); // <-- +//! assert_eq!(output.p, 50.0); +//! +//! // It won't change on repeat; the controller is proportional-only +//! let output = pid.next_control_output(10.0); +//! assert_eq!(output.output, 50.0); // <-- +//! assert_eq!(output.p, 50.0); +//! +//! // Add a new integral term to the controller and input again +//! pid.i(1.0, 100.0); +//! let output = pid.next_control_output(10.0); +//! +//! // Now that the integral makes the controller stateful, it will change +//! assert_eq!(output.output, 55.0); // <-- +//! assert_eq!(output.p, 50.0); +//! assert_eq!(output.i, 5.0); +//! +//! // Add our final derivative term and match our setpoint target +//! pid.d(2.0, 100.0); +//! let output = pid.next_control_output(15.0); +//! +//! // The output will now say to go down due to the derivative +//! assert_eq!(output.output, -5.0); // <-- +//! assert_eq!(output.p, 0.0); +//! assert_eq!(output.i, 5.0); +//! assert_eq!(output.d, -10.0); +//! + +#[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 {} + +// Implement `Number` for all types that +// satisfy `PartialOrd + num_traits::Signed + Copy`. +impl Number for T {} + +/// Adjustable proportional-integral-derivative (PID) controller. +/// +/// # Examples +/// +/// This controller provides a builder pattern interface which allows you to pick-and-choose which PID inputs you'd like to use during operation. Here's what a basic proportional-only controller could look like: +/// +/// ```rust +/// use physical::control::pid::Pid; +/// +/// // Create limited controller +/// let mut p_controller = Pid::new(15.0, 100.0); +/// p_controller.p(10.0, 100.0); +/// +/// // Get first output +/// let p_output = p_controller.next_control_output(400.0); +/// ``` +/// +/// This controller would give you set a proportional controller to `10.0` with a target of `15.0` and an output limit of `100.0` per [output](Self::next_control_output) iteration. The same controller with a full PID system built in looks like: +/// +/// ```rust +/// use physical::control::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); +/// +/// // Get first output +/// let full_output = full_controller.next_control_output(400.0); +/// ``` +/// +/// This [`next_control_output`](Self::next_control_output) method is what's used to input new values into the controller to tell it what the current state of the system is. In the examples above it's only being used once, but realistically this will be a hot method. Please see [ControlOutput] for examples of how to handle these outputs; it's quite straight forward and mirrors the values of this structure in some ways. +/// +/// 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. +/// +/// # 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)] +#[cfg_attr(feature = "serde", derive(Deserialize, 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. + pub ki: T, + /// 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, + /// Last calculated integral value if [Pid::ki] is used. + integral_term: T, + /// Previously found measurement whilst using the [Pid::next_control_output] method. + prev_measurement: Option, +} + +/// Output of [controller iterations](Pid::next_control_output) with weights +/// +/// # Example +/// +/// This structure is simple to use and features three weights: [p](Self::p), [i](Self::i), and [d](Self::d). These can be used to figure out how much each term from [Pid] contributed to the final [output](Self::output) value which should be taken as the final controller output for this iteration: +/// +/// ```rust +/// use physical::control::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); +/// +/// // 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 { + /// Contribution of the P term to the output. + pub p: T, + /// Contribution of the I term to the output. + /// + /// This integral term is equal to `sum[error(t) * ki(t)] (for all t)` + pub i: T, + /// Contribution of the D term to the output. + pub d: T, + /// Output of the PID controller. + pub output: T, +} + +impl Pid + where + T: Number, +{ + /// 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 { + 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(), + prev_measurement: None, + } + } + + /// Sets the [Self::p] term for this controller. + pub fn p(&mut self, gain: impl Into, limit: 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 { + 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 { + self.kd = gain.into(); + self.d_limit = limit.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 + } + + /// Given a new measurement, calculates the next [control output](ControlOutput). + /// + /// # Panics + /// + /// - If a setpoint has not been set via `update_setpoint()`. + pub fn next_control_output(&mut self, measurement: T) -> ControlOutput { + // Calculate the error between the ideal setpoint and the current + // measurement to compare against + let error = self.setpoint - measurement; + + // Calculate the proportional term and limit to it's individual limit + let p_unbounded = error * self.kp; + let p = apply_limit(self.p_limit, p_unbounded); + + // Mitigate output jumps when ki(t) != ki(t-1). + // While it's standard to use an error_integral that's a running sum of + // just the error (no ki), because we support ki changing dynamically, + // we store the entire term so that we don't need to remember previous + // ki values. + self.integral_term = self.integral_term + error * self.ki; + + // Mitigate integral windup: Don't want to keep building up error + // beyond what i_limit will allow. + self.integral_term = apply_limit(self.i_limit, self.integral_term); + + // Mitigate derivative kick: Use the derivative of the measurement + // rather than the derivative of the error. + let d_unbounded = -match self.prev_measurement.as_ref() { + Some(prev_measurement) => measurement - *prev_measurement, + None => T::zero(), + } * self.kd; + self.prev_measurement = Some(measurement); + let d = apply_limit(self.d_limit, d_unbounded); + + // Calculate the final output by adding together the PID terms, then + // apply the final defined output limit + let output = p + self.integral_term + d; + let output = apply_limit(self.output_limit, output); + + // Return the individual term's contributions and the final output + ControlOutput { + p, + i: self.integral_term, + d, + output: 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(); + } +} + +/// 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)] +mod tests { + use super::Pid; + use super::ControlOutput; + + /// 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); + assert_eq!(pid.setpoint, 10.0); + + // Test simple proportional + assert_eq!(pid.next_control_output(0.0).output, 20.0); + + // Test proportional limit + pid.p_limit = 10.0; + assert_eq!(pid.next_control_output(0.0).output, 10.0); + } + + /// 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); + + // Test that there's no derivative since it's the first measurement + assert_eq!(pid.next_control_output(0.0).output, 0.0); + + // Test that there's now a derivative + assert_eq!(pid.next_control_output(5.0).output, -10.0); + + // Test derivative limit + pid.d_limit = 5.0; + assert_eq!(pid.next_control_output(10.0).output, -5.0); + } + + /// 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); + + // 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); + + // Test limit + pid.i_limit = 50.0; + assert_eq!(pid.next_control_output(5.0).output, 50.0); + // Test that limit doesn't impede reversal of error integral + assert_eq!(pid.next_control_output(15.0).output, 40.0); + + // Test that error integral accumulates negative values + let mut pid2 = Pid::new(-10.0, 100.0); + pid2.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); + assert_eq!(pid2.next_control_output(0.0).output, -20.0); + assert_eq!(pid2.next_control_output(0.0).output, -40.0); + + pid2.i_limit = 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); + } + + /// 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 out = pid.next_control_output(0.0); + assert_eq!(out.p, 10.0); // 1.0 * 10.0 + assert_eq!(out.output, 1.0); + + let out = pid.next_control_output(20.0); + assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0) + assert_eq!(out.output, -1.0); + } + + /// 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 out = pid.next_control_output(0.0); + assert_eq!(out.p, 10.0); // 1.0 * 10.0 + assert_eq!(out.i, 1.0); // 0.1 * 10.0 + assert_eq!(out.d, 0.0); // -(1.0 * 0.0) + assert_eq!(out.output, 11.0); + + let out = pid.next_control_output(5.0); + 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); + 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); + 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) + assert_eq!(out.output, 2.4); + } + + // NOTE: use for new test in future: /// Full PID operation with mixed float checking to make sure they're equal + /// 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_f64 = Pid::new(10.0, 100.0f64); + pid_f64.p(0.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); + + for _ in 0..5 { + assert_eq!( + pid_f32.next_control_output(0.0).output, + pid_f64.next_control_output(0.0).output as f32 + ); + } + } + + // NOTE: use for new test in future: /// Full PID operation with mixed signed integer checking to make sure they're equal + /// 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_i32 = Pid::new(10i32, 100); + pid_i32.p(0, 100).i(0, 100).d(0, 100); + + for _ in 0..5 { + assert_eq!( + pid_i32.next_control_output(0).output, + pid_i8.next_control_output(0i8).output as i32 + ); + } + } + + /// 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 out = pid.next_control_output(0.0); + assert_eq!(out.p, 10.0); // 1.0 * 10.0 + assert_eq!(out.i, 1.0); // 0.1 * 10.0 + assert_eq!(out.d, 0.0); // -(1.0 * 0.0) + assert_eq!(out.output, 11.0); + + pid.setpoint(0.0); + + assert_eq!( + pid.next_control_output(0.0), + ControlOutput { + p: 0.0, + i: 1.0, + d: -0.0, + output: 1.0 + } + ); + } + + /// 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 out = pid.next_control_output(0.0); + assert_eq!(out.p, 10.0); + assert_eq!(out.i, 10.0); + assert_eq!(out.d, 0.0); + assert_eq!(out.output, 10.0); + } +} \ No newline at end of file diff --git a/src/lib.rs b/src/lib.rs index 112b8d4..669cbe5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,6 +1,7 @@ #![no_std] pub mod transducer; +pub mod control; pub mod cell; mod error; -- 2.43.0 From f1f5947f221bb2d31620d8a558f6d0df2560ee21 Mon Sep 17 00:00:00 2001 From: Zachary Sunforge Date: Tue, 4 Jun 2024 21:07:09 -0700 Subject: [PATCH 4/6] Removed implicit type conversions. Renamed some variables. --- src/control/pid.rs | 54 +++++++++++++++++++++------------------------- 1 file changed, 25 insertions(+), 29 deletions(-) diff --git a/src/control/pid.rs b/src/control/pid.rs index ab17db2..ee80f24 100644 --- a/src/control/pid.rs +++ b/src/control/pid.rs @@ -109,18 +109,18 @@ pub struct Pid { /// Defines the overall output filter limit. pub output_limit: T, /// Proportional gain. - pub kp: T, + pub p_gain: T, /// Integral gain. - pub ki: T, + pub i_gain: T, /// Derivative gain. - pub kd: T, + pub d_gain: 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, - /// Last calculated integral value if [Pid::ki] is used. + /// Last calculated integral value if [Pid::i_gain] is used. integral_term: T, /// Previously found measurement whilst using the [Pid::next_control_output] method. prev_measurement: Option, @@ -167,13 +167,13 @@ impl Pid /// - [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 { + pub fn new(setpoint: T, output_limit: T) -> Self { Self { - setpoint: setpoint.into(), - output_limit: output_limit.into(), - kp: T::zero(), - ki: T::zero(), - kd: T::zero(), + setpoint, + output_limit, + p_gain: T::zero(), + i_gain: T::zero(), + d_gain: T::zero(), p_limit: T::zero(), i_limit: T::zero(), d_limit: T::zero(), @@ -183,44 +183,40 @@ impl Pid } /// Sets the [Self::p] term for this controller. - pub fn p(&mut self, gain: impl Into, limit: impl Into) -> &mut Self { - self.kp = gain.into(); - self.p_limit = limit.into(); + pub fn p(&mut self, gain: T, limit: T) -> &mut Self { + self.p_gain = gain; + self.p_limit = limit; self } /// Sets the [Self::i] term for this controller. - pub fn i(&mut self, gain: impl Into, limit: impl Into) -> &mut Self { - self.ki = gain.into(); - self.i_limit = limit.into(); + pub fn i(&mut self, gain: T, limit: T) -> &mut Self { + self.i_gain = gain; + self.i_limit = limit; self } /// Sets the [Self::d] term for this controller. - pub fn d(&mut self, gain: impl Into, limit: impl Into) -> &mut Self { - self.kd = gain.into(); - self.d_limit = limit.into(); + pub fn d(&mut self, gain: T, limit: T) -> &mut Self { + self.d_gain = gain; + self.d_limit = limit; self } /// Sets the [Pid::setpoint] to target for this controller. - pub fn setpoint(&mut self, setpoint: impl Into) -> &mut Self { - self.setpoint = setpoint.into(); + pub fn setpoint(&mut self, setpoint: T) -> &mut Self { + self.setpoint = setpoint; self } /// Given a new measurement, calculates the next [control output](ControlOutput). - /// - /// # Panics - /// - /// - If a setpoint has not been set via `update_setpoint()`. pub fn next_control_output(&mut self, measurement: T) -> ControlOutput { // Calculate the error between the ideal setpoint and the current // measurement to compare against let error = self.setpoint - measurement; // Calculate the proportional term and limit to it's individual limit - let p_unbounded = error * self.kp; + let p_unbounded = error * self.p_gain; let p = apply_limit(self.p_limit, p_unbounded); // Mitigate output jumps when ki(t) != ki(t-1). @@ -228,7 +224,7 @@ impl Pid // just the error (no ki), because we support ki changing dynamically, // we store the entire term so that we don't need to remember previous // ki values. - self.integral_term = self.integral_term + error * self.ki; + self.integral_term = self.integral_term + error * self.i_gain; // Mitigate integral windup: Don't want to keep building up error // beyond what i_limit will allow. @@ -239,7 +235,7 @@ impl Pid let d_unbounded = -match self.prev_measurement.as_ref() { Some(prev_measurement) => measurement - *prev_measurement, None => T::zero(), - } * self.kd; + } * self.d_gain; self.prev_measurement = Some(measurement); let d = apply_limit(self.d_limit, d_unbounded); @@ -253,7 +249,7 @@ impl Pid p, i: self.integral_term, d, - output: output, + output, } } -- 2.43.0 From 46abb2150c48032442265e743fe39a9ef2146d5e Mon Sep 17 00:00:00 2001 From: Zachary Sunforge Date: Tue, 4 Jun 2024 22:29:45 -0700 Subject: [PATCH 5/6] Removed ControlOutput type. --- src/control/pid.rs | 163 ++++++++++++--------------------------------- 1 file changed, 43 insertions(+), 120 deletions(-) diff --git a/src/control/pid.rs b/src/control/pid.rs index ee80f24..ac2bf9d 100644 --- a/src/control/pid.rs +++ b/src/control/pid.rs @@ -15,33 +15,25 @@ //! let output = pid.next_control_output(10.0); //! //! // Show that the error is correct by multiplying by our kp -//! assert_eq!(output.output, 50.0); // <-- -//! assert_eq!(output.p, 50.0); +//! assert_eq!(output, 50.0); // <-- //! //! // It won't change on repeat; the controller is proportional-only //! let output = pid.next_control_output(10.0); -//! assert_eq!(output.output, 50.0); // <-- -//! assert_eq!(output.p, 50.0); +//! assert_eq!(output, 50.0); // <-- //! //! // Add a new integral term to the controller and input again //! pid.i(1.0, 100.0); //! let output = pid.next_control_output(10.0); //! //! // Now that the integral makes the controller stateful, it will change -//! assert_eq!(output.output, 55.0); // <-- -//! assert_eq!(output.p, 50.0); -//! assert_eq!(output.i, 5.0); +//! assert_eq!(output, 55.0); // <-- //! //! // Add our final derivative term and match our setpoint target //! pid.d(2.0, 100.0); //! let output = pid.next_control_output(15.0); //! //! // The output will now say to go down due to the derivative -//! assert_eq!(output.output, -5.0); // <-- -//! assert_eq!(output.p, 0.0); -//! assert_eq!(output.i, 5.0); -//! assert_eq!(output.d, -10.0); -//! +//! assert_eq!(output, -5.0); // <--//! #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; @@ -108,11 +100,14 @@ pub struct Pid { pub setpoint: T, /// Defines the overall output filter limit. pub output_limit: T, - /// Proportional gain. + /// Proportional gain. The proportional component is dependant only on the error. + /// It is the error * this value. pub p_gain: T, - /// Integral gain. + /// Integral gain. The integral component is dependent on the error and the integral term from the previous iteration. + /// It is the previous integral + (error * this value). pub i_gain: T, - /// Derivative gain. + /// Derivative gain. The derivative component is dependent on the rate of change of the measurement. + /// It is the (current measurement - previous measurement) * this value. pub d_gain: T, /// Limiter for the proportional term: `-p_limit <= P <= p_limit`. pub p_limit: T, @@ -126,40 +121,9 @@ pub struct Pid { prev_measurement: Option, } -/// Output of [controller iterations](Pid::next_control_output) with weights -/// -/// # Example -/// -/// This structure is simple to use and features three weights: [p](Self::p), [i](Self::i), and [d](Self::d). These can be used to figure out how much each term from [Pid] contributed to the final [output](Self::output) value which should be taken as the final controller output for this iteration: -/// -/// ```rust -/// use physical::control::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); -/// -/// // 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 { - /// Contribution of the P term to the output. - pub p: T, - /// Contribution of the I term to the output. - /// - /// This integral term is equal to `sum[error(t) * ki(t)] (for all t)` - pub i: T, - /// Contribution of the D term to the output. - pub d: T, - /// Output of the PID controller. - pub output: T, -} - impl Pid - where - T: Number, +where + T: Number, { /// Creates a new controller with the target setpoint and the output limit /// @@ -209,8 +173,8 @@ impl Pid self } - /// Given a new measurement, calculates the next [control output](ControlOutput). - pub fn next_control_output(&mut self, measurement: T) -> ControlOutput { + /// Given a new measurement, calculates the next control setting. + pub fn next_control_output(&mut self, measurement: T) -> T { // Calculate the error between the ideal setpoint and the current // measurement to compare against let error = self.setpoint - measurement; @@ -232,8 +196,8 @@ impl Pid // 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, + let d_unbounded = -match self.prev_measurement { + Some(prev_measurement) => measurement - prev_measurement, None => T::zero(), } * self.d_gain; self.prev_measurement = Some(measurement); @@ -244,13 +208,7 @@ impl Pid let output = p + self.integral_term + d; let output = apply_limit(self.output_limit, output); - // Return the individual term's contributions and the final output - ControlOutput { - p, - i: self.integral_term, - d, - output, - } + output } /// Resets the integral term back to zero, this may drastically change the @@ -268,7 +226,6 @@ fn apply_limit(limit: T, value: T) -> T { #[cfg(test)] mod tests { use super::Pid; - use super::ControlOutput; /// Proportional-only controller operation and limits #[test] @@ -278,11 +235,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), 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), 10.0); } /// Derivative-only controller operation and limits @@ -292,14 +249,14 @@ mod tests { pid.p(0.0, 100.0).i(0.0, 100.0).d(2.0, 100.0); // Test that there's no derivative since it's the first measurement - assert_eq!(pid.next_control_output(0.0).output, 0.0); + assert_eq!(pid.next_control_output(0.0), 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), -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), -5.0); } /// Integral-only controller operation and limits @@ -309,26 +266,26 @@ mod tests { pid.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); // Test basic integration - assert_eq!(pid.next_control_output(0.0).output, 20.0); - assert_eq!(pid.next_control_output(0.0).output, 40.0); - assert_eq!(pid.next_control_output(5.0).output, 50.0); + assert_eq!(pid.next_control_output(0.0), 20.0); + assert_eq!(pid.next_control_output(0.0), 40.0); + assert_eq!(pid.next_control_output(5.0), 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), 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), 40.0); // Test that error integral accumulates negative values let mut pid2 = Pid::new(-10.0, 100.0); pid2.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); - assert_eq!(pid2.next_control_output(0.0).output, -20.0); - assert_eq!(pid2.next_control_output(0.0).output, -40.0); + assert_eq!(pid2.next_control_output(0.0), -20.0); + assert_eq!(pid2.next_control_output(0.0), -40.0); pid2.i_limit = 50.0; - assert_eq!(pid2.next_control_output(-5.0).output, -50.0); + assert_eq!(pid2.next_control_output(-5.0), -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), -40.0); } /// Checks that a full PID controller's limits work properly through multiple output iterations @@ -338,12 +295,10 @@ mod tests { pid.p(1.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); let out = pid.next_control_output(0.0); - assert_eq!(out.p, 10.0); // 1.0 * 10.0 - assert_eq!(out.output, 1.0); + assert_eq!(out, 1.0); let out = pid.next_control_output(20.0); - assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0) - assert_eq!(out.output, -1.0); + assert_eq!(out, -1.0); } /// Combined PID operation @@ -353,28 +308,16 @@ mod tests { pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); let out = pid.next_control_output(0.0); - assert_eq!(out.p, 10.0); // 1.0 * 10.0 - assert_eq!(out.i, 1.0); // 0.1 * 10.0 - assert_eq!(out.d, 0.0); // -(1.0 * 0.0) - assert_eq!(out.output, 11.0); + assert_eq!(out, 11.0); let out = pid.next_control_output(5.0); - assert_eq!(out.p, 5.0); // 1.0 * 5.0 - assert_eq!(out.i, 1.5); // 0.1 * (10.0 + 5.0) - assert_eq!(out.d, -5.0); // -(1.0 * 5.0) - assert_eq!(out.output, 1.5); + assert_eq!(out, 1.5); let out = pid.next_control_output(11.0); - assert_eq!(out.p, -1.0); // 1.0 * -1.0 - assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1) - assert_eq!(out.d, -6.0); // -(1.0 * 6.0) - assert_eq!(out.output, -5.6); + assert_eq!(out, -5.6); let out = pid.next_control_output(10.0); - assert_eq!(out.p, 0.0); // 1.0 * 0.0 - assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1.0 + 0.0) - assert_eq!(out.d, 1.0); // -(1.0 * -1.0) - assert_eq!(out.output, 2.4); + assert_eq!(out, 2.4); } // NOTE: use for new test in future: /// Full PID operation with mixed float checking to make sure they're equal @@ -388,10 +331,7 @@ mod tests { pid_f64.p(0.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); for _ in 0..5 { - assert_eq!( - pid_f32.next_control_output(0.0).output, - pid_f64.next_control_output(0.0).output as f32 - ); + assert_eq!(pid_f32.next_control_output(0.0), pid_f64.next_control_output(0.0) as f32); } } @@ -406,10 +346,7 @@ mod tests { pid_i32.p(0, 100).i(0, 100).d(0, 100); for _ in 0..5 { - assert_eq!( - pid_i32.next_control_output(0).output, - pid_i8.next_control_output(0i8).output as i32 - ); + assert_eq!(pid_i32.next_control_output(0), pid_i8.next_control_output(0i8) as i32); } } @@ -420,22 +357,11 @@ mod tests { pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); let out = pid.next_control_output(0.0); - assert_eq!(out.p, 10.0); // 1.0 * 10.0 - assert_eq!(out.i, 1.0); // 0.1 * 10.0 - assert_eq!(out.d, 0.0); // -(1.0 * 0.0) - assert_eq!(out.output, 11.0); + assert_eq!(out, 11.0); pid.setpoint(0.0); - assert_eq!( - pid.next_control_output(0.0), - ControlOutput { - p: 0.0, - i: 1.0, - d: -0.0, - output: 1.0 - } - ); + assert_eq!(pid.next_control_output(0.0), 1.0); } /// Make sure negative limits don't break the controller @@ -445,9 +371,6 @@ mod tests { pid.p(1.0, -50.0).i(1.0, -50.0).d(1.0, -50.0); let out = pid.next_control_output(0.0); - assert_eq!(out.p, 10.0); - assert_eq!(out.i, 10.0); - assert_eq!(out.d, 0.0); - assert_eq!(out.output, 10.0); + assert_eq!(out, 10.0); } -} \ No newline at end of file +} -- 2.43.0 From d37d99537ef897c764dcaff46c0bbd63249fb3c0 Mon Sep 17 00:00:00 2001 From: Zachary Sunforge Date: Tue, 4 Jun 2024 23:05:51 -0700 Subject: [PATCH 6/6] Removed builder methods, easier to just set fields directly. --- src/control/pid.rs | 175 +++++++++++++++++++++------------------------ 1 file changed, 81 insertions(+), 94 deletions(-) diff --git a/src/control/pid.rs b/src/control/pid.rs index ac2bf9d..7988e58 100644 --- a/src/control/pid.rs +++ b/src/control/pid.rs @@ -9,7 +9,8 @@ //! //! // 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); +//! pid.proportional_gain = 10.0; +//! pid.proportional_limit = 100.0; //! //! // Input a measurement with an error of 5.0 from our setpoint //! let output = pid.next_control_output(10.0); @@ -22,14 +23,16 @@ //! assert_eq!(output, 50.0); // <-- //! //! // Add a new integral term to the controller and input again -//! pid.i(1.0, 100.0); +//! pid.integral_gain = 1.0; +//! pid.integral_limit = 100.0; //! let output = pid.next_control_output(10.0); //! //! // Now that the integral makes the controller stateful, it will change //! assert_eq!(output, 55.0); // <-- //! //! // Add our final derivative term and match our setpoint target -//! pid.d(2.0, 100.0); +//! pid.derivative_gain = 2.0; +//! pid.derivative_limit = 100.0; //! let output = pid.next_control_output(15.0); //! //! // The output will now say to go down due to the derivative @@ -58,40 +61,11 @@ impl Number for T {} /// Adjustable proportional-integral-derivative (PID) controller. /// -/// # Examples -/// -/// This controller provides a builder pattern interface which allows you to pick-and-choose which PID inputs you'd like to use during operation. Here's what a basic proportional-only controller could look like: -/// -/// ```rust -/// use physical::control::pid::Pid; -/// -/// // Create limited controller -/// let mut p_controller = Pid::new(15.0, 100.0); -/// p_controller.p(10.0, 100.0); -/// -/// // Get first output -/// let p_output = p_controller.next_control_output(400.0); -/// ``` -/// -/// This controller would give you set a proportional controller to `10.0` with a target of `15.0` and an output limit of `100.0` per [output](Self::next_control_output) iteration. The same controller with a full PID system built in looks like: -/// -/// ```rust -/// use physical::control::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); -/// -/// // Get first output -/// let full_output = full_controller.next_control_output(400.0); -/// ``` -/// /// This [`next_control_output`](Self::next_control_output) method is what's used to input new values into the controller to tell it what the current state of the system is. In the examples above it's only being used once, but realistically this will be a hot method. Please see [ControlOutput] for examples of how to handle these outputs; it's quite straight forward and mirrors the values of this structure in some ways. /// -/// 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. +/// The last item of note is that the gain and limit fields can be safely modified during use. /// /// # 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)] #[cfg_attr(feature = "serde", derive(Deserialize, Serialize))] @@ -102,19 +76,19 @@ pub struct Pid { pub output_limit: T, /// Proportional gain. The proportional component is dependant only on the error. /// It is the error * this value. - pub p_gain: T, + pub proportional_gain: T, /// Integral gain. The integral component is dependent on the error and the integral term from the previous iteration. - /// It is the previous integral + (error * this value). - pub i_gain: T, + /// It is the previous integral + (error * this value). + pub integral_gain: T, /// Derivative gain. The derivative component is dependent on the rate of change of the measurement. /// It is the (current measurement - previous measurement) * this value. - pub d_gain: T, + pub derivative_gain: T, /// Limiter for the proportional term: `-p_limit <= P <= p_limit`. - pub p_limit: T, + pub proportional_limit: T, /// Limiter for the integral term: `-i_limit <= I <= i_limit`. - pub i_limit: T, + pub integral_limit: T, /// Limiter for the derivative term: `-d_limit <= D <= d_limit`. - pub d_limit: T, + pub derivative_limit: T, /// Last calculated integral value if [Pid::i_gain] is used. integral_term: T, /// Previously found measurement whilst using the [Pid::next_control_output] method. @@ -126,47 +100,21 @@ where T: Number, { /// 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: T, output_limit: T) -> Self { Self { setpoint, output_limit, - p_gain: T::zero(), - i_gain: T::zero(), - d_gain: T::zero(), - p_limit: T::zero(), - i_limit: T::zero(), - d_limit: T::zero(), + proportional_gain: T::zero(), + integral_gain: T::zero(), + derivative_gain: T::zero(), + proportional_limit: T::zero(), + integral_limit: T::zero(), + derivative_limit: T::zero(), integral_term: T::zero(), prev_measurement: None, } } - /// Sets the [Self::p] term for this controller. - pub fn p(&mut self, gain: T, limit: T) -> &mut Self { - self.p_gain = gain; - self.p_limit = limit; - self - } - - /// Sets the [Self::i] term for this controller. - pub fn i(&mut self, gain: T, limit: T) -> &mut Self { - self.i_gain = gain; - self.i_limit = limit; - self - } - - /// Sets the [Self::d] term for this controller. - pub fn d(&mut self, gain: T, limit: T) -> &mut Self { - self.d_gain = gain; - self.d_limit = limit; - self - } - /// Sets the [Pid::setpoint] to target for this controller. pub fn setpoint(&mut self, setpoint: T) -> &mut Self { self.setpoint = setpoint; @@ -180,28 +128,28 @@ where let error = self.setpoint - measurement; // Calculate the proportional term and limit to it's individual limit - let p_unbounded = error * self.p_gain; - let p = apply_limit(self.p_limit, p_unbounded); + let p_unbounded = error * self.proportional_gain; + let p = apply_limit(self.proportional_limit, p_unbounded); // Mitigate output jumps when ki(t) != ki(t-1). // While it's standard to use an error_integral that's a running sum of // just the error (no ki), because we support ki changing dynamically, // we store the entire term so that we don't need to remember previous // ki values. - self.integral_term = self.integral_term + error * self.i_gain; + self.integral_term = self.integral_term + error * self.integral_gain; // 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.integral_term = apply_limit(self.integral_limit, self.integral_term); // Mitigate derivative kick: Use the derivative of the measurement // rather than the derivative of the error. let d_unbounded = -match self.prev_measurement { Some(prev_measurement) => measurement - prev_measurement, None => T::zero(), - } * self.d_gain; + } * self.derivative_gain; self.prev_measurement = Some(measurement); - let d = apply_limit(self.d_limit, d_unbounded); + let d = apply_limit(self.derivative_limit, d_unbounded); // Calculate the final output by adding together the PID terms, then // apply the final defined output limit @@ -231,14 +179,17 @@ mod tests { #[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); + pid.proportional_gain = 2.0; + pid.proportional_limit = 100.0; + pid.integral_limit = 100.0; + pid.derivative_limit = 100.0; assert_eq!(pid.setpoint, 10.0); // Test simple proportional assert_eq!(pid.next_control_output(0.0), 20.0); // Test proportional limit - pid.p_limit = 10.0; + pid.proportional_limit = 10.0; assert_eq!(pid.next_control_output(0.0), 10.0); } @@ -246,7 +197,10 @@ mod tests { #[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); + pid.proportional_limit = 100.0; + pid.integral_limit = 100.0; + pid.derivative_limit = 100.0; + pid.derivative_gain = 2.0; // Test that there's no derivative since it's the first measurement assert_eq!(pid.next_control_output(0.0), 0.0); @@ -255,7 +209,7 @@ mod tests { assert_eq!(pid.next_control_output(5.0), -10.0); // Test derivative limit - pid.d_limit = 5.0; + pid.derivative_limit = 5.0; assert_eq!(pid.next_control_output(10.0), -5.0); } @@ -263,7 +217,10 @@ mod tests { #[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); + pid.proportional_limit = 0.0; + pid.integral_gain = 2.0; + pid.integral_limit = 100.0; + pid.derivative_limit = 100.0; // Test basic integration assert_eq!(pid.next_control_output(0.0), 20.0); @@ -271,18 +228,22 @@ mod tests { assert_eq!(pid.next_control_output(5.0), 50.0); // Test limit - pid.i_limit = 50.0; + pid.integral_limit = 50.0; assert_eq!(pid.next_control_output(5.0), 50.0); // Test that limit doesn't impede reversal of error integral assert_eq!(pid.next_control_output(15.0), 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); + pid2.proportional_limit = 100.0; + pid2.integral_gain = 2.0; + pid2.integral_limit = 100.0; + pid2.derivative_limit = 100.0; + assert_eq!(pid2.next_control_output(0.0), -20.0); assert_eq!(pid2.next_control_output(0.0), -40.0); - pid2.i_limit = 50.0; + pid2.integral_limit = 50.0; assert_eq!(pid2.next_control_output(-5.0), -50.0); // Test that limit doesn't impede reversal of error integral assert_eq!(pid2.next_control_output(-15.0), -40.0); @@ -292,7 +253,10 @@ mod tests { #[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); + pid.proportional_gain = 2.0; + pid.proportional_limit = 100.0; + pid.integral_limit = 100.0; + pid.derivative_limit = 100.0; let out = pid.next_control_output(0.0); assert_eq!(out, 1.0); @@ -305,7 +269,12 @@ mod tests { #[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); + pid.proportional_gain = 1.0; + pid.proportional_limit = 100.0; + pid.integral_gain = 0.1; + pid.integral_limit = 100.0; + pid.derivative_gain = 1.0; + pid.derivative_limit = 100.0; let out = pid.next_control_output(0.0); assert_eq!(out, 11.0); @@ -325,10 +294,14 @@ mod tests { #[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); + pid_f32.proportional_limit = 100.0; + pid_f32.integral_limit = 100.0; + pid_f32.derivative_limit = 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); + pid_f64.proportional_limit = 100.0; + pid_f64.integral_limit = 100.0; + pid_f64.derivative_limit = 100.0; for _ in 0..5 { assert_eq!(pid_f32.next_control_output(0.0), pid_f64.next_control_output(0.0) as f32); @@ -340,10 +313,14 @@ mod tests { #[test] fn signed_integers_zeros() { let mut pid_i8 = Pid::new(10i8, 100); - pid_i8.p(0, 100).i(0, 100).d(0, 100); + pid_i8.proportional_limit = 100; + pid_i8.integral_limit = 100; + pid_i8.derivative_limit = 100; let mut pid_i32 = Pid::new(10i32, 100); - pid_i32.p(0, 100).i(0, 100).d(0, 100); + pid_i32.proportional_limit = 100; + pid_i32.integral_limit = 100; + pid_i32.derivative_limit = 100; for _ in 0..5 { assert_eq!(pid_i32.next_control_output(0), pid_i8.next_control_output(0i8) as i32); @@ -354,7 +331,12 @@ mod tests { #[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); + pid.proportional_gain = 1.0; + pid.proportional_limit = 100.0; + pid.integral_gain = 0.1; + pid.integral_limit = 100.0; + pid.derivative_gain = 1.0; + pid.derivative_limit = 100.0; let out = pid.next_control_output(0.0); assert_eq!(out, 11.0); @@ -368,7 +350,12 @@ mod tests { #[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); + pid.proportional_gain = 1.0; + pid.proportional_limit = -50.0; + pid.integral_gain = 1.0; + pid.integral_limit = -50.0; + pid.derivative_gain = 1.0; + pid.derivative_limit = -50.0; let out = pid.next_control_output(0.0); assert_eq!(out, 10.0); -- 2.43.0