ryan lin / pid

ported pid library from imu

Dependents:   wheelchaircontrol wheelchaircontrolRos

PID_v1.cpp@0:539e3b3b91e1, 2018-08-09 (annotated)

Committer:
ryanlin97
Date:
Thu Aug 09 16:36:32 2018 +0000
Revision:
0:539e3b3b91e1
Child:
1:45383b49536c
initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:539e3b3b91e1 1 /**********************************************************************************************
ryanlin97 0:539e3b3b91e1 2 * Arduino PID Library - Version 1.2.1
ryanlin97 0:539e3b3b91e1 3 * by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
ryanlin97 0:539e3b3b91e1 4 *
ryanlin97 0:539e3b3b91e1 5 * This Library is licensed under the MIT License
ryanlin97 0:539e3b3b91e1 6 **********************************************************************************************/
ryanlin97 0:539e3b3b91e1 7 /*
ryanlin97 0:539e3b3b91e1 8 #if ARDUINO >= 100
ryanlin97 0:539e3b3b91e1 9 #include "Arduino.h"
ryanlin97 0:539e3b3b91e1 10 #else
ryanlin97 0:539e3b3b91e1 11 #include "WProgram.h"
ryanlin97 0:539e3b3b91e1 12 #endif*/
ryanlin97 0:539e3b3b91e1 13
ryanlin97 0:539e3b3b91e1 14 #include "mbed.h"
ryanlin97 0:539e3b3b91e1 15 #include "PID_v1.h"
ryanlin97 0:539e3b3b91e1 16
ryanlin97 0:539e3b3b91e1 17 Timer PIDtimer;
ryanlin97 0:539e3b3b91e1 18 /*Constructor (...)*********************************************************
ryanlin97 0:539e3b3b91e1 19 * The parameters specified here are those for for which we can't set up
ryanlin97 0:539e3b3b91e1 20 * reliable defaults, so we need to have the user set them.
ryanlin97 0:539e3b3b91e1 21 ***************************************************************************/
ryanlin97 0:539e3b3b91e1 22 PID::PID(double* Input, double* Output, double* Setpoint,
ryanlin97 0:539e3b3b91e1 23 double Kp, double Ki, double Kd, int POn, int ControllerDirection)
ryanlin97 0:539e3b3b91e1 24 {
ryanlin97 0:539e3b3b91e1 25 myOutput = Output;
ryanlin97 0:539e3b3b91e1 26 myInput = Input;
ryanlin97 0:539e3b3b91e1 27 mySetpoint = Setpoint;
ryanlin97 0:539e3b3b91e1 28 inAuto = false;
ryanlin97 0:539e3b3b91e1 29 PIDtimer.start();
ryanlin97 0:539e3b3b91e1 30 PID::SetOutputLimits(0, 1); //default output limit corresponds to
ryanlin97 0:539e3b3b91e1 31 //the arduino pwm limits
ryanlin97 0:539e3b3b91e1 32
ryanlin97 0:539e3b3b91e1 33 SampleTime = 100; //default Controller Sample Time is 0.1 seconds
ryanlin97 0:539e3b3b91e1 34
ryanlin97 0:539e3b3b91e1 35 PID::SetControllerDirection(ControllerDirection);
ryanlin97 0:539e3b3b91e1 36 PID::SetTunings(Kp, Ki, Kd, POn);
ryanlin97 0:539e3b3b91e1 37
ryanlin97 0:539e3b3b91e1 38 lastTime = PIDtimer.read()-SampleTime;
ryanlin97 0:539e3b3b91e1 39 printf("input %f output %f pOn %i Setpoint %f\n", *myInput, *myOutput, pOn, *mySetpoint);
ryanlin97 0:539e3b3b91e1 40 }
ryanlin97 0:539e3b3b91e1 41
ryanlin97 0:539e3b3b91e1 42 /*Constructor (...)*********************************************************
ryanlin97 0:539e3b3b91e1 43 * To allow backwards compatability for v1.1, or for people that just want
ryanlin97 0:539e3b3b91e1 44 * to use Proportional on Error without explicitly saying so
ryanlin97 0:539e3b3b91e1 45 ***************************************************************************/
ryanlin97 0:539e3b3b91e1 46
ryanlin97 0:539e3b3b91e1 47 PID::PID(double* Input, double* Output, double* Setpoint,
ryanlin97 0:539e3b3b91e1 48 double Kp, double Ki, double Kd, int ControllerDirection)
ryanlin97 0:539e3b3b91e1 49 {
ryanlin97 0:539e3b3b91e1 50 PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection);
ryanlin97 0:539e3b3b91e1 51 }
ryanlin97 0:539e3b3b91e1 52
ryanlin97 0:539e3b3b91e1 53
ryanlin97 0:539e3b3b91e1 54 /* Compute() **********************************************************************
ryanlin97 0:539e3b3b91e1 55 * This, as they say, is where the magic happens. this function should be called
ryanlin97 0:539e3b3b91e1 56 * every time "void loop()" executes. the function will decide for itself whether a new
ryanlin97 0:539e3b3b91e1 57 * pid Output needs to be computed. returns true when the output is computed,
ryanlin97 0:539e3b3b91e1 58 * false when nothing has been done.
ryanlin97 0:539e3b3b91e1 59 **********************************************************************************/
ryanlin97 0:539e3b3b91e1 60 bool PID::Compute()
ryanlin97 0:539e3b3b91e1 61 {
ryanlin97 0:539e3b3b91e1 62 printf("input %f output %f pOn %i Setpoint %f outmin %f outmax %f \n", *myInput, *myOutput, pOn, *mySetpoint, outMin, outMax);
ryanlin97 0:539e3b3b91e1 63 if(!inAuto) return false;
ryanlin97 0:539e3b3b91e1 64 unsigned long now = PIDtimer.read();
ryanlin97 0:539e3b3b91e1 65 unsigned long timeChange = (now - lastTime);
ryanlin97 0:539e3b3b91e1 66 if(timeChange>=SampleTime)
ryanlin97 0:539e3b3b91e1 67 {
ryanlin97 0:539e3b3b91e1 68 /*Compute all the working error variables*/
ryanlin97 0:539e3b3b91e1 69 double input = *myInput;
ryanlin97 0:539e3b3b91e1 70 double error = *mySetpoint - input;
ryanlin97 0:539e3b3b91e1 71 double dInput = (input - lastInput);
ryanlin97 0:539e3b3b91e1 72 outputSum+= (ki * error);
ryanlin97 0:539e3b3b91e1 73
ryanlin97 0:539e3b3b91e1 74 /*Add Proportional on Measurement, if P_ON_M is specified*/
ryanlin97 0:539e3b3b91e1 75 if(!pOnE) outputSum-= kp * dInput;
ryanlin97 0:539e3b3b91e1 76
ryanlin97 0:539e3b3b91e1 77 if(outputSum > outMax) outputSum= outMax;
ryanlin97 0:539e3b3b91e1 78 else if(outputSum < outMin) outputSum= outMin;
ryanlin97 0:539e3b3b91e1 79
ryanlin97 0:539e3b3b91e1 80 /*Add Proportional on Error, if P_ON_E is specified*/
ryanlin97 0:539e3b3b91e1 81 double output;
ryanlin97 0:539e3b3b91e1 82 if(pOnE) output = kp * error;
ryanlin97 0:539e3b3b91e1 83 else output = 0;
ryanlin97 0:539e3b3b91e1 84
ryanlin97 0:539e3b3b91e1 85 /*Compute Rest of PID Output*/
ryanlin97 0:539e3b3b91e1 86 output += outputSum - kd * dInput;
ryanlin97 0:539e3b3b91e1 87
ryanlin97 0:539e3b3b91e1 88 if(output > outMax) output = outMax;
ryanlin97 0:539e3b3b91e1 89 else if(output < outMin) output = outMin;
ryanlin97 0:539e3b3b91e1 90 *myOutput = output;
ryanlin97 0:539e3b3b91e1 91
ryanlin97 0:539e3b3b91e1 92 /*Remember some variables for next time*/
ryanlin97 0:539e3b3b91e1 93 lastInput = input;
ryanlin97 0:539e3b3b91e1 94 lastTime = now;
ryanlin97 0:539e3b3b91e1 95 return true;
ryanlin97 0:539e3b3b91e1 96 }
ryanlin97 0:539e3b3b91e1 97 else return false;
ryanlin97 0:539e3b3b91e1 98 }
ryanlin97 0:539e3b3b91e1 99
ryanlin97 0:539e3b3b91e1 100 /* SetTunings(...)*************************************************************
ryanlin97 0:539e3b3b91e1 101 * This function allows the controller's dynamic performance to be adjusted.
ryanlin97 0:539e3b3b91e1 102 * it's called automatically from the constructor, but tunings can also
ryanlin97 0:539e3b3b91e1 103 * be adjusted on the fly during normal operation
ryanlin97 0:539e3b3b91e1 104 ******************************************************************************/
ryanlin97 0:539e3b3b91e1 105 void PID::SetTunings(double Kp, double Ki, double Kd, int POn)
ryanlin97 0:539e3b3b91e1 106 {
ryanlin97 0:539e3b3b91e1 107 if (Kp<0 || Ki<0 || Kd<0) return;
ryanlin97 0:539e3b3b91e1 108
ryanlin97 0:539e3b3b91e1 109 pOn = POn;
ryanlin97 0:539e3b3b91e1 110 pOnE = POn == P_ON_E;
ryanlin97 0:539e3b3b91e1 111
ryanlin97 0:539e3b3b91e1 112 dispKp = Kp; dispKi = Ki; dispKd = Kd;
ryanlin97 0:539e3b3b91e1 113
ryanlin97 0:539e3b3b91e1 114 double SampleTimeInSec = ((double)SampleTime)/1000;
ryanlin97 0:539e3b3b91e1 115 kp = Kp;
ryanlin97 0:539e3b3b91e1 116 ki = Ki * SampleTimeInSec;
ryanlin97 0:539e3b3b91e1 117 kd = Kd / SampleTimeInSec;
ryanlin97 0:539e3b3b91e1 118
ryanlin97 0:539e3b3b91e1 119 if(controllerDirection ==REVERSE)
ryanlin97 0:539e3b3b91e1 120 {
ryanlin97 0:539e3b3b91e1 121 kp = (0 - kp);
ryanlin97 0:539e3b3b91e1 122 ki = (0 - ki);
ryanlin97 0:539e3b3b91e1 123 kd = (0 - kd);
ryanlin97 0:539e3b3b91e1 124 }
ryanlin97 0:539e3b3b91e1 125 }
ryanlin97 0:539e3b3b91e1 126
ryanlin97 0:539e3b3b91e1 127 /* SetTunings(...)*************************************************************
ryanlin97 0:539e3b3b91e1 128 * Set Tunings using the last-rembered POn setting
ryanlin97 0:539e3b3b91e1 129 ******************************************************************************/
ryanlin97 0:539e3b3b91e1 130 void PID::SetTunings(double Kp, double Ki, double Kd){
ryanlin97 0:539e3b3b91e1 131 SetTunings(Kp, Ki, Kd, pOn);
ryanlin97 0:539e3b3b91e1 132 }
ryanlin97 0:539e3b3b91e1 133
ryanlin97 0:539e3b3b91e1 134 /* SetSampleTime(...) *********************************************************
ryanlin97 0:539e3b3b91e1 135 * sets the period, in Milliseconds, at which the calculation is performed
ryanlin97 0:539e3b3b91e1 136 ******************************************************************************/
ryanlin97 0:539e3b3b91e1 137 void PID::SetSampleTime(int NewSampleTime)
ryanlin97 0:539e3b3b91e1 138 {
ryanlin97 0:539e3b3b91e1 139 if (NewSampleTime > 0)
ryanlin97 0:539e3b3b91e1 140 {
ryanlin97 0:539e3b3b91e1 141 double ratio = (double)NewSampleTime
ryanlin97 0:539e3b3b91e1 142 / (double)SampleTime;
ryanlin97 0:539e3b3b91e1 143 ki *= ratio;
ryanlin97 0:539e3b3b91e1 144 kd /= ratio;
ryanlin97 0:539e3b3b91e1 145 SampleTime = (unsigned long)NewSampleTime;
ryanlin97 0:539e3b3b91e1 146 }
ryanlin97 0:539e3b3b91e1 147 }
ryanlin97 0:539e3b3b91e1 148
ryanlin97 0:539e3b3b91e1 149 /* SetOutputLimits(...)****************************************************
ryanlin97 0:539e3b3b91e1 150 * This function will be used far more often than SetInputLimits. while
ryanlin97 0:539e3b3b91e1 151 * the input to the controller will generally be in the 0-1023 range (which is
ryanlin97 0:539e3b3b91e1 152 * the default already,) the output will be a little different. maybe they'll
ryanlin97 0:539e3b3b91e1 153 * be doing a time window and will need 0-8000 or something. or maybe they'll
ryanlin97 0:539e3b3b91e1 154 * want to clamp it from 0-125. who knows. at any rate, that can all be done
ryanlin97 0:539e3b3b91e1 155 * here.
ryanlin97 0:539e3b3b91e1 156 **************************************************************************/
ryanlin97 0:539e3b3b91e1 157 void PID::SetOutputLimits(double Min, double Max)
ryanlin97 0:539e3b3b91e1 158 {
ryanlin97 0:539e3b3b91e1 159 if(Min >= Max) return;
ryanlin97 0:539e3b3b91e1 160 outMin = Min;
ryanlin97 0:539e3b3b91e1 161 outMax = Max;
ryanlin97 0:539e3b3b91e1 162
ryanlin97 0:539e3b3b91e1 163 if(inAuto)
ryanlin97 0:539e3b3b91e1 164 {
ryanlin97 0:539e3b3b91e1 165 if(*myOutput > outMax) *myOutput = outMax;
ryanlin97 0:539e3b3b91e1 166 else if(*myOutput < outMin) *myOutput = outMin;
ryanlin97 0:539e3b3b91e1 167
ryanlin97 0:539e3b3b91e1 168 if(outputSum > outMax) outputSum= outMax;
ryanlin97 0:539e3b3b91e1 169 else if(outputSum < outMin) outputSum= outMin;
ryanlin97 0:539e3b3b91e1 170 }
ryanlin97 0:539e3b3b91e1 171 }
ryanlin97 0:539e3b3b91e1 172
ryanlin97 0:539e3b3b91e1 173 /* SetMode(...)****************************************************************
ryanlin97 0:539e3b3b91e1 174 * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
ryanlin97 0:539e3b3b91e1 175 * when the transition from manual to auto occurs, the controller is
ryanlin97 0:539e3b3b91e1 176 * automatically initialized
ryanlin97 0:539e3b3b91e1 177 ******************************************************************************/
ryanlin97 0:539e3b3b91e1 178 void PID::SetMode(int Mode)
ryanlin97 0:539e3b3b91e1 179 {
ryanlin97 0:539e3b3b91e1 180 bool newAuto = (Mode == AUTOMATIC);
ryanlin97 0:539e3b3b91e1 181 if(newAuto && !inAuto)
ryanlin97 0:539e3b3b91e1 182 { /*we just went from manual to auto*/
ryanlin97 0:539e3b3b91e1 183 PID::Initialize();
ryanlin97 0:539e3b3b91e1 184 }
ryanlin97 0:539e3b3b91e1 185 inAuto = newAuto;
ryanlin97 0:539e3b3b91e1 186 }
ryanlin97 0:539e3b3b91e1 187
ryanlin97 0:539e3b3b91e1 188 /* Initialize()****************************************************************
ryanlin97 0:539e3b3b91e1 189 * does all the things that need to happen to ensure a bumpless transfer
ryanlin97 0:539e3b3b91e1 190 * from manual to automatic mode.
ryanlin97 0:539e3b3b91e1 191 ******************************************************************************/
ryanlin97 0:539e3b3b91e1 192 void PID::Initialize()
ryanlin97 0:539e3b3b91e1 193 {
ryanlin97 0:539e3b3b91e1 194 outputSum = *myOutput;
ryanlin97 0:539e3b3b91e1 195 lastInput = *myInput;
ryanlin97 0:539e3b3b91e1 196 if(outputSum > outMax) outputSum = outMax;
ryanlin97 0:539e3b3b91e1 197 else if(outputSum < outMin) outputSum = outMin;
ryanlin97 0:539e3b3b91e1 198 }
ryanlin97 0:539e3b3b91e1 199
ryanlin97 0:539e3b3b91e1 200 /* SetControllerDirection(...)*************************************************
ryanlin97 0:539e3b3b91e1 201 * The PID will either be connected to a DIRECT acting process (+Output leads
ryanlin97 0:539e3b3b91e1 202 * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to
ryanlin97 0:539e3b3b91e1 203 * know which one, because otherwise we may increase the output when we should
ryanlin97 0:539e3b3b91e1 204 * be decreasing. This is called from the constructor.
ryanlin97 0:539e3b3b91e1 205 ******************************************************************************/
ryanlin97 0:539e3b3b91e1 206 void PID::SetControllerDirection(int Direction)
ryanlin97 0:539e3b3b91e1 207 {
ryanlin97 0:539e3b3b91e1 208 if(inAuto && Direction !=controllerDirection)
ryanlin97 0:539e3b3b91e1 209 {
ryanlin97 0:539e3b3b91e1 210 kp = (0 - kp);
ryanlin97 0:539e3b3b91e1 211 ki = (0 - ki);
ryanlin97 0:539e3b3b91e1 212 kd = (0 - kd);
ryanlin97 0:539e3b3b91e1 213 }
ryanlin97 0:539e3b3b91e1 214 controllerDirection = Direction;
ryanlin97 0:539e3b3b91e1 215 }
ryanlin97 0:539e3b3b91e1 216
ryanlin97 0:539e3b3b91e1 217 /* Status Funcions*************************************************************
ryanlin97 0:539e3b3b91e1 218 * Just because you set the Kp=-1 doesn't mean it actually happened. these
ryanlin97 0:539e3b3b91e1 219 * functions query the internal state of the PID. they're here for display
ryanlin97 0:539e3b3b91e1 220 * purposes. this are the functions the PID Front-end uses for example
ryanlin97 0:539e3b3b91e1 221 ******************************************************************************/
ryanlin97 0:539e3b3b91e1 222 double PID::GetKp(){ return dispKp; }
ryanlin97 0:539e3b3b91e1 223 double PID::GetKi(){ return dispKi;}
ryanlin97 0:539e3b3b91e1 224 double PID::GetKd(){ return dispKd;}
ryanlin97 0:539e3b3b91e1 225 int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;}
ryanlin97 0:539e3b3b91e1 226 int PID::GetDirection(){ return controllerDirection;}
ryanlin97 0:539e3b3b91e1 227
ryanlin97 0:539e3b3b91e1 228 /*void printEverything() {
ryanlin97 0:539e3b3b91e1 229 printf("input %f output %f pOn %f Setpoint %f outmin %f outmax %f \n", *myInput, *myOutput, pOn, *mySetpoint, outMin, outMax);
ryanlin97 0:539e3b3b91e1 230 printf("KP %f KI %f Kd %f \n", PID::GetKp(), PID::GetKi(), PID::GetKd());
ryanlin97 0:539e3b3b91e1 231 }
ryanlin97 0:539e3b3b91e1 232 */