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 +}