From e9ab0c46827f84b07b319932189e12429ec92be5 Mon Sep 17 00:00:00 2001 From: Ruben Nitsche Date: Thu, 26 Oct 2023 12:24:36 +0200 Subject: [PATCH 1/4] Added dt handover --- src/lib.rs | 62 +++++++++++++++++++++++++++--------------------------- 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 54583ae..3573b32 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -210,12 +210,12 @@ where self } - /// Given a new measurement, calculates the next [control output](ControlOutput). + /// Given a new measurement and dt, calculates the next [control output](ControlOutput). /// /// # Panics /// /// - If a setpoint has not been set via `update_setpoint()`. - pub fn next_control_output(&mut self, measurement: T) -> ControlOutput { + pub fn next_control_output(&mut self, measurement: T, dt: T) -> ControlOutput { // Calculate the error between the ideal setpoint and the current // measurement to compare against let error = self.setpoint - measurement; @@ -229,7 +229,7 @@ where // just the error (no ki), because we support ki changing dynamically, // we store the entire term so that we don't need to remember previous // ki values. - self.integral_term = self.integral_term + error * self.ki; + self.integral_term = self.integral_term + error * self.ki * dt; // Mitigate integral windup: Don't want to keep building up error // beyond what i_limit will allow. @@ -240,7 +240,7 @@ where let d_unbounded = -match self.prev_measurement.as_ref() { Some(prev_measurement) => measurement - *prev_measurement, None => T::zero(), - } * self.kd; + } * self.kd / dt; self.prev_measurement = Some(measurement); let d = apply_limit(self.d_limit, d_unbounded); @@ -290,11 +290,11 @@ mod tests { assert_eq!(pid.setpoint, 10.0); // Test simple proportional - assert_eq!(pid.next_control_output(0.0).output, 20.0); + assert_eq!(pid.next_control_output(0.0,1.0).output, 20.0); // Test proportional limit pid.p_limit = 10.0; - assert_eq!(pid.next_control_output(0.0).output, 10.0); + assert_eq!(pid.next_control_output(0.0,1.0).output, 10.0); } /// Derivative-only controller operation and limits @@ -304,14 +304,14 @@ mod tests { pid.p(0.0, 100.0).i(0.0, 100.0).d(2.0, 100.0); // Test that there's no derivative since it's the first measurement - assert_eq!(pid.next_control_output(0.0).output, 0.0); + assert_eq!(pid.next_control_output(0.0,1.0).output, 0.0); // Test that there's now a derivative - assert_eq!(pid.next_control_output(5.0).output, -10.0); + assert_eq!(pid.next_control_output(5.0,1.0).output, -10.0); // Test derivative limit pid.d_limit = 5.0; - assert_eq!(pid.next_control_output(10.0).output, -5.0); + assert_eq!(pid.next_control_output(10.0,1.0).output, -5.0); } /// Integral-only controller operation and limits @@ -321,26 +321,26 @@ mod tests { pid.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); // Test basic integration - assert_eq!(pid.next_control_output(0.0).output, 20.0); - assert_eq!(pid.next_control_output(0.0).output, 40.0); - assert_eq!(pid.next_control_output(5.0).output, 50.0); + assert_eq!(pid.next_control_output(0.0,1.0).output, 20.0); + assert_eq!(pid.next_control_output(0.0,1.0).output, 40.0); + assert_eq!(pid.next_control_output(5.0,1.0).output, 50.0); // Test limit pid.i_limit = 50.0; - assert_eq!(pid.next_control_output(5.0).output, 50.0); + assert_eq!(pid.next_control_output(5.0,1.0).output, 50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid.next_control_output(15.0).output, 40.0); + assert_eq!(pid.next_control_output(15.0,1.0).output, 40.0); // Test that error integral accumulates negative values let mut pid2 = Pid::new(-10.0, 100.0); pid2.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); - assert_eq!(pid2.next_control_output(0.0).output, -20.0); - assert_eq!(pid2.next_control_output(0.0).output, -40.0); + assert_eq!(pid2.next_control_output(0.0,1.0).output, -20.0); + assert_eq!(pid2.next_control_output(0.0,1.0).output, -40.0); pid2.i_limit = 50.0; - assert_eq!(pid2.next_control_output(-5.0).output, -50.0); + assert_eq!(pid2.next_control_output(-5.0,1.0).output, -50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid2.next_control_output(-15.0).output, -40.0); + assert_eq!(pid2.next_control_output(-15.0,1.0).output, -40.0); } /// Checks that a full PID controller's limits work properly through multiple output iterations @@ -349,11 +349,11 @@ mod tests { let mut pid = Pid::new(10.0, 1.0); pid.p(1.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); - let out = pid.next_control_output(0.0); + let out = pid.next_control_output(0.0,1.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.output, 1.0); - let out = pid.next_control_output(20.0); + let out = pid.next_control_output(20.0,1.0); assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0) assert_eq!(out.output, -1.0); } @@ -364,25 +364,25 @@ mod tests { let mut pid = Pid::new(10.0, 100.0); pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); - let out = pid.next_control_output(0.0); + let out = pid.next_control_output(0.0,1.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.i, 1.0); // 0.1 * 10.0 assert_eq!(out.d, 0.0); // -(1.0 * 0.0) assert_eq!(out.output, 11.0); - let out = pid.next_control_output(5.0); + let out = pid.next_control_output(5.0,1.0); assert_eq!(out.p, 5.0); // 1.0 * 5.0 assert_eq!(out.i, 1.5); // 0.1 * (10.0 + 5.0) assert_eq!(out.d, -5.0); // -(1.0 * 5.0) assert_eq!(out.output, 1.5); - let out = pid.next_control_output(11.0); + let out = pid.next_control_output(11.0,1.0); assert_eq!(out.p, -1.0); // 1.0 * -1.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1) assert_eq!(out.d, -6.0); // -(1.0 * 6.0) assert_eq!(out.output, -5.6); - let out = pid.next_control_output(10.0); + let out = pid.next_control_output(10.0,1.0); assert_eq!(out.p, 0.0); // 1.0 * 0.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1.0 + 0.0) assert_eq!(out.d, 1.0); // -(1.0 * -1.0) @@ -401,8 +401,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_f32.next_control_output(0.0).output, - pid_f64.next_control_output(0.0).output as f32 + pid_f32.next_control_output(0.0,1.0).output, + pid_f64.next_control_output(0.0,1.0).output as f32 ); } } @@ -419,8 +419,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_i32.next_control_output(0).output, - pid_i8.next_control_output(0i8).output as i32 + pid_i32.next_control_output(0,1).output, + pid_i8.next_control_output(0i8,1i8).output as i32 ); } } @@ -431,7 +431,7 @@ mod tests { let mut pid = Pid::new(10.0, 100.0); pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); - let out = pid.next_control_output(0.0); + let out = pid.next_control_output(0.0, 1.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.i, 1.0); // 0.1 * 10.0 assert_eq!(out.d, 0.0); // -(1.0 * 0.0) @@ -440,7 +440,7 @@ mod tests { pid.setpoint(0.0); assert_eq!( - pid.next_control_output(0.0), + pid.next_control_output(0.0, 1.0), ControlOutput { p: 0.0, i: 1.0, @@ -456,7 +456,7 @@ mod tests { let mut pid = Pid::new(10.0f32, -10.0); pid.p(1.0, -50.0).i(1.0, -50.0).d(1.0, -50.0); - let out = pid.next_control_output(0.0); + let out = pid.next_control_output(0.0, 1.0); assert_eq!(out.p, 10.0); assert_eq!(out.i, 10.0); assert_eq!(out.d, 0.0); From 796814add0f3baee80333036fd171d6a5e7bd742 Mon Sep 17 00:00:00 2001 From: Mathilda Grace Date: Thu, 27 Mar 2025 10:25:52 -0400 Subject: [PATCH 2/4] Format code --- src/lib.rs | 51 ++++++++++++++++++++++++++------------------------- 1 file changed, 26 insertions(+), 25 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 3573b32..50a2040 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -240,7 +240,8 @@ where let d_unbounded = -match self.prev_measurement.as_ref() { Some(prev_measurement) => measurement - *prev_measurement, None => T::zero(), - } * self.kd / dt; + } * self.kd + / dt; self.prev_measurement = Some(measurement); let d = apply_limit(self.d_limit, d_unbounded); @@ -290,11 +291,11 @@ mod tests { assert_eq!(pid.setpoint, 10.0); // Test simple proportional - assert_eq!(pid.next_control_output(0.0,1.0).output, 20.0); + assert_eq!(pid.next_control_output(0.0, 1.0).output, 20.0); // Test proportional limit pid.p_limit = 10.0; - assert_eq!(pid.next_control_output(0.0,1.0).output, 10.0); + assert_eq!(pid.next_control_output(0.0, 1.0).output, 10.0); } /// Derivative-only controller operation and limits @@ -304,14 +305,14 @@ mod tests { pid.p(0.0, 100.0).i(0.0, 100.0).d(2.0, 100.0); // Test that there's no derivative since it's the first measurement - assert_eq!(pid.next_control_output(0.0,1.0).output, 0.0); + assert_eq!(pid.next_control_output(0.0, 1.0).output, 0.0); // Test that there's now a derivative - assert_eq!(pid.next_control_output(5.0,1.0).output, -10.0); + assert_eq!(pid.next_control_output(5.0, 1.0).output, -10.0); // Test derivative limit pid.d_limit = 5.0; - assert_eq!(pid.next_control_output(10.0,1.0).output, -5.0); + assert_eq!(pid.next_control_output(10.0, 1.0).output, -5.0); } /// Integral-only controller operation and limits @@ -321,26 +322,26 @@ mod tests { pid.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); // Test basic integration - assert_eq!(pid.next_control_output(0.0,1.0).output, 20.0); - assert_eq!(pid.next_control_output(0.0,1.0).output, 40.0); - assert_eq!(pid.next_control_output(5.0,1.0).output, 50.0); + assert_eq!(pid.next_control_output(0.0, 1.0).output, 20.0); + assert_eq!(pid.next_control_output(0.0, 1.0).output, 40.0); + assert_eq!(pid.next_control_output(5.0, 1.0).output, 50.0); // Test limit pid.i_limit = 50.0; - assert_eq!(pid.next_control_output(5.0,1.0).output, 50.0); + assert_eq!(pid.next_control_output(5.0, 1.0).output, 50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid.next_control_output(15.0,1.0).output, 40.0); + assert_eq!(pid.next_control_output(15.0, 1.0).output, 40.0); // Test that error integral accumulates negative values let mut pid2 = Pid::new(-10.0, 100.0); pid2.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); - assert_eq!(pid2.next_control_output(0.0,1.0).output, -20.0); - assert_eq!(pid2.next_control_output(0.0,1.0).output, -40.0); + assert_eq!(pid2.next_control_output(0.0, 1.0).output, -20.0); + assert_eq!(pid2.next_control_output(0.0, 1.0).output, -40.0); pid2.i_limit = 50.0; - assert_eq!(pid2.next_control_output(-5.0,1.0).output, -50.0); + assert_eq!(pid2.next_control_output(-5.0, 1.0).output, -50.0); // Test that limit doesn't impede reversal of error integral - assert_eq!(pid2.next_control_output(-15.0,1.0).output, -40.0); + assert_eq!(pid2.next_control_output(-15.0, 1.0).output, -40.0); } /// Checks that a full PID controller's limits work properly through multiple output iterations @@ -349,11 +350,11 @@ mod tests { let mut pid = Pid::new(10.0, 1.0); pid.p(1.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); - let out = pid.next_control_output(0.0,1.0); + let out = pid.next_control_output(0.0, 1.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.output, 1.0); - let out = pid.next_control_output(20.0,1.0); + let out = pid.next_control_output(20.0, 1.0); assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0) assert_eq!(out.output, -1.0); } @@ -364,25 +365,25 @@ mod tests { let mut pid = Pid::new(10.0, 100.0); pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); - let out = pid.next_control_output(0.0,1.0); + let out = pid.next_control_output(0.0, 1.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.i, 1.0); // 0.1 * 10.0 assert_eq!(out.d, 0.0); // -(1.0 * 0.0) assert_eq!(out.output, 11.0); - let out = pid.next_control_output(5.0,1.0); + let out = pid.next_control_output(5.0, 1.0); assert_eq!(out.p, 5.0); // 1.0 * 5.0 assert_eq!(out.i, 1.5); // 0.1 * (10.0 + 5.0) assert_eq!(out.d, -5.0); // -(1.0 * 5.0) assert_eq!(out.output, 1.5); - let out = pid.next_control_output(11.0,1.0); + let out = pid.next_control_output(11.0, 1.0); assert_eq!(out.p, -1.0); // 1.0 * -1.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1) assert_eq!(out.d, -6.0); // -(1.0 * 6.0) assert_eq!(out.output, -5.6); - let out = pid.next_control_output(10.0,1.0); + let out = pid.next_control_output(10.0, 1.0); assert_eq!(out.p, 0.0); // 1.0 * 0.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1.0 + 0.0) assert_eq!(out.d, 1.0); // -(1.0 * -1.0) @@ -401,8 +402,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_f32.next_control_output(0.0,1.0).output, - pid_f64.next_control_output(0.0,1.0).output as f32 + pid_f32.next_control_output(0.0, 1.0).output, + pid_f64.next_control_output(0.0, 1.0).output as f32 ); } } @@ -419,8 +420,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_i32.next_control_output(0,1).output, - pid_i8.next_control_output(0i8,1i8).output as i32 + pid_i32.next_control_output(0, 1).output, + pid_i8.next_control_output(0i8, 1i8).output as i32 ); } } From c8b43eb57cf506407f8aae4833ab2584e584b2b3 Mon Sep 17 00:00:00 2001 From: Mathilda Grace Date: Thu, 27 Mar 2025 11:16:07 -0400 Subject: [PATCH 3/4] Add missing argument to tests --- src/lib.rs | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 50a2040..e692b5f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -12,20 +12,20 @@ //! pid.p(10.0, 100.0); //! //! // Input a measurement with an error of 5.0 from our setpoint -//! let output = pid.next_control_output(10.0); +//! let output = pid.next_control_output(10.0, 1.0); //! //! // Show that the error is correct by multiplying by our kp //! assert_eq!(output.output, 50.0); // <-- //! assert_eq!(output.p, 50.0); //! //! // It won't change on repeat; the controller is proportional-only -//! let output = pid.next_control_output(10.0); +//! let output = pid.next_control_output(10.0, 1.0); //! assert_eq!(output.output, 50.0); // <-- //! assert_eq!(output.p, 50.0); //! //! // Add a new integral term to the controller and input again //! pid.i(1.0, 100.0); -//! let output = pid.next_control_output(10.0); +//! let output = pid.next_control_output(10.0, 1.0); //! //! // Now that the integral makes the controller stateful, it will change //! assert_eq!(output.output, 55.0); // <-- @@ -34,7 +34,7 @@ //! //! // Add our final derivative term and match our setpoint target //! pid.d(2.0, 100.0); -//! let output = pid.next_control_output(15.0); +//! let output = pid.next_control_output(15.0, 1.0); //! //! // The output will now say to go down due to the derivative //! assert_eq!(output.output, -5.0); // <-- @@ -79,7 +79,7 @@ impl Number for T {} /// p_controller.p(10.0, 100.0); /// /// // Get first output -/// let p_output = p_controller.next_control_output(400.0); +/// let p_output = p_controller.next_control_output(400.0, 1.0); /// ``` /// /// This controller would give you set a proportional controller to `10.0` with a target of `15.0` and an output limit of `100.0` per [output](Self::next_control_output) iteration. The same controller with a full PID system built in looks like: @@ -92,7 +92,7 @@ impl Number for T {} /// full_controller.p(10.0, 100.0).i(4.5, 100.0).d(0.25, 100.0); /// /// // Get first output -/// let full_output = full_controller.next_control_output(400.0); +/// let full_output = full_controller.next_control_output(400.0, 1.0); /// ``` /// /// This [`next_control_output`](Self::next_control_output) method is what's used to input new values into the controller to tell it what the current state of the system is. In the examples above it's only being used once, but realistically this will be a hot method. Please see [ControlOutput] for examples of how to handle these outputs; it's quite straight forward and mirrors the values of this structure in some ways. @@ -141,7 +141,7 @@ pub struct Pid { /// pid.p(10.0, 100.0).i(1.0, 100.0).d(2.0, 100.0); /// /// // Input an example value and get a report for an output iteration -/// let output = pid.next_control_output(26.2456); +/// let output = pid.next_control_output(26.2456, 1.0); /// println!("P: {}\nI: {}\nD: {}\nFinal Output: {}", output.p, output.i, output.d, output.output); /// ``` #[derive(Debug, PartialEq, Eq)] From 8682b696b4cff6a73e20035d430b170c78f1e027 Mon Sep 17 00:00:00 2001 From: Mathilda Grace Date: Wed, 2 Apr 2025 18:58:58 -0400 Subject: [PATCH 4/4] Refactor `Pid::next_control_output()` to call new method `Pid::next_control_output_with_dt()` --- src/lib.rs | 81 +++++++++++++++++++++++++++++++----------------------- 1 file changed, 46 insertions(+), 35 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index e692b5f..aae5d66 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -12,20 +12,20 @@ //! pid.p(10.0, 100.0); //! //! // Input a measurement with an error of 5.0 from our setpoint -//! let output = pid.next_control_output(10.0, 1.0); +//! 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, 1.0); +//! 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, 1.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); // <-- @@ -34,7 +34,7 @@ //! //! // Add our final derivative term and match our setpoint target //! pid.d(2.0, 100.0); -//! let output = pid.next_control_output(15.0, 1.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); // <-- @@ -79,7 +79,7 @@ impl Number for T {} /// p_controller.p(10.0, 100.0); /// /// // Get first output -/// let p_output = p_controller.next_control_output(400.0, 1.0); +/// let p_output = p_controller.next_control_output(400.0); /// ``` /// /// This controller would give you set a proportional controller to `10.0` with a target of `15.0` and an output limit of `100.0` per [output](Self::next_control_output) iteration. The same controller with a full PID system built in looks like: @@ -92,7 +92,7 @@ impl Number for T {} /// full_controller.p(10.0, 100.0).i(4.5, 100.0).d(0.25, 100.0); /// /// // Get first output -/// let full_output = full_controller.next_control_output(400.0, 1.0); +/// let full_output = full_controller.next_control_output(400.0); /// ``` /// /// This [`next_control_output`](Self::next_control_output) method is what's used to input new values into the controller to tell it what the current state of the system is. In the examples above it's only being used once, but realistically this will be a hot method. Please see [ControlOutput] for examples of how to handle these outputs; it's quite straight forward and mirrors the values of this structure in some ways. @@ -141,7 +141,7 @@ pub struct Pid { /// pid.p(10.0, 100.0).i(1.0, 100.0).d(2.0, 100.0); /// /// // Input an example value and get a report for an output iteration -/// let output = pid.next_control_output(26.2456, 1.0); +/// let output = pid.next_control_output(26.2456); /// println!("P: {}\nI: {}\nD: {}\nFinal Output: {}", output.p, output.i, output.d, output.output); /// ``` #[derive(Debug, PartialEq, Eq)] @@ -210,12 +210,23 @@ where self } + /// Given a new measurement, calculates the next [control output](ControlOutput). + /// + /// Assumes a constant time delta (`dt`) of one. + /// + /// # Panics + /// + /// - If a setpoint has not been set via `update_setpoint()`. + pub fn next_control_output(&mut self, measurement: T) -> ControlOutput { + self.next_control_output_with_dt(measurement, T::one()) + } + /// Given a new measurement and dt, calculates the next [control output](ControlOutput). /// /// # Panics /// /// - If a setpoint has not been set via `update_setpoint()`. - pub fn next_control_output(&mut self, measurement: T, dt: T) -> ControlOutput { + pub fn next_control_output_with_dt(&mut self, measurement: T, dt: T) -> ControlOutput { // Calculate the error between the ideal setpoint and the current // measurement to compare against let error = self.setpoint - measurement; @@ -291,11 +302,11 @@ mod tests { assert_eq!(pid.setpoint, 10.0); // Test simple proportional - assert_eq!(pid.next_control_output(0.0, 1.0).output, 20.0); + assert_eq!(pid.next_control_output(0.0).output, 20.0); // Test proportional limit pid.p_limit = 10.0; - assert_eq!(pid.next_control_output(0.0, 1.0).output, 10.0); + assert_eq!(pid.next_control_output(0.0).output, 10.0); } /// Derivative-only controller operation and limits @@ -305,14 +316,14 @@ mod tests { pid.p(0.0, 100.0).i(0.0, 100.0).d(2.0, 100.0); // Test that there's no derivative since it's the first measurement - assert_eq!(pid.next_control_output(0.0, 1.0).output, 0.0); + assert_eq!(pid.next_control_output(0.0).output, 0.0); // Test that there's now a derivative - assert_eq!(pid.next_control_output(5.0, 1.0).output, -10.0); + assert_eq!(pid.next_control_output(5.0).output, -10.0); // Test derivative limit pid.d_limit = 5.0; - assert_eq!(pid.next_control_output(10.0, 1.0).output, -5.0); + assert_eq!(pid.next_control_output(10.0).output, -5.0); } /// Integral-only controller operation and limits @@ -322,26 +333,26 @@ mod tests { pid.p(0.0, 100.0).i(2.0, 100.0).d(0.0, 100.0); // Test basic integration - assert_eq!(pid.next_control_output(0.0, 1.0).output, 20.0); - assert_eq!(pid.next_control_output(0.0, 1.0).output, 40.0); - assert_eq!(pid.next_control_output(5.0, 1.0).output, 50.0); + assert_eq!(pid.next_control_output(0.0).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, 1.0).output, 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, 1.0).output, 40.0); + 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, 1.0).output, -20.0); - assert_eq!(pid2.next_control_output(0.0, 1.0).output, -40.0); + assert_eq!(pid2.next_control_output(0.0).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, 1.0).output, -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, 1.0).output, -40.0); + 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 @@ -350,11 +361,11 @@ mod tests { let mut pid = Pid::new(10.0, 1.0); pid.p(1.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); - let out = pid.next_control_output(0.0, 1.0); + let out = pid.next_control_output(0.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.output, 1.0); - let out = pid.next_control_output(20.0, 1.0); + let out = pid.next_control_output(20.0); assert_eq!(out.p, -10.0); // 1.0 * (10.0 - 20.0) assert_eq!(out.output, -1.0); } @@ -365,25 +376,25 @@ mod tests { let mut pid = Pid::new(10.0, 100.0); pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); - let out = pid.next_control_output(0.0, 1.0); + let out = pid.next_control_output(0.0); assert_eq!(out.p, 10.0); // 1.0 * 10.0 assert_eq!(out.i, 1.0); // 0.1 * 10.0 assert_eq!(out.d, 0.0); // -(1.0 * 0.0) assert_eq!(out.output, 11.0); - let out = pid.next_control_output(5.0, 1.0); + let out = pid.next_control_output(5.0); assert_eq!(out.p, 5.0); // 1.0 * 5.0 assert_eq!(out.i, 1.5); // 0.1 * (10.0 + 5.0) assert_eq!(out.d, -5.0); // -(1.0 * 5.0) assert_eq!(out.output, 1.5); - let out = pid.next_control_output(11.0, 1.0); + let out = pid.next_control_output(11.0); assert_eq!(out.p, -1.0); // 1.0 * -1.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1) assert_eq!(out.d, -6.0); // -(1.0 * 6.0) assert_eq!(out.output, -5.6); - let out = pid.next_control_output(10.0, 1.0); + let out = pid.next_control_output(10.0); assert_eq!(out.p, 0.0); // 1.0 * 0.0 assert_eq!(out.i, 1.4); // 0.1 * (10.0 + 5.0 - 1.0 + 0.0) assert_eq!(out.d, 1.0); // -(1.0 * -1.0) @@ -402,8 +413,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_f32.next_control_output(0.0, 1.0).output, - pid_f64.next_control_output(0.0, 1.0).output as f32 + pid_f32.next_control_output(0.0).output, + pid_f64.next_control_output(0.0).output as f32 ); } } @@ -420,8 +431,8 @@ mod tests { for _ in 0..5 { assert_eq!( - pid_i32.next_control_output(0, 1).output, - pid_i8.next_control_output(0i8, 1i8).output as i32 + pid_i32.next_control_output(0).output, + pid_i8.next_control_output(0i8).output as i32 ); } } @@ -432,7 +443,7 @@ mod tests { let mut pid = Pid::new(10.0, 100.0); pid.p(1.0, 100.0).i(0.1, 100.0).d(1.0, 100.0); - let out = pid.next_control_output(0.0, 1.0); + let out = pid.next_control_output(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) @@ -441,7 +452,7 @@ mod tests { pid.setpoint(0.0); assert_eq!( - pid.next_control_output(0.0, 1.0), + pid.next_control_output(0.0), ControlOutput { p: 0.0, i: 1.0, @@ -457,7 +468,7 @@ mod tests { let mut pid = Pid::new(10.0f32, -10.0); pid.p(1.0, -50.0).i(1.0, -50.0).d(1.0, -50.0); - let out = pid.next_control_output(0.0, 1.0); + let out = pid.next_control_output(0.0); assert_eq!(out.p, 10.0); assert_eq!(out.i, 10.0); assert_eq!(out.d, 0.0);