Andrew Olguin / Mbed 2 deprecated RTOS_Controller_v2

Robosub controller

Dependencies:   IMU MODSERIAL Servo mbed

Fork of RTOS_Controller by Marco Rubio

PID.cpp@7:396fa2a8648d, 2016-07-29 (annotated)

Committer:
aolgu003
Date:
Fri Jul 29 15:34:59 2016 +0000
Revision:
7:396fa2a8648d
Parent:
5:07bbe020eb65 
Fixed issues and integrated with the sub.

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