Rogelio Vazquez / Mbed 2 deprecated Control_interface

UCR Robosub manual control / PID tuning interface

Dependencies:   mbed HMC5883L

PID.cpp@1:3f291f2f80d3, 2017-07-27 (annotated)

Committer:
roger_wee
Date:
Thu Jul 27 05:45:10 2017 +0000
Revision:
1:3f291f2f80d3
Parent:
0:048a74dd7c3a 
control edit

Who changed what in which revision?

UserRevisionLine numberNew 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;}