pid-4.0.0/.cargo_vcs_info.json0000644000000001360000000000100116110ustar { "git": { "sha1": "ee0fe7a43df59f98c116649bc6448c9d2804d9ca" }, "path_in_vcs": "" }pid-4.0.0/.gitignore000064400000000000000000000000640072674642500124210ustar 00000000000000*~ *.swp /.idea /target **/*.rs.bk Cargo.lock pid-4.0.0/.travis.yml000064400000000000000000000002000072674642500125320ustar 00000000000000language: rust rust: - stable - beta - nightly matrix: allow_failures: - rust: nightly fast_finish: true pid-4.0.0/Cargo.toml0000644000000017460000000000100076170ustar # THIS FILE IS AUTOMATICALLY GENERATED BY CARGO # # When uploading crates to the registry Cargo will automatically # "normalize" Cargo.toml files for maximal compatibility # with all versions of Cargo and also rewrite `path` dependencies # to registry (e.g., crates.io) dependencies. # # If you are reading this file be aware that the original Cargo.toml # will likely look very different (and much more reasonable). # See Cargo.toml.orig for the original contents. [package] edition = "2018" name = "pid" version = "4.0.0" authors = [ "Ken Elkabany ", "Owez Griffiths ", ] description = "A PID controller." readme = "README.md" keywords = ["pid"] license = "MIT OR Apache-2.0" repository = "https://github.com/braincore/pid-rs" [dependencies.num-traits] version = "0.2" default-features = false [dependencies.serde] version = "1.0" features = ["derive"] optional = true default-features = false [badges.travis-ci] repository = "braincore/pid-rs" pid-4.0.0/Cargo.toml.orig000064400000000000000000000010470072674642500133220ustar 00000000000000[package] name = "pid" version = "4.0.0" edition = "2018" authors = ["Ken Elkabany ", "Owez Griffiths "] license = "MIT OR Apache-2.0" description = "A PID controller." repository = "https://github.com/braincore/pid-rs" keywords = ["pid"] readme = "README.md" [dependencies.num-traits] version = "0.2" default-features = false [dependencies.serde] version = "1.0" optional = true default-features = false features = ["derive"] [badges] travis-ci = { repository = "braincore/pid-rs" } pid-4.0.0/LICENSE-APACHE000064400000000000000000000264500072674642500123640ustar 00000000000000 Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 1. Definitions. "License" shall mean the terms and conditions for use, reproduction, and distribution as defined by Sections 1 through 9 of this document. "Licensor" shall mean the copyright owner or entity authorized by the copyright owner that is granting the License. "Legal Entity" shall mean the union of the acting entity and all other entities that control, are controlled by, or are under common control with that entity. For the purposes of this definition, "control" means (i) the power, direct or indirect, to cause the direction or management of such entity, whether by contract or otherwise, or (ii) ownership of fifty percent (50%) or more of the outstanding shares, or (iii) beneficial ownership of such entity. "You" (or "Your") shall mean an individual or Legal Entity exercising permissions granted by this License. "Source" form shall mean the preferred form for making modifications, including but not limited to software source code, documentation source, and configuration files. "Object" form shall mean any form resulting from mechanical transformation or translation of a Source form, including but not limited to compiled object code, generated documentation, and conversions to other media types. "Work" shall mean the work of authorship, whether in Source or Object form, made available under the License, as indicated by a copyright notice that is included in or attached to the work (an example is provided in the Appendix below). "Derivative Works" shall mean any work, whether in Source or Object form, that is based on (or derived from) the Work and for which the editorial revisions, annotations, elaborations, or other modifications represent, as a whole, an original work of authorship. For the purposes of this License, Derivative Works shall not include works that remain separable from, or merely link (or bind by name) to the interfaces of, the Work and Derivative Works thereof. "Contribution" shall mean any work of authorship, including the original version of the Work and any modifications or additions to that Work or Derivative Works thereof, that is intentionally submitted to Licensor for inclusion in the Work by the copyright owner or by an individual or Legal Entity authorized to submit on behalf of the copyright owner. For the purposes of this definition, "submitted" means any form of electronic, verbal, or written communication sent to the Licensor or its representatives, including but not limited to communication on electronic mailing lists, source code control systems, and issue tracking systems that are managed by, or on behalf of, the Licensor for the purpose of discussing and improving the Work, but excluding communication that is conspicuously marked or otherwise designated in writing by the copyright owner as "Not a Contribution." "Contributor" shall mean Licensor and any individual or Legal Entity on behalf of whom a Contribution has been received by Licensor and subsequently incorporated within the Work. 2. Grant of Copyright License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable copyright license to reproduce, prepare Derivative Works of, publicly display, publicly perform, sublicense, and distribute the Work and such Derivative Works in Source or Object form. 3. Grant of Patent License. Subject to the terms and conditions of this License, each Contributor hereby grants to You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, irrevocable (except as stated in this section) patent license to make, have made, use, offer to sell, sell, import, and otherwise transfer the Work, where such license applies only to those patent claims licensable by such Contributor that are necessarily infringed by their Contribution(s) alone or by combination of their Contribution(s) with the Work to which such Contribution(s) was submitted. If You institute patent litigation against any entity (including a cross-claim or counterclaim in a lawsuit) alleging that the Work or a Contribution incorporated within the Work constitutes direct or contributory patent infringement, then any patent licenses granted to You under this License for that Work shall terminate as of the date such litigation is filed. 4. Redistribution. You may reproduce and distribute copies of the Work or Derivative Works thereof in any medium, with or without modifications, and in Source or Object form, provided that You meet the following conditions: (a) You must give any other recipients of the Work or Derivative Works a copy of this License; and (b) You must cause any modified files to carry prominent notices stating that You changed the files; and (c) You must retain, in the Source form of any Derivative Works that You distribute, all copyright, patent, trademark, and attribution notices from the Source form of the Work, excluding those notices that do not pertain to any part of the Derivative Works; and (d) If the Work includes a "NOTICE" text file as part of its distribution, then any Derivative Works that You distribute must include a readable copy of the attribution notices contained within such NOTICE file, excluding those notices that do not pertain to any part of the Derivative Works, in at least one of the following places: within a NOTICE text file distributed as part of the Derivative Works; within the Source form or documentation, if provided along with the Derivative Works; or, within a display generated by the Derivative Works, if and wherever such third-party notices normally appear. The contents of the NOTICE file are for informational purposes only and do not modify the License. You may add Your own attribution notices within Derivative Works that You distribute, alongside or as an addendum to the NOTICE text from the Work, provided that such additional attribution notices cannot be construed as modifying the License. You may add Your own copyright statement to Your modifications and may provide additional or different license terms and conditions for use, reproduction, or distribution of Your modifications, or for any such Derivative Works as a whole, provided Your use, reproduction, and distribution of the Work otherwise complies with the conditions stated in this License. 5. Submission of Contributions. Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 6. Trademarks. This License does not grant permission to use the trade names, trademarks, service marks, or product names of the Licensor, except as required for reasonable and customary use in describing the origin of the Work and reproducing the content of the NOTICE file. 7. Disclaimer of Warranty. Unless required by applicable law or agreed to in writing, Licensor provides the Work (and each Contributor provides its Contributions) on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied, including, without limitation, any warranties or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A PARTICULAR PURPOSE. You are solely responsible for determining the appropriateness of using or redistributing the Work and assume any risks associated with Your exercise of permissions under this License. 8. Limitation of Liability. In no event and under no legal theory, whether in tort (including negligence), contract, or otherwise, unless required by applicable law (such as deliberate and grossly negligent acts) or agreed to in writing, shall any Contributor be liable to You for damages, including any direct, indirect, special, incidental, or consequential damages of any character arising as a result of this License or out of the use or inability to use the Work (including but not limited to damages for loss of goodwill, work stoppage, computer failure or malfunction, or any and all other commercial damages or losses), even if such Contributor has been advised of the possibility of such damages. 9. Accepting Warranty or Additional Liability. While redistributing the Work or Derivative Works thereof, You may choose to offer, and charge a fee for, acceptance of support, warranty, indemnity, or other liability obligations and/or rights consistent with this License. However, in accepting such obligations, You may act only on Your own behalf and on Your sole responsibility, not on behalf of any other Contributor, and only if You agree to indemnify, defend, and hold each Contributor harmless for any liability incurred by, or claims asserted against, such Contributor by reason of your accepting any such warranty or additional liability. END OF TERMS AND CONDITIONS APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a file or class name and description of purpose be included on the same "printed page" as the copyright notice for easier identification within third-party archives. Copyright [yyyy] [name of copyright owner] Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. pid-4.0.0/LICENSE-MIT000064400000000000000000000020710072674642500120650ustar 00000000000000Copyright (c) 2018 Ken Elkabany Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. pid-4.0.0/README.md000064400000000000000000000074340072674642500117200ustar 00000000000000# PID Controller for Rust [![Latest Version]][crates.io] [![Documentation]][docs.rs] [![Build Status]][travis] [Build Status]: https://api.travis-ci.org/braincore/pid-rs.svg?branch=master [travis]: https://travis-ci.org/braincore/pid-rs [Latest Version]: https://img.shields.io/crates/v/pid.svg [crates.io]: https://crates.io/crates/pid [Documentation]: https://docs.rs/pid/badge.svg [docs.rs]: https://docs.rs/pid A proportional-integral-derivative (PID) controller. ## Features * Visibility into individual contribution of P, I, and D terms which often need to be logged for later analysis and parameter tuning. * Output limits on a per term basis. * Three-term control output limit. * Mitigation of integral windup using integral term limit. * Mitigation of derivative kick by using the derivative of the measurement rather than the derivative of the error. * On-the-fly changes to `setpoint`/`kp`/`ki`/`kd`. * Mitigation of output jumps when changing `ki` by storing the integration of `e(t) * ki(t)` rather than only `e(t)`. * Generic float type parameter to support `f32` or `f64`. * Support for `no_std` environments, such as embedded systems. * Optional support for [Serde](https://crates.io/crates/serde). Enable the `serde` Cargo feature, if you need `Pid` to implement `Serialize`/`Deserialize`. ## Example ```rust 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); // 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); ``` ## Assumptions * Measurements occur at equal spacing. (`t(i) = t(i-1) + C`) * Output limits per term are symmetric around 0 (`-limit <= term <= limit`). ## Formulation There are several different formulations of PID controllers. This library uses the independent form: ![PID independent form]( https://latex.codecogs.com/gif.latex?C(t)&space;=&space;&space;K_p&space;\cdot&space;e(t)&space;+&space;K_i&space;\cdot&space;\int{e(t)dt}&space;-&space;K_d&space;\cdot&space;\frac{dP(t)}{dt}) where: - C(t) = control output, the output to the actuator. - P(t) = process variable, the measured value. - e(t) = error = S(t) - P(t) - S(t) = set point, the desired target for the process variable. `kp`/`ki`/`kd` can be changed during operation and can therefore be a function of time. If you're interested in the dependent form, add your own logic that computes `kp`/`ki`/`kd` using dead time, time constant, `kc`, or whatever else. ## Todo - [ ] Helper for (auto-)tuning by detecting frequency & amplitude of oscillations. ## License Licensed under either at your discretion: - Apache License, Version 2.0 (LICENSE-APACHE or http://www.apache.org/licenses/LICENSE-2.0) - MIT license (LICENSE-MIT or http://opensource.org/licenses/MIT) pid-4.0.0/src/lib.rs000064400000000000000000000360060072674642500123410ustar 00000000000000//! 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 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); //! ``` #![no_std] use num_traits::float::FloatCore; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; /// 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 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 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. #[derive(Clone, Copy, Debug, 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 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: FloatCore, { /// 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` (`-limit <= output <= limit`). fn apply_limit(limit: T, value: T) -> T { limit.min(value.abs()) * value.signum() } #[cfg(test)] mod tests { use super::Pid; use crate::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); } /// Full PID operation with mixed f32/f64 checking to make sure they're equal #[test] fn f32_and_f64() { let mut pid32 = Pid::new(10.0f32, 100.0); pid32.p(0.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); let mut pid64 = Pid::new(10.0, 100.0f64); pid64.p(0.0, 100.0).i(0.0, 100.0).d(0.0, 100.0); assert_eq!( pid32.next_control_output(0.0).output, pid64.next_control_output(0.0).output as f32 ); assert_eq!( pid32.next_control_output(0.0).output as f64, pid64.next_control_output(0.0).output ); } /// 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 } ); } }