Denver Hayward / PID

Simple PID

pid_controller.cpp@0:3ce55a50a6b3, 2017-07-31 (annotated)

Committer:
williampeers
Date:
Mon Jul 31 04:01:31 2017 +0000
Revision:
0:3ce55a50a6b3
Denver version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
williampeers 0:3ce55a50a6b3 1 //*********************************************************************************
williampeers 0:3ce55a50a6b3 2 // Arduino PID Library Version 1.0.1 Modified Version for C++
williampeers 0:3ce55a50a6b3 3 // Platform Independent
williampeers 0:3ce55a50a6b3 4 //
williampeers 0:3ce55a50a6b3 5 // Revision: 1.1
williampeers 0:3ce55a50a6b3 6 //
williampeers 0:3ce55a50a6b3 7 // Description: The PID Controller module originally meant for Arduino made
williampeers 0:3ce55a50a6b3 8 // platform independent. Some small bugs present in the original Arduino source
williampeers 0:3ce55a50a6b3 9 // have been rectified as well.
williampeers 0:3ce55a50a6b3 10 //
williampeers 0:3ce55a50a6b3 11 // For a detailed explanation of the theory behind this library, go to:
williampeers 0:3ce55a50a6b3 12 // http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
williampeers 0:3ce55a50a6b3 13 //
williampeers 0:3ce55a50a6b3 14 // Revisions can be found here:
williampeers 0:3ce55a50a6b3 15 // https://github.com/tcleg
williampeers 0:3ce55a50a6b3 16 //
williampeers 0:3ce55a50a6b3 17 // Modified by: Trent Cleghorn , <trentoncleghorn@gmail.com>
williampeers 0:3ce55a50a6b3 18 //
williampeers 0:3ce55a50a6b3 19 // Copyright (C) Brett Beauregard , <br3ttb@gmail.com>
williampeers 0:3ce55a50a6b3 20 //
williampeers 0:3ce55a50a6b3 21 // GPLv3 License
williampeers 0:3ce55a50a6b3 22 //
williampeers 0:3ce55a50a6b3 23 // This program is free software: you can redistribute it and/or modify it under
williampeers 0:3ce55a50a6b3 24 // the terms of the GNU General Public License as published by the Free Software
williampeers 0:3ce55a50a6b3 25 // Foundation, either version 3 of the License, or (at your option) any later
williampeers 0:3ce55a50a6b3 26 // version.
williampeers 0:3ce55a50a6b3 27 //
williampeers 0:3ce55a50a6b3 28 // This program is distributed in the hope that it will be useful, but WITHOUT ANY
williampeers 0:3ce55a50a6b3 29 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
williampeers 0:3ce55a50a6b3 30 // PARTICULAR PURPOSE. See the GNU General Public License for more details.
williampeers 0:3ce55a50a6b3 31 //
williampeers 0:3ce55a50a6b3 32 // You should have received a copy of the GNU General Public License along with
williampeers 0:3ce55a50a6b3 33 // this program. If not, see <http://www.gnu.org/licenses/>.
williampeers 0:3ce55a50a6b3 34 //*********************************************************************************
williampeers 0:3ce55a50a6b3 35
williampeers 0:3ce55a50a6b3 36 //*********************************************************************************
williampeers 0:3ce55a50a6b3 37 // Headers
williampeers 0:3ce55a50a6b3 38 //*********************************************************************************
williampeers 0:3ce55a50a6b3 39 #include "pid_controller.h"
williampeers 0:3ce55a50a6b3 40
williampeers 0:3ce55a50a6b3 41 //*********************************************************************************
williampeers 0:3ce55a50a6b3 42 // Macros and Globals
williampeers 0:3ce55a50a6b3 43 //*********************************************************************************
williampeers 0:3ce55a50a6b3 44 #define CONSTRAIN(x,lower,upper) ((x)<(lower)?(lower):((x)>(upper)?(upper):(x)))
williampeers 0:3ce55a50a6b3 45
williampeers 0:3ce55a50a6b3 46 //*********************************************************************************
williampeers 0:3ce55a50a6b3 47 // Public Class Functions
williampeers 0:3ce55a50a6b3 48 //*********************************************************************************
williampeers 0:3ce55a50a6b3 49
williampeers 0:3ce55a50a6b3 50 PIDControl::
williampeers 0:3ce55a50a6b3 51 PIDControl (float kp, float ki, float kd, float sampleTimeSeconds, float minOutput,
williampeers 0:3ce55a50a6b3 52 float maxOutput, PIDMode mode, PIDDirection controllerDirection)
williampeers 0:3ce55a50a6b3 53 {
williampeers 0:3ce55a50a6b3 54 controllerDirection = controllerDirection;
williampeers 0:3ce55a50a6b3 55 mode = mode;
williampeers 0:3ce55a50a6b3 56 iTerm = 0.0f;
williampeers 0:3ce55a50a6b3 57 input = 0.0f;
williampeers 0:3ce55a50a6b3 58 lastInput = 0.0f;
williampeers 0:3ce55a50a6b3 59 output = 0.0f;
williampeers 0:3ce55a50a6b3 60 setpoint = 0.0f;
williampeers 0:3ce55a50a6b3 61
williampeers 0:3ce55a50a6b3 62 if(sampleTimeSeconds > 0.0f)
williampeers 0:3ce55a50a6b3 63 {
williampeers 0:3ce55a50a6b3 64 sampleTime = sampleTimeSeconds;
williampeers 0:3ce55a50a6b3 65 }
williampeers 0:3ce55a50a6b3 66 else
williampeers 0:3ce55a50a6b3 67 {
williampeers 0:3ce55a50a6b3 68 // If the passed parameter was incorrect, set to 1 second
williampeers 0:3ce55a50a6b3 69 sampleTime = 1.0f;
williampeers 0:3ce55a50a6b3 70 }
williampeers 0:3ce55a50a6b3 71
williampeers 0:3ce55a50a6b3 72 PIDOutputLimitsSet(minOutput, maxOutput);
williampeers 0:3ce55a50a6b3 73 PIDTuningsSet(kp, ki, kd);
williampeers 0:3ce55a50a6b3 74 }
williampeers 0:3ce55a50a6b3 75
williampeers 0:3ce55a50a6b3 76 bool PIDControl::
williampeers 0:3ce55a50a6b3 77 PIDCompute()
williampeers 0:3ce55a50a6b3 78 {
williampeers 0:3ce55a50a6b3 79 float error, dInput;
williampeers 0:3ce55a50a6b3 80
williampeers 0:3ce55a50a6b3 81 if(mode == MANUAL)
williampeers 0:3ce55a50a6b3 82 {
williampeers 0:3ce55a50a6b3 83 return false;
williampeers 0:3ce55a50a6b3 84 }
williampeers 0:3ce55a50a6b3 85
williampeers 0:3ce55a50a6b3 86 // The classic PID error term
williampeers 0:3ce55a50a6b3 87 error = setpoint - input;
williampeers 0:3ce55a50a6b3 88
williampeers 0:3ce55a50a6b3 89 // Compute the integral term separately ahead of time
williampeers 0:3ce55a50a6b3 90 iTerm += alteredKi * error;
williampeers 0:3ce55a50a6b3 91
williampeers 0:3ce55a50a6b3 92 // Constrain the integrator to make sure it does not exceed output bounds
williampeers 0:3ce55a50a6b3 93 iTerm = CONSTRAIN(iTerm, outMin, outMax);
williampeers 0:3ce55a50a6b3 94
williampeers 0:3ce55a50a6b3 95 // Take the "derivative on measurement" instead of "derivative on error"
williampeers 0:3ce55a50a6b3 96 dInput = input - lastInput;
williampeers 0:3ce55a50a6b3 97
williampeers 0:3ce55a50a6b3 98 // Run all the terms together to get the overall output
williampeers 0:3ce55a50a6b3 99 output = alteredKp * error + iTerm - alteredKd * dInput;
williampeers 0:3ce55a50a6b3 100
williampeers 0:3ce55a50a6b3 101 // Bound the output
williampeers 0:3ce55a50a6b3 102 output = CONSTRAIN(output, outMin, outMax);
williampeers 0:3ce55a50a6b3 103
williampeers 0:3ce55a50a6b3 104 // Make the current input the former input
williampeers 0:3ce55a50a6b3 105 lastInput = input;
williampeers 0:3ce55a50a6b3 106
williampeers 0:3ce55a50a6b3 107 return true;
williampeers 0:3ce55a50a6b3 108 }
williampeers 0:3ce55a50a6b3 109
williampeers 0:3ce55a50a6b3 110 void PIDControl::
williampeers 0:3ce55a50a6b3 111 PIDModeSet(PIDMode mode)
williampeers 0:3ce55a50a6b3 112 {
williampeers 0:3ce55a50a6b3 113 // If the mode changed from MANUAL to AUTOMATIC
williampeers 0:3ce55a50a6b3 114 if(mode != mode && mode == AUTOMATIC)
williampeers 0:3ce55a50a6b3 115 {
williampeers 0:3ce55a50a6b3 116 // Initialize a few PID parameters to new values
williampeers 0:3ce55a50a6b3 117 iTerm = output;
williampeers 0:3ce55a50a6b3 118 lastInput = input;
williampeers 0:3ce55a50a6b3 119
williampeers 0:3ce55a50a6b3 120 // Constrain the integrator to make sure it does not exceed output bounds
williampeers 0:3ce55a50a6b3 121 iTerm = CONSTRAIN(iTerm, outMin, outMax);
williampeers 0:3ce55a50a6b3 122 }
williampeers 0:3ce55a50a6b3 123
williampeers 0:3ce55a50a6b3 124 mode = mode;
williampeers 0:3ce55a50a6b3 125 }
williampeers 0:3ce55a50a6b3 126
williampeers 0:3ce55a50a6b3 127 void PIDControl::
williampeers 0:3ce55a50a6b3 128 PIDOutputLimitsSet(float min, float max)
williampeers 0:3ce55a50a6b3 129 {
williampeers 0:3ce55a50a6b3 130 // Check if the parameters are valid
williampeers 0:3ce55a50a6b3 131 if(min >= max)
williampeers 0:3ce55a50a6b3 132 {
williampeers 0:3ce55a50a6b3 133 return;
williampeers 0:3ce55a50a6b3 134 }
williampeers 0:3ce55a50a6b3 135
williampeers 0:3ce55a50a6b3 136 // Save the parameters
williampeers 0:3ce55a50a6b3 137 outMin = min;
williampeers 0:3ce55a50a6b3 138 outMax = max;
williampeers 0:3ce55a50a6b3 139
williampeers 0:3ce55a50a6b3 140 // If in automatic, apply the new constraints
williampeers 0:3ce55a50a6b3 141 if(mode == AUTOMATIC)
williampeers 0:3ce55a50a6b3 142 {
williampeers 0:3ce55a50a6b3 143 output = CONSTRAIN(output, min, max);
williampeers 0:3ce55a50a6b3 144 iTerm = CONSTRAIN(iTerm, min, max);
williampeers 0:3ce55a50a6b3 145 }
williampeers 0:3ce55a50a6b3 146 }
williampeers 0:3ce55a50a6b3 147
williampeers 0:3ce55a50a6b3 148 void PIDControl::
williampeers 0:3ce55a50a6b3 149 PIDTuningsSet(float kp, float ki, float kd)
williampeers 0:3ce55a50a6b3 150 {
williampeers 0:3ce55a50a6b3 151 // Check if the parameters are valid
williampeers 0:3ce55a50a6b3 152 if(kp < 0.0f || ki < 0.0f || kd < 0.0f)
williampeers 0:3ce55a50a6b3 153 {
williampeers 0:3ce55a50a6b3 154 return;
williampeers 0:3ce55a50a6b3 155 }
williampeers 0:3ce55a50a6b3 156
williampeers 0:3ce55a50a6b3 157 // Save the parameters for displaying purposes
williampeers 0:3ce55a50a6b3 158 dispKp = kp;
williampeers 0:3ce55a50a6b3 159 dispKi = ki;
williampeers 0:3ce55a50a6b3 160 dispKd = kd;
williampeers 0:3ce55a50a6b3 161
williampeers 0:3ce55a50a6b3 162 // Alter the parameters for PID
williampeers 0:3ce55a50a6b3 163 alteredKp = kp;
williampeers 0:3ce55a50a6b3 164 alteredKi = ki * sampleTime;
williampeers 0:3ce55a50a6b3 165 alteredKd = kd / sampleTime;
williampeers 0:3ce55a50a6b3 166
williampeers 0:3ce55a50a6b3 167 // Apply reverse direction to the altered values if necessary
williampeers 0:3ce55a50a6b3 168 if(controllerDirection == REVERSE)
williampeers 0:3ce55a50a6b3 169 {
williampeers 0:3ce55a50a6b3 170 alteredKp = -(alteredKp);
williampeers 0:3ce55a50a6b3 171 alteredKi = -(alteredKi);
williampeers 0:3ce55a50a6b3 172 alteredKd = -(alteredKd);
williampeers 0:3ce55a50a6b3 173 }
williampeers 0:3ce55a50a6b3 174 }
williampeers 0:3ce55a50a6b3 175
williampeers 0:3ce55a50a6b3 176 void PIDControl::
williampeers 0:3ce55a50a6b3 177 PIDTuningKpSet(float kp)
williampeers 0:3ce55a50a6b3 178 {
williampeers 0:3ce55a50a6b3 179 PIDTuningsSet(kp, dispKi, dispKd);
williampeers 0:3ce55a50a6b3 180 }
williampeers 0:3ce55a50a6b3 181
williampeers 0:3ce55a50a6b3 182 void PIDControl::
williampeers 0:3ce55a50a6b3 183 PIDTuningKiSet(float ki)
williampeers 0:3ce55a50a6b3 184 {
williampeers 0:3ce55a50a6b3 185 PIDTuningsSet(dispKp, ki, dispKd);
williampeers 0:3ce55a50a6b3 186 }
williampeers 0:3ce55a50a6b3 187
williampeers 0:3ce55a50a6b3 188 void PIDControl::
williampeers 0:3ce55a50a6b3 189 PIDTuningKdSet(float kd)
williampeers 0:3ce55a50a6b3 190 {
williampeers 0:3ce55a50a6b3 191 PIDTuningsSet(dispKp, dispKi, kd);
williampeers 0:3ce55a50a6b3 192 }
williampeers 0:3ce55a50a6b3 193
williampeers 0:3ce55a50a6b3 194 void PIDControl::
williampeers 0:3ce55a50a6b3 195 PIDControllerDirectionSet(PIDDirection controllerDirection)
williampeers 0:3ce55a50a6b3 196 {
williampeers 0:3ce55a50a6b3 197 // If in automatic mode and the controller's sense of direction is reversed
williampeers 0:3ce55a50a6b3 198 if(mode == AUTOMATIC && controllerDirection == REVERSE)
williampeers 0:3ce55a50a6b3 199 {
williampeers 0:3ce55a50a6b3 200 // Reverse sense of direction of PID gain constants
williampeers 0:3ce55a50a6b3 201 alteredKp = -(alteredKp);
williampeers 0:3ce55a50a6b3 202 alteredKi = -(alteredKi);
williampeers 0:3ce55a50a6b3 203 alteredKd = -(alteredKd);
williampeers 0:3ce55a50a6b3 204 }
williampeers 0:3ce55a50a6b3 205
williampeers 0:3ce55a50a6b3 206 controllerDirection = controllerDirection;
williampeers 0:3ce55a50a6b3 207 }
williampeers 0:3ce55a50a6b3 208
williampeers 0:3ce55a50a6b3 209 void PIDControl::
williampeers 0:3ce55a50a6b3 210 PIDSampleTimeSet(float sampleTimeSeconds)
williampeers 0:3ce55a50a6b3 211 {
williampeers 0:3ce55a50a6b3 212 float ratio;
williampeers 0:3ce55a50a6b3 213
williampeers 0:3ce55a50a6b3 214 if(sampleTimeSeconds > 0.0f)
williampeers 0:3ce55a50a6b3 215 {
williampeers 0:3ce55a50a6b3 216 // Find the ratio of change and apply to the altered values
williampeers 0:3ce55a50a6b3 217 ratio = sampleTimeSeconds / sampleTime;
williampeers 0:3ce55a50a6b3 218 alteredKi *= ratio;
williampeers 0:3ce55a50a6b3 219 alteredKd /= ratio;
williampeers 0:3ce55a50a6b3 220
williampeers 0:3ce55a50a6b3 221 // Save the new sampling time
williampeers 0:3ce55a50a6b3 222 sampleTime = sampleTimeSeconds;
williampeers 0:3ce55a50a6b3 223 }
williampeers 0:3ce55a50a6b3 224 }
williampeers 0:3ce55a50a6b3 225