Andrew Olguin / Mbed 2 deprecated RTOS_Controller_v2

Robosub controller

Dependencies:   IMU MODSERIAL Servo mbed

Fork of RTOS_Controller by Marco Rubio

PID.cpp@3:5ffe7e9c0bb3, 2016-07-04 (annotated)

Committer:
gelmes
Date:
Mon Jul 04 18:56:23 2016 +0000
Revision:
3:5ffe7e9c0bb3
Child:
4:b37fd183e46a
Quaternions are holding me back

Who changed what in which revision?

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