Hang Liu
Fork of
PID
by 
Kamil Foryszewski
PID.h@3:7f0ed54318df, 2016-11-08 (annotated)
- Committer:
- HangL
- Date:
- Tue Nov 08 21:19:04 2016 +0000
- Revision:
- 3:7f0ed54318df
- Parent:
- 2:0fff4827f3b6
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| aberk | 0:6e12a3e5af19 | 1 | |
| aberk | 0:6e12a3e5af19 | 2 | #ifndef PID_H |
| aberk | 0:6e12a3e5af19 | 3 | #define PID_H |
| aberk | 0:6e12a3e5af19 | 4 | |
| aberk | 0:6e12a3e5af19 | 5 | /** |
| aberk | 0:6e12a3e5af19 | 6 | * Includes |
| aberk | 0:6e12a3e5af19 | 7 | */ |
| aberk | 0:6e12a3e5af19 | 8 | #include "mbed.h" |
| aberk | 0:6e12a3e5af19 | 9 | |
| aberk | 0:6e12a3e5af19 | 10 | /** |
| aberk | 0:6e12a3e5af19 | 11 | * Defines |
| aberk | 0:6e12a3e5af19 | 12 | */ |
| aberk | 0:6e12a3e5af19 | 13 | #define MANUAL_MODE 0 |
| aberk | 0:6e12a3e5af19 | 14 | #define AUTO_MODE 1 |
| aberk | 0:6e12a3e5af19 | 15 | |
| aberk | 0:6e12a3e5af19 | 16 | /** |
| aberk | 0:6e12a3e5af19 | 17 | * Proportional-integral-derivative controller. |
| aberk | 0:6e12a3e5af19 | 18 | */ |
| aberk | 0:6e12a3e5af19 | 19 | class PID { |
| aberk | 0:6e12a3e5af19 | 20 | |
| aberk | 0:6e12a3e5af19 | 21 | public: |
| aberk | 0:6e12a3e5af19 | 22 | |
| aberk | 0:6e12a3e5af19 | 23 | /** |
| aberk | 0:6e12a3e5af19 | 24 | * Constructor. |
| aberk | 0:6e12a3e5af19 | 25 | * |
| kfforex | 1:0ffb635770b3 | 26 | * Sets default limits , calculates tuning parameters, and sets |
| aberk | 0:6e12a3e5af19 | 27 | * manual mode with no bias. |
| aberk | 0:6e12a3e5af19 | 28 | * |
| aberk | 0:6e12a3e5af19 | 29 | * @param Kc - Tuning parameter |
| aberk | 0:6e12a3e5af19 | 30 | * @param tauI - Tuning parameter |
| aberk | 0:6e12a3e5af19 | 31 | * @param tauD - Tuning parameter |
| aberk | 0:6e12a3e5af19 | 32 | * @param interval PID calculation performed every interval seconds. |
| aberk | 0:6e12a3e5af19 | 33 | */ |
| HangL | 2:0fff4827f3b6 | 34 | // PID(float Kc, float tauI, float tauD, float interval); |
| HangL | 2:0fff4827f3b6 | 35 | /*****************************************************************/ |
| HangL | 2:0fff4827f3b6 | 36 | PID(float interval); |
| HangL | 2:0fff4827f3b6 | 37 | /*****************************************************************/ |
| aberk | 0:6e12a3e5af19 | 38 | /** |
| aberk | 0:6e12a3e5af19 | 39 | * Scale from inputs to 0-100%. |
| aberk | 0:6e12a3e5af19 | 40 | * |
| aberk | 0:6e12a3e5af19 | 41 | * @param InMin The real world value corresponding to 0%. |
| aberk | 0:6e12a3e5af19 | 42 | * @param InMax The real world value corresponding to 100%. |
| aberk | 0:6e12a3e5af19 | 43 | */ |
| aberk | 0:6e12a3e5af19 | 44 | void setInputLimits(float inMin , float inMax); |
| aberk | 0:6e12a3e5af19 | 45 | |
| aberk | 0:6e12a3e5af19 | 46 | /** |
| aberk | 0:6e12a3e5af19 | 47 | * Scale from outputs to 0-100%. |
| aberk | 0:6e12a3e5af19 | 48 | * |
| aberk | 0:6e12a3e5af19 | 49 | * @param outMin The real world value corresponding to 0%. |
| aberk | 0:6e12a3e5af19 | 50 | * @param outMax The real world value corresponding to 100%. |
| aberk | 0:6e12a3e5af19 | 51 | */ |
| aberk | 0:6e12a3e5af19 | 52 | void setOutputLimits(float outMin, float outMax); |
| aberk | 0:6e12a3e5af19 | 53 | |
| aberk | 0:6e12a3e5af19 | 54 | /** |
| aberk | 0:6e12a3e5af19 | 55 | * Calculate PID constants. |
| aberk | 0:6e12a3e5af19 | 56 | * |
| aberk | 0:6e12a3e5af19 | 57 | * Allows parameters to be changed on the fly without ruining calculations. |
| aberk | 0:6e12a3e5af19 | 58 | * |
| aberk | 0:6e12a3e5af19 | 59 | * @param Kc - Tuning parameter |
| aberk | 0:6e12a3e5af19 | 60 | * @param tauI - Tuning parameter |
| aberk | 0:6e12a3e5af19 | 61 | * @param tauD - Tuning parameter |
| aberk | 0:6e12a3e5af19 | 62 | */ |
| HangL | 2:0fff4827f3b6 | 63 | // void setTunings(float Kc, float tauI, float tauD); |
| aberk | 0:6e12a3e5af19 | 64 | |
| aberk | 0:6e12a3e5af19 | 65 | /** |
| aberk | 0:6e12a3e5af19 | 66 | * Reinitializes controller internals. Automatically |
| aberk | 0:6e12a3e5af19 | 67 | * called on a manual to auto transition. |
| aberk | 0:6e12a3e5af19 | 68 | */ |
| HangL | 2:0fff4827f3b6 | 69 | /***************************************************************/ |
| HangL | 2:0fff4827f3b6 | 70 | void setStandTunings(float Kc,float tauI,float tauD); |
| HangL | 2:0fff4827f3b6 | 71 | // void setSpeedTunings(float Kc,float tauI,float tauD); |
| HangL | 2:0fff4827f3b6 | 72 | /***************************************************************/ |
| aberk | 0:6e12a3e5af19 | 73 | void reset(void); |
| aberk | 0:6e12a3e5af19 | 74 | |
| aberk | 0:6e12a3e5af19 | 75 | /** |
| aberk | 0:6e12a3e5af19 | 76 | * Set PID to manual or auto mode. |
| aberk | 0:6e12a3e5af19 | 77 | * |
| aberk | 0:6e12a3e5af19 | 78 | * @param mode 0 -> Manual |
| aberk | 0:6e12a3e5af19 | 79 | * Non-zero -> Auto |
| aberk | 0:6e12a3e5af19 | 80 | */ |
| aberk | 0:6e12a3e5af19 | 81 | void setMode(int mode); |
| aberk | 0:6e12a3e5af19 | 82 | |
| aberk | 0:6e12a3e5af19 | 83 | /** |
| aberk | 0:6e12a3e5af19 | 84 | * Set how fast the PID loop is run. |
| aberk | 0:6e12a3e5af19 | 85 | * |
| aberk | 0:6e12a3e5af19 | 86 | * @param interval PID calculation peformed every interval seconds. |
| aberk | 0:6e12a3e5af19 | 87 | */ |
| aberk | 0:6e12a3e5af19 | 88 | void setInterval(float interval); |
| aberk | 0:6e12a3e5af19 | 89 | |
| aberk | 0:6e12a3e5af19 | 90 | /** |
| aberk | 0:6e12a3e5af19 | 91 | * Set the set point. |
| aberk | 0:6e12a3e5af19 | 92 | * |
| aberk | 0:6e12a3e5af19 | 93 | * @param sp The set point as a real world value. |
| aberk | 0:6e12a3e5af19 | 94 | */ |
| aberk | 0:6e12a3e5af19 | 95 | void setSetPoint(float sp); |
| aberk | 0:6e12a3e5af19 | 96 | |
| aberk | 0:6e12a3e5af19 | 97 | /** |
| aberk | 0:6e12a3e5af19 | 98 | * Set the process value. |
| aberk | 0:6e12a3e5af19 | 99 | * |
| aberk | 0:6e12a3e5af19 | 100 | * @param pv The process value as a real world value. |
| aberk | 0:6e12a3e5af19 | 101 | */ |
| aberk | 0:6e12a3e5af19 | 102 | void setProcessValue(float pv); |
| aberk | 0:6e12a3e5af19 | 103 | |
| aberk | 0:6e12a3e5af19 | 104 | /** |
| aberk | 0:6e12a3e5af19 | 105 | * Set the bias. |
| aberk | 0:6e12a3e5af19 | 106 | * |
| aberk | 0:6e12a3e5af19 | 107 | * @param bias The bias for the controller output. |
| aberk | 0:6e12a3e5af19 | 108 | */ |
| aberk | 0:6e12a3e5af19 | 109 | void setBias(float bias); |
| aberk | 0:6e12a3e5af19 | 110 | |
| aberk | 0:6e12a3e5af19 | 111 | /** |
| aberk | 0:6e12a3e5af19 | 112 | * PID calculation. |
| aberk | 0:6e12a3e5af19 | 113 | * |
| aberk | 0:6e12a3e5af19 | 114 | * @return The controller output as a float between outMin and outMax. |
| aberk | 0:6e12a3e5af19 | 115 | */ |
| aberk | 0:6e12a3e5af19 | 116 | float compute(void); |
| HangL | 2:0fff4827f3b6 | 117 | float Speedcompute(void); |
| HangL | 2:0fff4827f3b6 | 118 | float Speedcompute(double Stepper,double Target); |
| aberk | 0:6e12a3e5af19 | 119 | |
| aberk | 0:6e12a3e5af19 | 120 | //Getters. |
| aberk | 0:6e12a3e5af19 | 121 | float getInMin(); |
| aberk | 0:6e12a3e5af19 | 122 | float getInMax(); |
| aberk | 0:6e12a3e5af19 | 123 | float getOutMin(); |
| aberk | 0:6e12a3e5af19 | 124 | float getOutMax(); |
| aberk | 0:6e12a3e5af19 | 125 | float getInterval(); |
| aberk | 0:6e12a3e5af19 | 126 | float getPParam(); |
| aberk | 0:6e12a3e5af19 | 127 | float getIParam(); |
| aberk | 0:6e12a3e5af19 | 128 | float getDParam(); |
| HangL | 2:0fff4827f3b6 | 129 | // float StandKc=120.0;float StandtauI=0.004;float StandtauD=0.0007; |
| HangL | 2:0fff4827f3b6 | 130 | private: |
| aberk | 0:6e12a3e5af19 | 131 | |
| HangL | 2:0fff4827f3b6 | 132 | |
| HangL | 2:0fff4827f3b6 | 133 | // float StandKc=120.0;float StandtauI=0.004;float StandtauD=0.0007; |
| aberk | 0:6e12a3e5af19 | 134 | |
| aberk | 0:6e12a3e5af19 | 135 | bool usingFeedForward; |
| aberk | 0:6e12a3e5af19 | 136 | bool inAuto; |
| aberk | 0:6e12a3e5af19 | 137 | |
| aberk | 0:6e12a3e5af19 | 138 | //Actual tuning parameters used in PID calculation. |
| aberk | 0:6e12a3e5af19 | 139 | float Kc_; |
| aberk | 0:6e12a3e5af19 | 140 | float tauR_; |
| aberk | 0:6e12a3e5af19 | 141 | float tauD_; |
| HangL | 2:0fff4827f3b6 | 142 | /*****************************************/ |
| HangL | 2:0fff4827f3b6 | 143 | // float Speed_Kc_; |
| HangL | 2:0fff4827f3b6 | 144 | // float Speed_tauI_; |
| HangL | 2:0fff4827f3b6 | 145 | |
| HangL | 2:0fff4827f3b6 | 146 | /*****************************************/ |
| aberk | 0:6e12a3e5af19 | 147 | |
| aberk | 0:6e12a3e5af19 | 148 | //Raw tuning parameters. |
| aberk | 0:6e12a3e5af19 | 149 | float pParam_; |
| aberk | 0:6e12a3e5af19 | 150 | float iParam_; |
| aberk | 0:6e12a3e5af19 | 151 | float dParam_; |
| aberk | 0:6e12a3e5af19 | 152 | |
| aberk | 0:6e12a3e5af19 | 153 | //The point we want to reach. |
| aberk | 0:6e12a3e5af19 | 154 | float setPoint_; |
| aberk | 0:6e12a3e5af19 | 155 | //The thing we measure. |
| aberk | 0:6e12a3e5af19 | 156 | float processVariable_; |
| aberk | 0:6e12a3e5af19 | 157 | float prevProcessVariable_; |
| aberk | 0:6e12a3e5af19 | 158 | //The output that affects the process variable. |
| aberk | 0:6e12a3e5af19 | 159 | float controllerOutput_; |
| aberk | 0:6e12a3e5af19 | 160 | float prevControllerOutput_; |
| aberk | 0:6e12a3e5af19 | 161 | |
| aberk | 0:6e12a3e5af19 | 162 | //We work in % for calculations so these will scale from |
| aberk | 0:6e12a3e5af19 | 163 | //real world values to 0-100% and back again. |
| aberk | 0:6e12a3e5af19 | 164 | float inMin_; |
| aberk | 0:6e12a3e5af19 | 165 | float inMax_; |
| aberk | 0:6e12a3e5af19 | 166 | float inSpan_; |
| aberk | 0:6e12a3e5af19 | 167 | float outMin_; |
| aberk | 0:6e12a3e5af19 | 168 | float outMax_; |
| aberk | 0:6e12a3e5af19 | 169 | float outSpan_; |
| aberk | 0:6e12a3e5af19 | 170 | |
| aberk | 0:6e12a3e5af19 | 171 | //The accumulated error, i.e. integral. |
| aberk | 0:6e12a3e5af19 | 172 | float accError_; |
| aberk | 0:6e12a3e5af19 | 173 | //The controller output bias. |
| aberk | 0:6e12a3e5af19 | 174 | float bias_; |
| aberk | 0:6e12a3e5af19 | 175 | |
| aberk | 0:6e12a3e5af19 | 176 | //The interval between samples. |
| aberk | 0:6e12a3e5af19 | 177 | float tSample_; |
| aberk | 0:6e12a3e5af19 | 178 | |
| aberk | 0:6e12a3e5af19 | 179 | //Controller output as a real world value. |
| aberk | 0:6e12a3e5af19 | 180 | volatile float realOutput_; |
| aberk | 0:6e12a3e5af19 | 181 | |
| aberk | 0:6e12a3e5af19 | 182 | }; |
| aberk | 0:6e12a3e5af19 | 183 | |
| aberk | 0:6e12a3e5af19 | 184 | #endif /* PID_H */ |