Andrew Olguin / Mbed 2 deprecated RTOS_Controller_v2

Robosub controller

Dependencies:   IMU MODSERIAL Servo mbed

Fork of RTOS_Controller by Marco Rubio

PID.cpp@4:b37fd183e46a, 2016-07-09 (annotated)

Committer:
gelmes
Date:
Sat Jul 09 20:41:49 2016 +0000
Revision:
4:b37fd183e46a
Parent:
3:5ffe7e9c0bb3
Child:
5:07bbe020eb65
Implementing Update function that controls motors

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 4:b37fd183e46a 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 4:b37fd183e46a 41
gelmes 4:b37fd183e46a 42 Serial pt(USBTX, USBRX); // tx, rx
gelmes 4:b37fd183e46a 43
gelmes 3:5ffe7e9c0bb3 44 bool PID::Compute()
gelmes 3:5ffe7e9c0bb3 45 {
gelmes 3:5ffe7e9c0bb3 46 if(!inAuto) return false;
gelmes 3:5ffe7e9c0bb3 47 unsigned long now = t.read_us();
gelmes 3:5ffe7e9c0bb3 48 unsigned long timeChange = (now - lastTime);
gelmes 3:5ffe7e9c0bb3 49
gelmes 3:5ffe7e9c0bb3 50 //cp.printf("%f, %f, %f\n\r", timeChange, SampleTime, lastTime);
gelmes 3:5ffe7e9c0bb3 51 if(timeChange>=SampleTime)
gelmes 3:5ffe7e9c0bb3 52 {
gelmes 3:5ffe7e9c0bb3 53 /*Compute all the working error variables*/
gelmes 3:5ffe7e9c0bb3 54 double input = *myInput;
gelmes 4:b37fd183e46a 55
gelmes 4:b37fd183e46a 56
gelmes 3:5ffe7e9c0bb3 57 double error = *mySetpoint - input;
gelmes 4:b37fd183e46a 58 pt.printf("pid1: %f, %f, %f, %f\n\r", error, error, *mySetpoint, input);
gelmes 3:5ffe7e9c0bb3 59 ITerm+= (ki * error);
gelmes 3:5ffe7e9c0bb3 60 if(ITerm > outMax) ITerm= outMax;
gelmes 3:5ffe7e9c0bb3 61 else if(ITerm < outMin) ITerm= outMin;
gelmes 3:5ffe7e9c0bb3 62 double dInput = (input - lastInput);
gelmes 3:5ffe7e9c0bb3 63
gelmes 3:5ffe7e9c0bb3 64 /*Compute PID Output*/
gelmes 3:5ffe7e9c0bb3 65 double output = kp * error + ITerm- kd * dInput;
gelmes 3:5ffe7e9c0bb3 66
gelmes 4:b37fd183e46a 67 pt.printf("pid2: %f, %f, %f, %f, %f\n\r", error, output, *mySetpoint, input, ITerm);
gelmes 4:b37fd183e46a 68
gelmes 3:5ffe7e9c0bb3 69 if(output > outMax) output = outMax;
gelmes 3:5ffe7e9c0bb3 70 else if(output < outMin) output = outMin;
gelmes 3:5ffe7e9c0bb3 71 *myOutput = output;
gelmes 3:5ffe7e9c0bb3 72
gelmes 4:b37fd183e46a 73 pt.printf("pid3: %f, %f, %f, %f, %f\n\r", error, output, *mySetpoint, input, ki);
gelmes 4:b37fd183e46a 74
gelmes 3:5ffe7e9c0bb3 75 /*Remember some variables for next time*/
gelmes 3:5ffe7e9c0bb3 76 lastInput = input;
gelmes 3:5ffe7e9c0bb3 77 lastTime = now;
gelmes 3:5ffe7e9c0bb3 78 return true;
gelmes 3:5ffe7e9c0bb3 79 }
gelmes 3:5ffe7e9c0bb3 80 else return false;
gelmes 3:5ffe7e9c0bb3 81 }
gelmes 3:5ffe7e9c0bb3 82
gelmes 3:5ffe7e9c0bb3 83
gelmes 3:5ffe7e9c0bb3 84 /* SetTunings(...)*************************************************************
gelmes 3:5ffe7e9c0bb3 85 * This function allows the controller's dynamic performance to be adjusted.
gelmes 3:5ffe7e9c0bb3 86 * it's called automatically from the constructor, but tunings can also
gelmes 3:5ffe7e9c0bb3 87 * be adjusted on the fly during normal operation
gelmes 3:5ffe7e9c0bb3 88 ******************************************************************************/
gelmes 3:5ffe7e9c0bb3 89 void PID::SetTunings(double Kp, double Ki, double Kd)
gelmes 3:5ffe7e9c0bb3 90 {
gelmes 3:5ffe7e9c0bb3 91 if (Kp<0 || Ki<0 || Kd<0) return;
gelmes 3:5ffe7e9c0bb3 92
gelmes 3:5ffe7e9c0bb3 93 dispKp = Kp; dispKi = Ki; dispKd = Kd;
gelmes 3:5ffe7e9c0bb3 94
gelmes 3:5ffe7e9c0bb3 95 double SampleTimeInSec = ((double)SampleTime)/1000;
gelmes 3:5ffe7e9c0bb3 96 kp = Kp;
gelmes 3:5ffe7e9c0bb3 97 ki = Ki * SampleTimeInSec;
gelmes 3:5ffe7e9c0bb3 98 kd = Kd / SampleTimeInSec;
gelmes 3:5ffe7e9c0bb3 99
gelmes 3:5ffe7e9c0bb3 100 if(controllerDirection ==REVERSE)
gelmes 3:5ffe7e9c0bb3 101 {
gelmes 3:5ffe7e9c0bb3 102 kp = (0 - kp);
gelmes 3:5ffe7e9c0bb3 103 ki = (0 - ki);
gelmes 3:5ffe7e9c0bb3 104 kd = (0 - kd);
gelmes 3:5ffe7e9c0bb3 105 }
gelmes 4:b37fd183e46a 106 ITerm = 0.0;
gelmes 3:5ffe7e9c0bb3 107 }
gelmes 3:5ffe7e9c0bb3 108
gelmes 3:5ffe7e9c0bb3 109 /* SetSampleTime(...) *********************************************************
gelmes 3:5ffe7e9c0bb3 110 * sets the period, in Milliseconds, at which the calculation is performed
gelmes 3:5ffe7e9c0bb3 111 ******************************************************************************/
gelmes 3:5ffe7e9c0bb3 112 void PID::SetSampleTime(int NewSampleTime)
gelmes 3:5ffe7e9c0bb3 113 {
gelmes 3:5ffe7e9c0bb3 114 if (NewSampleTime > 0)
gelmes 3:5ffe7e9c0bb3 115 {
gelmes 3:5ffe7e9c0bb3 116 double ratio = (double)NewSampleTime
gelmes 3:5ffe7e9c0bb3 117 / (double)SampleTime;
gelmes 3:5ffe7e9c0bb3 118 ki *= ratio;
gelmes 3:5ffe7e9c0bb3 119 kd /= ratio;
gelmes 3:5ffe7e9c0bb3 120 SampleTime = (unsigned long)NewSampleTime;
gelmes 3:5ffe7e9c0bb3 121 }
gelmes 3:5ffe7e9c0bb3 122 }
gelmes 3:5ffe7e9c0bb3 123
gelmes 3:5ffe7e9c0bb3 124 /* SetOutputLimits(...)****************************************************
gelmes 3:5ffe7e9c0bb3 125 * This function will be used far more often than SetInputLimits. while
gelmes 3:5ffe7e9c0bb3 126 * the input to the controller will generally be in the 0-1023 range (which is
gelmes 3:5ffe7e9c0bb3 127 * the default already,) the output will be a little different. maybe they'll
gelmes 3:5ffe7e9c0bb3 128 * be doing a time window and will need 0-8000 or something. or maybe they'll
gelmes 3:5ffe7e9c0bb3 129 * want to clamp it from 0-125. who knows. at any rate, that can all be done
gelmes 3:5ffe7e9c0bb3 130 * here.
gelmes 3:5ffe7e9c0bb3 131 **************************************************************************/
gelmes 3:5ffe7e9c0bb3 132 void PID::SetOutputLimits(double Min, double Max)
gelmes 3:5ffe7e9c0bb3 133 {
gelmes 3:5ffe7e9c0bb3 134 if(Min >= Max) return;
gelmes 3:5ffe7e9c0bb3 135 outMin = Min;
gelmes 3:5ffe7e9c0bb3 136 outMax = Max;
gelmes 3:5ffe7e9c0bb3 137
gelmes 3:5ffe7e9c0bb3 138 if(inAuto)
gelmes 3:5ffe7e9c0bb3 139 {
gelmes 3:5ffe7e9c0bb3 140 if(*myOutput > outMax) *myOutput = outMax;
gelmes 3:5ffe7e9c0bb3 141 else if(*myOutput < outMin) *myOutput = outMin;
gelmes 3:5ffe7e9c0bb3 142
gelmes 3:5ffe7e9c0bb3 143 if(ITerm > outMax) ITerm= outMax;
gelmes 3:5ffe7e9c0bb3 144 else if(ITerm < outMin) ITerm= outMin;
gelmes 3:5ffe7e9c0bb3 145 }
gelmes 3:5ffe7e9c0bb3 146 }
gelmes 3:5ffe7e9c0bb3 147
gelmes 3:5ffe7e9c0bb3 148 /* SetMode(...)****************************************************************
gelmes 3:5ffe7e9c0bb3 149 * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
gelmes 3:5ffe7e9c0bb3 150 * when the transition from manual to auto occurs, the controller is
gelmes 3:5ffe7e9c0bb3 151 * automatically initialized
gelmes 3:5ffe7e9c0bb3 152 ******************************************************************************/
gelmes 3:5ffe7e9c0bb3 153 void PID::SetMode(int Mode)
gelmes 3:5ffe7e9c0bb3 154 {
gelmes 3:5ffe7e9c0bb3 155 bool newAuto = (Mode == AUTOMATIC);
gelmes 3:5ffe7e9c0bb3 156 if(newAuto == !inAuto)
gelmes 3:5ffe7e9c0bb3 157 { /*we just went from manual to auto*/
gelmes 3:5ffe7e9c0bb3 158 PID::Initialize();
gelmes 3:5ffe7e9c0bb3 159 }
gelmes 3:5ffe7e9c0bb3 160 inAuto = newAuto;
gelmes 3:5ffe7e9c0bb3 161 }
gelmes 3:5ffe7e9c0bb3 162
gelmes 3:5ffe7e9c0bb3 163 /* Initialize()****************************************************************
gelmes 3:5ffe7e9c0bb3 164 * does all the things that need to happen to ensure a bumpless transfer
gelmes 3:5ffe7e9c0bb3 165 * from manual to automatic mode.
gelmes 3:5ffe7e9c0bb3 166 ******************************************************************************/
gelmes 3:5ffe7e9c0bb3 167 void PID::Initialize()
gelmes 3:5ffe7e9c0bb3 168 {
gelmes 3:5ffe7e9c0bb3 169 ITerm = *myOutput;
gelmes 3:5ffe7e9c0bb3 170 lastInput = *myInput;
gelmes 3:5ffe7e9c0bb3 171 t.start();
gelmes 3:5ffe7e9c0bb3 172 if(ITerm > outMax) ITerm = outMax;
gelmes 3:5ffe7e9c0bb3 173 else if(ITerm < outMin) ITerm = outMin;
gelmes 3:5ffe7e9c0bb3 174 }
gelmes 3:5ffe7e9c0bb3 175
gelmes 3:5ffe7e9c0bb3 176 /* SetControllerDirection(...)*************************************************
gelmes 3:5ffe7e9c0bb3 177 * The PID will either be connected to a DIRECT acting process (+Output leads
gelmes 3:5ffe7e9c0bb3 178 * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to
gelmes 3:5ffe7e9c0bb3 179 * know which one, because otherwise we may increase the output when we should
gelmes 3:5ffe7e9c0bb3 180 * be decreasing. This is called from the constructor.
gelmes 3:5ffe7e9c0bb3 181 ******************************************************************************/
gelmes 3:5ffe7e9c0bb3 182 void PID::SetControllerDirection(int Direction)
gelmes 3:5ffe7e9c0bb3 183 {
gelmes 3:5ffe7e9c0bb3 184 if(inAuto && Direction !=controllerDirection)
gelmes 3:5ffe7e9c0bb3 185 {
gelmes 3:5ffe7e9c0bb3 186 kp = (0 - kp);
gelmes 3:5ffe7e9c0bb3 187 ki = (0 - ki);
gelmes 3:5ffe7e9c0bb3 188 kd = (0 - kd);
gelmes 3:5ffe7e9c0bb3 189 }
gelmes 3:5ffe7e9c0bb3 190 controllerDirection = Direction;
gelmes 3:5ffe7e9c0bb3 191 }
gelmes 3:5ffe7e9c0bb3 192
gelmes 3:5ffe7e9c0bb3 193 /* Status Funcions*************************************************************
gelmes 3:5ffe7e9c0bb3 194 * Just because you set the Kp=-1 doesn't mean it actually happened. these
gelmes 3:5ffe7e9c0bb3 195 * functions query the internal state of the PID. they're here for display
gelmes 3:5ffe7e9c0bb3 196 * purposes. this are the functions the PID Front-end uses for example
gelmes 3:5ffe7e9c0bb3 197 ******************************************************************************/
gelmes 3:5ffe7e9c0bb3 198 double PID::GetKp(){ return dispKp; }
gelmes 3:5ffe7e9c0bb3 199 double PID::GetKi(){ return dispKi;}
gelmes 3:5ffe7e9c0bb3 200 double PID::GetKd(){ return dispKd;}
gelmes 3:5ffe7e9c0bb3 201 int PID::GetMode(){ return inAuto ? AUTOMATIC : MANUAL;}
gelmes 3:5ffe7e9c0bb3 202 int PID::GetDirection(){ return controllerDirection;}
gelmes 3:5ffe7e9c0bb3 203
gelmes 3:5ffe7e9c0bb3 204