Amr El-Gendy / Mbed 2 deprecated QUAD_PROJECT

a quadcopter code

Dependencies:   Pulse RangeFinder mbed

PID.cpp@0:4a55d0a21ea9, 2015-11-24 (annotated)

Committer:
Gendy
Date:
Tue Nov 24 19:57:06 2015 +0000
Revision:
0:4a55d0a21ea9
Quad_project PID on angle;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gendy 0:4a55d0a21ea9 1 #include <PID.h>
Gendy 0:4a55d0a21ea9 2 #include <stdlib.h>
Gendy 0:4a55d0a21ea9 3 #include <math.h>
Gendy 0:4a55d0a21ea9 4 /*Constructor (...)*********************************************************
Gendy 0:4a55d0a21ea9 5 * The parameters specified here are those for for which we can't set up
Gendy 0:4a55d0a21ea9 6 * reliable defaults, so we need to have the user set them.
Gendy 0:4a55d0a21ea9 7 ***************************************************************************/
Gendy 0:4a55d0a21ea9 8 PID::PID(float* Input, float* Output, float* Setpoint,
Gendy 0:4a55d0a21ea9 9 float Kp, float Ki, float Kd)
Gendy 0:4a55d0a21ea9 10 {
Gendy 0:4a55d0a21ea9 11
Gendy 0:4a55d0a21ea9 12 myOutput = Output;
Gendy 0:4a55d0a21ea9 13 myInput = Input;
Gendy 0:4a55d0a21ea9 14 mySetpoint = Setpoint;
Gendy 0:4a55d0a21ea9 15
Gendy 0:4a55d0a21ea9 16
Gendy 0:4a55d0a21ea9 17 PID::SetOutputLimits(-1000,1000); //default output limit corresponds to
Gendy 0:4a55d0a21ea9 18 //the arduino pwm limits
Gendy 0:4a55d0a21ea9 19 PID::SetTunings(Kp, Ki, Kd);
Gendy 0:4a55d0a21ea9 20 //PID::SetIntegratorLimits(399);
Gendy 0:4a55d0a21ea9 21
Gendy 0:4a55d0a21ea9 22
Gendy 0:4a55d0a21ea9 23
Gendy 0:4a55d0a21ea9 24 }
Gendy 0:4a55d0a21ea9 25
Gendy 0:4a55d0a21ea9 26
Gendy 0:4a55d0a21ea9 27 /* Compute() **********************************************************************
Gendy 0:4a55d0a21ea9 28 * This, as they say, is where the magic happens. this function should be called
Gendy 0:4a55d0a21ea9 29 * every time "void loop()" executes. the function will decide for itself whether a new
Gendy 0:4a55d0a21ea9 30 * pid Output needs to be computed. returns true when the output is computed,
Gendy 0:4a55d0a21ea9 31 * false when nothing has been done.
Gendy 0:4a55d0a21ea9 32 **********************************************************************************/
Gendy 0:4a55d0a21ea9 33 void PID::Compute()
Gendy 0:4a55d0a21ea9 34 {
Gendy 0:4a55d0a21ea9 35
Gendy 0:4a55d0a21ea9 36 float input = *myInput;
Gendy 0:4a55d0a21ea9 37 float error =(*mySetpoint)-input;
Gendy 0:4a55d0a21ea9 38
Gendy 0:4a55d0a21ea9 39 ITerm+= (error+lasterror)/2;
Gendy 0:4a55d0a21ea9 40 /*
Gendy 0:4a55d0a21ea9 41 if(!ki==0){
Gendy 0:4a55d0a21ea9 42
Gendy 0:4a55d0a21ea9 43 if(ITerm < (-abs(max/ki))){
Gendy 0:4a55d0a21ea9 44 ITerm= (-abs(max/ki));
Gendy 0:4a55d0a21ea9 45 }
Gendy 0:4a55d0a21ea9 46 else if(ITerm > abs(max/ki)){
Gendy 0:4a55d0a21ea9 47 ITerm= abs(max/ki);
Gendy 0:4a55d0a21ea9 48 }
Gendy 0:4a55d0a21ea9 49 }
Gendy 0:4a55d0a21ea9 50 */
Gendy 0:4a55d0a21ea9 51 float dInput = (error - lasterror);
Gendy 0:4a55d0a21ea9 52
Gendy 0:4a55d0a21ea9 53
Gendy 0:4a55d0a21ea9 54 /*Compute PID Output*/
Gendy 0:4a55d0a21ea9 55 float output = (kp * error )+(ki * (ITerm))+(kd * (dInput));
Gendy 0:4a55d0a21ea9 56 if(output > outMax) output = outMax;
Gendy 0:4a55d0a21ea9 57 else if(output < outMin) output = outMin;
Gendy 0:4a55d0a21ea9 58 *myOutput =output;
Gendy 0:4a55d0a21ea9 59
Gendy 0:4a55d0a21ea9 60 /*Remember some variables for next time*/
Gendy 0:4a55d0a21ea9 61 lasterror=error;
Gendy 0:4a55d0a21ea9 62
Gendy 0:4a55d0a21ea9 63
Gendy 0:4a55d0a21ea9 64 }
Gendy 0:4a55d0a21ea9 65
Gendy 0:4a55d0a21ea9 66
Gendy 0:4a55d0a21ea9 67 /* SetTunings(...)*************************************************************
Gendy 0:4a55d0a21ea9 68 * This function allows the controller's dynamic performance to be adjusted.
Gendy 0:4a55d0a21ea9 69 * it's called automatically from the constructor, but tunings can also
Gendy 0:4a55d0a21ea9 70 * be adjusted on the fly during normal operation
Gendy 0:4a55d0a21ea9 71 ******************************************************************************/
Gendy 0:4a55d0a21ea9 72 void PID::SetTunings(float Kp, float Ki, float Kd)
Gendy 0:4a55d0a21ea9 73 {
Gendy 0:4a55d0a21ea9 74 kp = Kp;
Gendy 0:4a55d0a21ea9 75 ki = Ki;
Gendy 0:4a55d0a21ea9 76 kd = Kd;
Gendy 0:4a55d0a21ea9 77 //lasterror=0;
Gendy 0:4a55d0a21ea9 78 //ITerm=0;
Gendy 0:4a55d0a21ea9 79
Gendy 0:4a55d0a21ea9 80 }
Gendy 0:4a55d0a21ea9 81
Gendy 0:4a55d0a21ea9 82 /* SetOutputLimits(...)****************************************************
Gendy 0:4a55d0a21ea9 83 * This function will be used far more often than SetInputLimits. while
Gendy 0:4a55d0a21ea9 84 * the input to the controller will generally be in the 0-1023 range (which is
Gendy 0:4a55d0a21ea9 85 * the default already,) the output will be a little different. maybe they'll
Gendy 0:4a55d0a21ea9 86 * be doing a time window and will need 0-8000 or something. or maybe they'll
Gendy 0:4a55d0a21ea9 87 * want to clamp it from 0-125. who knows. at any rate, that can all be done
Gendy 0:4a55d0a21ea9 88 * here.
Gendy 0:4a55d0a21ea9 89 **************************************************************************/
Gendy 0:4a55d0a21ea9 90 void PID::SetOutputLimits(float Min, float Max)
Gendy 0:4a55d0a21ea9 91 {
Gendy 0:4a55d0a21ea9 92 if(Min >= Max) return;
Gendy 0:4a55d0a21ea9 93 outMin = Min;
Gendy 0:4a55d0a21ea9 94 outMax = Max;
Gendy 0:4a55d0a21ea9 95
Gendy 0:4a55d0a21ea9 96 }
Gendy 0:4a55d0a21ea9 97 /****************SetIntegratorLimits**********/
Gendy 0:4a55d0a21ea9 98 void PID::SetIntegratorLimits(float Max)
Gendy 0:4a55d0a21ea9 99 {
Gendy 0:4a55d0a21ea9 100 max=Max;
Gendy 0:4a55d0a21ea9 101 }
Gendy 0:4a55d0a21ea9 102
Gendy 0:4a55d0a21ea9 103