Denver Hayward
Simple PID
Revision 0:3ce55a50a6b3, committed 2017-07-31
- Comitter:
- williampeers
- Date:
- Mon Jul 31 04:01:31 2017 +0000
- Commit message:
- Denver version
Changed in this revision
| pid_controller.cpp | Show annotated file Show diff for this revision Revisions of this file |
| pid_controller.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r 000000000000 -r 3ce55a50a6b3 pid_controller.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/pid_controller.cpp Mon Jul 31 04:01:31 2017 +0000
@@ -0,0 +1,225 @@
+//*********************************************************************************
+// Arduino PID Library Version 1.0.1 Modified Version for C++
+// Platform Independent
+//
+// Revision: 1.1
+//
+// Description: The PID Controller module originally meant for Arduino made
+// platform independent. Some small bugs present in the original Arduino source
+// have been rectified as well.
+//
+// For a detailed explanation of the theory behind this library, go to:
+// http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
+//
+// Revisions can be found here:
+// https://github.com/tcleg
+//
+// Modified by: Trent Cleghorn , <trentoncleghorn@gmail.com>
+//
+// Copyright (C) Brett Beauregard , <br3ttb@gmail.com>
+//
+// GPLv3 License
+//
+// This program is free software: you can redistribute it and/or modify it under
+// the terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or (at your option) any later
+// version.
+//
+// This program is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
+// PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License along with
+// this program. If not, see <http://www.gnu.org/licenses/>.
+//*********************************************************************************
+
+//*********************************************************************************
+// Headers
+//*********************************************************************************
+#include "pid_controller.h"
+
+//*********************************************************************************
+// Macros and Globals
+//*********************************************************************************
+#define CONSTRAIN(x,lower,upper) ((x)<(lower)?(lower):((x)>(upper)?(upper):(x)))
+
+//*********************************************************************************
+// Public Class Functions
+//*********************************************************************************
+
+PIDControl::
+PIDControl (float kp, float ki, float kd, float sampleTimeSeconds, float minOutput,
+ float maxOutput, PIDMode mode, PIDDirection controllerDirection)
+{
+ controllerDirection = controllerDirection;
+ mode = mode;
+ iTerm = 0.0f;
+ input = 0.0f;
+ lastInput = 0.0f;
+ output = 0.0f;
+ setpoint = 0.0f;
+
+ if(sampleTimeSeconds > 0.0f)
+ {
+ sampleTime = sampleTimeSeconds;
+ }
+ else
+ {
+ // If the passed parameter was incorrect, set to 1 second
+ sampleTime = 1.0f;
+ }
+
+ PIDOutputLimitsSet(minOutput, maxOutput);
+ PIDTuningsSet(kp, ki, kd);
+}
+
+bool PIDControl::
+PIDCompute()
+{
+ float error, dInput;
+
+ if(mode == MANUAL)
+ {
+ return false;
+ }
+
+ // The classic PID error term
+ error = setpoint - input;
+
+ // Compute the integral term separately ahead of time
+ iTerm += alteredKi * error;
+
+ // Constrain the integrator to make sure it does not exceed output bounds
+ iTerm = CONSTRAIN(iTerm, outMin, outMax);
+
+ // Take the "derivative on measurement" instead of "derivative on error"
+ dInput = input - lastInput;
+
+ // Run all the terms together to get the overall output
+ output = alteredKp * error + iTerm - alteredKd * dInput;
+
+ // Bound the output
+ output = CONSTRAIN(output, outMin, outMax);
+
+ // Make the current input the former input
+ lastInput = input;
+
+ return true;
+}
+
+void PIDControl::
+PIDModeSet(PIDMode mode)
+{
+ // If the mode changed from MANUAL to AUTOMATIC
+ if(mode != mode && mode == AUTOMATIC)
+ {
+ // Initialize a few PID parameters to new values
+ iTerm = output;
+ lastInput = input;
+
+ // Constrain the integrator to make sure it does not exceed output bounds
+ iTerm = CONSTRAIN(iTerm, outMin, outMax);
+ }
+
+ mode = mode;
+}
+
+void PIDControl::
+PIDOutputLimitsSet(float min, float max)
+{
+ // Check if the parameters are valid
+ if(min >= max)
+ {
+ return;
+ }
+
+ // Save the parameters
+ outMin = min;
+ outMax = max;
+
+ // If in automatic, apply the new constraints
+ if(mode == AUTOMATIC)
+ {
+ output = CONSTRAIN(output, min, max);
+ iTerm = CONSTRAIN(iTerm, min, max);
+ }
+}
+
+void PIDControl::
+PIDTuningsSet(float kp, float ki, float kd)
+{
+ // Check if the parameters are valid
+ if(kp < 0.0f || ki < 0.0f || kd < 0.0f)
+ {
+ return;
+ }
+
+ // Save the parameters for displaying purposes
+ dispKp = kp;
+ dispKi = ki;
+ dispKd = kd;
+
+ // Alter the parameters for PID
+ alteredKp = kp;
+ alteredKi = ki * sampleTime;
+ alteredKd = kd / sampleTime;
+
+ // Apply reverse direction to the altered values if necessary
+ if(controllerDirection == REVERSE)
+ {
+ alteredKp = -(alteredKp);
+ alteredKi = -(alteredKi);
+ alteredKd = -(alteredKd);
+ }
+}
+
+void PIDControl::
+PIDTuningKpSet(float kp)
+{
+ PIDTuningsSet(kp, dispKi, dispKd);
+}
+
+void PIDControl::
+PIDTuningKiSet(float ki)
+{
+ PIDTuningsSet(dispKp, ki, dispKd);
+}
+
+void PIDControl::
+PIDTuningKdSet(float kd)
+{
+ PIDTuningsSet(dispKp, dispKi, kd);
+}
+
+void PIDControl::
+PIDControllerDirectionSet(PIDDirection controllerDirection)
+{
+ // If in automatic mode and the controller's sense of direction is reversed
+ if(mode == AUTOMATIC && controllerDirection == REVERSE)
+ {
+ // Reverse sense of direction of PID gain constants
+ alteredKp = -(alteredKp);
+ alteredKi = -(alteredKi);
+ alteredKd = -(alteredKd);
+ }
+
+ controllerDirection = controllerDirection;
+}
+
+void PIDControl::
+PIDSampleTimeSet(float sampleTimeSeconds)
+{
+ float ratio;
+
+ if(sampleTimeSeconds > 0.0f)
+ {
+ // Find the ratio of change and apply to the altered values
+ ratio = sampleTimeSeconds / sampleTime;
+ alteredKi *= ratio;
+ alteredKd /= ratio;
+
+ // Save the new sampling time
+ sampleTime = sampleTimeSeconds;
+ }
+}
+
diff -r 000000000000 -r 3ce55a50a6b3 pid_controller.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/pid_controller.h Mon Jul 31 04:01:31 2017 +0000
@@ -0,0 +1,383 @@
+//*********************************************************************************
+// Arduino PID Library Version 1.0.1 Modified Version for C++
+// Platform Independent
+//
+// Revision: 1.1
+//
+// Description: The PID Controller module originally meant for Arduino made
+// platform independent. Some small bugs present in the original Arduino source
+// have been rectified as well.
+//
+// For a detailed explanation of the theory behind this library, go to:
+// http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
+//
+// Revisions can be found here:
+// https://github.com/tcleg
+//
+// Modified by: Trent Cleghorn , <trentoncleghorn@gmail.com>
+//
+// Copyright (C) Brett Beauregard , <br3ttb@gmail.com>
+//
+// GPLv3 License
+//
+// This program is free software: you can redistribute it and/or modify it under
+// the terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or (at your option) any later
+// version.
+//
+// This program is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
+// PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License along with
+// this program. If not, see <http://www.gnu.org/licenses/>.
+//*********************************************************************************
+
+//
+// Header Guard
+//
+#ifndef PID_CONTROLLER_H
+#define PID_CONTROLLER_H
+
+//*********************************************************************************
+// Headers
+//*********************************************************************************
+#include <stdint.h>
+#include <stdbool.h>
+
+//*********************************************************************************
+// Macros and Globals
+//*********************************************************************************
+
+typedef enum
+{
+ MANUAL,
+ AUTOMATIC
+}
+PIDMode;
+
+typedef enum
+{
+ DIRECT,
+ REVERSE
+}
+PIDDirection;
+
+//*********************************************************************************
+// Class
+//*********************************************************************************
+
+class
+PIDControl
+{
+ public:
+ //
+ // Constructor
+ // Description:
+ // Initializes the PIDControl instantiation. This should be called at
+ // least once before any other PID functions are called on the
+ // instantiation.
+ // Parameters:
+ // kp - Positive P gain constant value.
+ // ki - Positive I gain constant value.
+ // kd - Positive D gain constant value.
+ // sampleTimeSeconds - Interval in seconds on which PIDCompute will be
+ // called.
+ // minOutput - Constrain PID output to this minimum value.
+ // maxOutput - Constrain PID output to this maximum value.
+ // mode - Tells how the controller should respond if the user has
+ // taken over manual control or not.
+ // MANUAL: PID controller is off. User can manually control the
+ // output.
+ // AUTOMATIC: PID controller is on. PID controller controls the
+ // output.
+ // controllerDirection - The sense of direction of the controller
+ // DIRECT: A positive setpoint gives a positive output.
+ // REVERSE: A positive setpoint gives a negative output.
+ // Returns:
+ // Nothing.
+ //
+ PIDControl(float kp, float ki, float kd, float sampleTimeSeconds,
+ float minOutput, float maxOutput, PIDMode mode,
+ PIDDirection controllerDirection);
+
+ //
+ // PID Compute
+ // Description:
+ // Should be called on a regular interval defined by sampleTimeSeconds.
+ // Typically, PIDSetpointSet and PIDInputSet should be called
+ // immediately before PIDCompute.
+ // Parameters:
+ // None.
+ // Returns:
+ // True if in AUTOMATIC. False if in MANUAL.
+ //
+ bool PIDCompute();
+
+ //
+ // PID Mode Set
+ // Description:
+ // Sets the PID controller to a new mode. Tells how the controller
+ // should respond if the user has taken over manual control or not.
+ // Parameters:
+ // mode -
+ // MANUAL: PID controller is off. User can manually control the
+ // output.
+ // AUTOMATIC: PID controller is on. PID controller controls the
+ // output.
+ // Returns:
+ // Nothing.
+ //
+ void PIDModeSet(PIDMode mode);
+
+ //
+ // PID Output Limits Set
+ // Description:
+ // Sets the new output limits. The new limits are applied to the PID
+ // immediately.
+ // Parameters:
+ // min - Constrain PID output to this minimum value.
+ // max - Constrain PID output to this maximum value.
+ // Returns:
+ // Nothing.
+ //
+ void PIDOutputLimitsSet(float min, float max);
+
+ //
+ // PID Tunings Set
+ // Description:
+ // Sets the new gain constant values.
+ // Parameters:
+ // kp - Positive P gain constant value.
+ // ki - Positive I gain constant value.
+ // kd - Positive D gain constant value.
+ // Returns:
+ // Nothing.
+ //
+ void PIDTuningsSet(float kp, float ki, float kd);
+
+ //
+ // PID Tuning Gain Constant P Set
+ // Description:
+ // Sets the proportional gain constant value.
+ // Parameters:
+ // kp - Positive P gain constant value.
+ // Returns:
+ // Nothing.
+ //
+ void PIDTuningKpSet(float kp);
+
+ //
+ // PID Tuning Gain Constant I Set
+ // Description:
+ // Sets the proportional gain constant value.
+ // Parameters:
+ // ki - Positive I gain constant value.
+ // Returns:
+ // Nothing.
+ //
+ void PIDTuningKiSet(float ki);
+
+ //
+ // PID Tuning Gain Constant D Set
+ // Description:
+ // Sets the proportional gain constant value.
+ // Parameters:
+ // kd - Positive D gain constant value.
+ // Returns:
+ // Nothing.
+ //
+ void PIDTuningKdSet(float kd);
+
+ //
+ // PID Controller Direction Set
+ // Description:
+ // Sets the new controller direction.
+ // Parameters:
+ // controllerDirection - The sense of direction of the controller
+ // DIRECT: A positive setpoint gives a positive output
+ // REVERSE: A positive setpoint gives a negative output
+ // Returns:
+ // Nothing.
+ //
+ void PIDControllerDirectionSet(PIDDirection controllerDirection);
+
+ //
+ // PID Sample Time Set
+ // Description:
+ // Sets the new sampling time (in seconds).
+ // Parameters:
+ // sampleTimeSeconds - Interval in seconds on which PIDCompute will be
+ // called.
+ // Returns:
+ // Nothing.
+ //
+ void PIDSampleTimeSet(float sampleTimeSeconds);
+
+ //
+ // PID Setpoint Set
+ // Description:
+ // Alters the setpoint the PID controller will try to achieve.
+ // Parameters:
+ // setpoint - The desired setpoint the PID controller will try to
+ // obtain.
+ // Returns:
+ // Nothing.
+ //
+ inline void PIDSetpointSet(float setpoint) { this->setpoint = setpoint; }
+
+ //
+ // PID Input Set
+ // Description:
+ // Should be called before calling PIDCompute so the PID controller
+ // will have an updated input value to work with.
+ // Parameters:
+ // input - The value the controller will work with.
+ // Returns:
+ // Nothing.
+ //
+ inline void PIDInputSet(float input) { this->input = input; }
+
+ //
+ // PID Output Get
+ // Description:
+ // Typically, this function is called after PIDCompute in order to
+ // retrieve the output of the controller.
+ // Parameters:
+ // None.
+ // Returns:
+ // The output of the specific PID controller.
+ //
+ inline float PIDOutputGet() { return this->output; }
+
+ //
+ // PID Proportional Gain Constant Get
+ // Description:
+ // Returns the proportional gain constant value the particular
+ // controller is set to.
+ // Parameters:
+ // None.
+ // Returns:
+ // The proportional gain constant.
+ //
+ inline float PIDKpGet() { return this->dispKp; }
+
+ //
+ // PID Integral Gain Constant Get
+ // Description:
+ // Returns the integral gain constant value the particular
+ // controller is set to.
+ // Parameters:
+ // None.
+ // Returns:
+ // The integral gain constant.
+ //
+ inline float PIDKiGet() { return this->dispKi; }
+
+ //
+ // PID Derivative Gain Constant Get
+ // Description:
+ // Returns the derivative gain constant value the particular
+ // controller is set to.
+ // Parameters:
+ // None.
+ // Returns:
+ // The derivative gain constant.
+ //
+ inline float PIDKdGet() { return this->dispKd; }
+
+ //
+ // PID Mode Get
+ // Description:
+ // Returns the mode the particular controller is set to.
+ // Parameters:
+ // None.
+ // Returns:
+ // MANUAL or AUTOMATIC depending on what the user set the
+ // controller to.
+ //
+ inline PIDMode PIDModeGet() { return this->mode; }
+
+ //
+ // PID Direction Get
+ // Description:
+ // Returns the direction the particular controller is set to.
+ // Parameters:
+ // None.
+ // Returns:
+ // DIRECT or REVERSE depending on what the user set the
+ // controller to.
+ //
+ inline PIDDirection PIDDirectionGet() { return this->controllerDirection; }
+
+ private:
+ //
+ // Input to the PID Controller
+ //
+ float input;
+
+ //
+ // Previous input to the PID Controller
+ //
+ float lastInput;
+
+ //
+ // Output of the PID Controller
+ //
+ float output;
+
+ //
+ // Gain constant values that were passed by the user
+ // These are for display purposes
+ //
+ float dispKp;
+ float dispKi;
+ float dispKd;
+
+ //
+ // Gain constant values that the controller alters for
+ // its own use
+ //
+ float alteredKp;
+ float alteredKi;
+ float alteredKd;
+
+ //
+ // The Integral Term
+ //
+ float iTerm;
+
+ //
+ // The interval (in seconds) on which the PID controller
+ // will be called
+ //
+ float sampleTime;
+
+ //
+ // The values that the output will be constrained to
+ //
+ float outMin;
+ float outMax;
+
+ //
+ // The user chosen operating point
+ //
+ float setpoint;
+
+ //
+ // The sense of direction of the controller
+ // DIRECT: A positive setpoint gives a positive output
+ // REVERSE: A positive setpoint gives a negative output
+ //
+ PIDDirection controllerDirection;
+
+ //
+ // Tells how the controller should respond if the user has
+ // taken over manual control or not
+ // MANUAL: PID controller is off.
+ // AUTOMATIC: PID controller is on.
+ //
+ PIDMode mode;
+};
+
+#endif // PID_CONTROLLER_H
\ No newline at end of file