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