Rogelio Vazquez / Mbed 2 deprecated Robosub_test

first publish

Dependencies:   HMC5883L mbed

PID.cpp@3:394c971eab83, 2017-06-04 (annotated)

Committer:
roger_wee
Date:
Sun Jun 04 06:58:45 2017 +0000
Revision:
3:394c971eab83
Parent:
0:ce3ac53af6e4 
9-dof implementation using madgwick's filter

Who changed what in which revision?

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