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