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?
User | Revision | Line number | New 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 |