Warwick Racing
PID library (not mine)
Dependents: ElecPneuShifter_4 ipod
PID.h@0:9fe5d80c3c5e, 2010-11-28 (annotated)
- Committer:
- WarwickRacing
- Date:
- Sun Nov 28 14:20:07 2010 +0000
- Revision:
- 0:9fe5d80c3c5e
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| WarwickRacing | 0:9fe5d80c3c5e | 1 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 2 | * @author Aaron Berk |
| WarwickRacing | 0:9fe5d80c3c5e | 3 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 4 | * @section LICENSE |
| WarwickRacing | 0:9fe5d80c3c5e | 5 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 6 | * Copyright (c) 2010 ARM Limited |
| WarwickRacing | 0:9fe5d80c3c5e | 7 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
| WarwickRacing | 0:9fe5d80c3c5e | 9 | * of this software and associated documentation files (the "Software"), to deal |
| WarwickRacing | 0:9fe5d80c3c5e | 10 | * in the Software without restriction, including without limitation the rights |
| WarwickRacing | 0:9fe5d80c3c5e | 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
| WarwickRacing | 0:9fe5d80c3c5e | 12 | * copies of the Software, and to permit persons to whom the Software is |
| WarwickRacing | 0:9fe5d80c3c5e | 13 | * furnished to do so, subject to the following conditions: |
| WarwickRacing | 0:9fe5d80c3c5e | 14 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 15 | * The above copyright notice and this permission notice shall be included in |
| WarwickRacing | 0:9fe5d80c3c5e | 16 | * all copies or substantial portions of the Software. |
| WarwickRacing | 0:9fe5d80c3c5e | 17 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
| WarwickRacing | 0:9fe5d80c3c5e | 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
| WarwickRacing | 0:9fe5d80c3c5e | 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
| WarwickRacing | 0:9fe5d80c3c5e | 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
| WarwickRacing | 0:9fe5d80c3c5e | 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
| WarwickRacing | 0:9fe5d80c3c5e | 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
| WarwickRacing | 0:9fe5d80c3c5e | 24 | * THE SOFTWARE. |
| WarwickRacing | 0:9fe5d80c3c5e | 25 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 26 | * @section DESCRIPTION |
| WarwickRacing | 0:9fe5d80c3c5e | 27 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 28 | * A PID controller is a widely used feedback controller commonly found in |
| WarwickRacing | 0:9fe5d80c3c5e | 29 | * industry. |
| WarwickRacing | 0:9fe5d80c3c5e | 30 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 31 | * This library is a port of Brett Beauregard's Arduino PID library: |
| WarwickRacing | 0:9fe5d80c3c5e | 32 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 33 | * http://www.arduino.cc/playground/Code/PIDLibrary |
| WarwickRacing | 0:9fe5d80c3c5e | 34 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 35 | * The wikipedia article on PID controllers is a good place to start on |
| WarwickRacing | 0:9fe5d80c3c5e | 36 | * understanding how they work: |
| WarwickRacing | 0:9fe5d80c3c5e | 37 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 38 | * http://en.wikipedia.org/wiki/PID_controller |
| WarwickRacing | 0:9fe5d80c3c5e | 39 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 40 | * For a clear and elegant explanation of how to implement and tune a |
| WarwickRacing | 0:9fe5d80c3c5e | 41 | * controller, the controlguru website by Douglas J. Cooper (who also happened |
| WarwickRacing | 0:9fe5d80c3c5e | 42 | * to be Brett's controls professor) is an excellent reference: |
| WarwickRacing | 0:9fe5d80c3c5e | 43 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 44 | * http://www.controlguru.com/ |
| WarwickRacing | 0:9fe5d80c3c5e | 45 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 46 | |
| WarwickRacing | 0:9fe5d80c3c5e | 47 | #ifndef PID_H |
| WarwickRacing | 0:9fe5d80c3c5e | 48 | #define PID_H |
| WarwickRacing | 0:9fe5d80c3c5e | 49 | |
| WarwickRacing | 0:9fe5d80c3c5e | 50 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 51 | * Includes |
| WarwickRacing | 0:9fe5d80c3c5e | 52 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 53 | #include "mbed.h" |
| WarwickRacing | 0:9fe5d80c3c5e | 54 | |
| WarwickRacing | 0:9fe5d80c3c5e | 55 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 56 | * Defines |
| WarwickRacing | 0:9fe5d80c3c5e | 57 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 58 | #define MANUAL_MODE 0 |
| WarwickRacing | 0:9fe5d80c3c5e | 59 | #define AUTO_MODE 1 |
| WarwickRacing | 0:9fe5d80c3c5e | 60 | |
| WarwickRacing | 0:9fe5d80c3c5e | 61 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 62 | * Proportional-integral-derivative controller. |
| WarwickRacing | 0:9fe5d80c3c5e | 63 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 64 | class PID { |
| WarwickRacing | 0:9fe5d80c3c5e | 65 | |
| WarwickRacing | 0:9fe5d80c3c5e | 66 | public: |
| WarwickRacing | 0:9fe5d80c3c5e | 67 | |
| WarwickRacing | 0:9fe5d80c3c5e | 68 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 69 | * Constructor. |
| WarwickRacing | 0:9fe5d80c3c5e | 70 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 71 | * Sets default limits [0-3.3V], calculates tuning parameters, and sets |
| WarwickRacing | 0:9fe5d80c3c5e | 72 | * manual mode with no bias. |
| WarwickRacing | 0:9fe5d80c3c5e | 73 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 74 | * @param Kc - Tuning parameter |
| WarwickRacing | 0:9fe5d80c3c5e | 75 | * @param tauI - Tuning parameter |
| WarwickRacing | 0:9fe5d80c3c5e | 76 | * @param tauD - Tuning parameter |
| WarwickRacing | 0:9fe5d80c3c5e | 77 | * @param interval PID calculation performed every interval seconds. |
| WarwickRacing | 0:9fe5d80c3c5e | 78 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 79 | PID(float Kc, float tauI, float tauD, float interval); |
| WarwickRacing | 0:9fe5d80c3c5e | 80 | |
| WarwickRacing | 0:9fe5d80c3c5e | 81 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 82 | * Scale from inputs to 0-100%. |
| WarwickRacing | 0:9fe5d80c3c5e | 83 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 84 | * @param InMin The real world value corresponding to 0%. |
| WarwickRacing | 0:9fe5d80c3c5e | 85 | * @param InMax The real world value corresponding to 100%. |
| WarwickRacing | 0:9fe5d80c3c5e | 86 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 87 | void setInputLimits(float inMin , float inMax); |
| WarwickRacing | 0:9fe5d80c3c5e | 88 | |
| WarwickRacing | 0:9fe5d80c3c5e | 89 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 90 | * Scale from outputs to 0-100%. |
| WarwickRacing | 0:9fe5d80c3c5e | 91 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 92 | * @param outMin The real world value corresponding to 0%. |
| WarwickRacing | 0:9fe5d80c3c5e | 93 | * @param outMax The real world value corresponding to 100%. |
| WarwickRacing | 0:9fe5d80c3c5e | 94 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 95 | void setOutputLimits(float outMin, float outMax); |
| WarwickRacing | 0:9fe5d80c3c5e | 96 | |
| WarwickRacing | 0:9fe5d80c3c5e | 97 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 98 | * Calculate PID constants. |
| WarwickRacing | 0:9fe5d80c3c5e | 99 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 100 | * Allows parameters to be changed on the fly without ruining calculations. |
| WarwickRacing | 0:9fe5d80c3c5e | 101 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 102 | * @param Kc - Tuning parameter |
| WarwickRacing | 0:9fe5d80c3c5e | 103 | * @param tauI - Tuning parameter |
| WarwickRacing | 0:9fe5d80c3c5e | 104 | * @param tauD - Tuning parameter |
| WarwickRacing | 0:9fe5d80c3c5e | 105 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 106 | void setTunings(float Kc, float tauI, float tauD); |
| WarwickRacing | 0:9fe5d80c3c5e | 107 | |
| WarwickRacing | 0:9fe5d80c3c5e | 108 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 109 | * Reinitializes controller internals. Automatically |
| WarwickRacing | 0:9fe5d80c3c5e | 110 | * called on a manual to auto transition. |
| WarwickRacing | 0:9fe5d80c3c5e | 111 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 112 | void reset(void); |
| WarwickRacing | 0:9fe5d80c3c5e | 113 | |
| WarwickRacing | 0:9fe5d80c3c5e | 114 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 115 | * Set PID to manual or auto mode. |
| WarwickRacing | 0:9fe5d80c3c5e | 116 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 117 | * @param mode 0 -> Manual |
| WarwickRacing | 0:9fe5d80c3c5e | 118 | * Non-zero -> Auto |
| WarwickRacing | 0:9fe5d80c3c5e | 119 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 120 | void setMode(int mode); |
| WarwickRacing | 0:9fe5d80c3c5e | 121 | |
| WarwickRacing | 0:9fe5d80c3c5e | 122 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 123 | * Set how fast the PID loop is run. |
| WarwickRacing | 0:9fe5d80c3c5e | 124 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 125 | * @param interval PID calculation peformed every interval seconds. |
| WarwickRacing | 0:9fe5d80c3c5e | 126 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 127 | void setInterval(float interval); |
| WarwickRacing | 0:9fe5d80c3c5e | 128 | |
| WarwickRacing | 0:9fe5d80c3c5e | 129 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 130 | * Set the set point. |
| WarwickRacing | 0:9fe5d80c3c5e | 131 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 132 | * @param sp The set point as a real world value. |
| WarwickRacing | 0:9fe5d80c3c5e | 133 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 134 | void setSetPoint(float sp); |
| WarwickRacing | 0:9fe5d80c3c5e | 135 | |
| WarwickRacing | 0:9fe5d80c3c5e | 136 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 137 | * Set the process value. |
| WarwickRacing | 0:9fe5d80c3c5e | 138 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 139 | * @param pv The process value as a real world value. |
| WarwickRacing | 0:9fe5d80c3c5e | 140 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 141 | void setProcessValue(float pv); |
| WarwickRacing | 0:9fe5d80c3c5e | 142 | |
| WarwickRacing | 0:9fe5d80c3c5e | 143 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 144 | * Set the bias. |
| WarwickRacing | 0:9fe5d80c3c5e | 145 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 146 | * @param bias The bias for the controller output. |
| WarwickRacing | 0:9fe5d80c3c5e | 147 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 148 | void setBias(float bias); |
| WarwickRacing | 0:9fe5d80c3c5e | 149 | |
| WarwickRacing | 0:9fe5d80c3c5e | 150 | /** |
| WarwickRacing | 0:9fe5d80c3c5e | 151 | * PID calculation. |
| WarwickRacing | 0:9fe5d80c3c5e | 152 | * |
| WarwickRacing | 0:9fe5d80c3c5e | 153 | * @return The controller output as a float between outMin and outMax. |
| WarwickRacing | 0:9fe5d80c3c5e | 154 | */ |
| WarwickRacing | 0:9fe5d80c3c5e | 155 | float compute(void); |
| WarwickRacing | 0:9fe5d80c3c5e | 156 | |
| WarwickRacing | 0:9fe5d80c3c5e | 157 | //Getters. |
| WarwickRacing | 0:9fe5d80c3c5e | 158 | float getInMin(); |
| WarwickRacing | 0:9fe5d80c3c5e | 159 | float getInMax(); |
| WarwickRacing | 0:9fe5d80c3c5e | 160 | float getOutMin(); |
| WarwickRacing | 0:9fe5d80c3c5e | 161 | float getOutMax(); |
| WarwickRacing | 0:9fe5d80c3c5e | 162 | float getInterval(); |
| WarwickRacing | 0:9fe5d80c3c5e | 163 | float getPParam(); |
| WarwickRacing | 0:9fe5d80c3c5e | 164 | float getIParam(); |
| WarwickRacing | 0:9fe5d80c3c5e | 165 | float getDParam(); |
| WarwickRacing | 0:9fe5d80c3c5e | 166 | |
| WarwickRacing | 0:9fe5d80c3c5e | 167 | private: |
| WarwickRacing | 0:9fe5d80c3c5e | 168 | |
| WarwickRacing | 0:9fe5d80c3c5e | 169 | bool usingFeedForward; |
| WarwickRacing | 0:9fe5d80c3c5e | 170 | bool inAuto; |
| WarwickRacing | 0:9fe5d80c3c5e | 171 | |
| WarwickRacing | 0:9fe5d80c3c5e | 172 | //Actual tuning parameters used in PID calculation. |
| WarwickRacing | 0:9fe5d80c3c5e | 173 | float Kc_; |
| WarwickRacing | 0:9fe5d80c3c5e | 174 | float tauR_; |
| WarwickRacing | 0:9fe5d80c3c5e | 175 | float tauD_; |
| WarwickRacing | 0:9fe5d80c3c5e | 176 | |
| WarwickRacing | 0:9fe5d80c3c5e | 177 | //Raw tuning parameters. |
| WarwickRacing | 0:9fe5d80c3c5e | 178 | float pParam_; |
| WarwickRacing | 0:9fe5d80c3c5e | 179 | float iParam_; |
| WarwickRacing | 0:9fe5d80c3c5e | 180 | float dParam_; |
| WarwickRacing | 0:9fe5d80c3c5e | 181 | |
| WarwickRacing | 0:9fe5d80c3c5e | 182 | //The point we want to reach. |
| WarwickRacing | 0:9fe5d80c3c5e | 183 | float setPoint_; |
| WarwickRacing | 0:9fe5d80c3c5e | 184 | //The thing we measure. |
| WarwickRacing | 0:9fe5d80c3c5e | 185 | float processVariable_; |
| WarwickRacing | 0:9fe5d80c3c5e | 186 | float prevProcessVariable_; |
| WarwickRacing | 0:9fe5d80c3c5e | 187 | //The output that affects the process variable. |
| WarwickRacing | 0:9fe5d80c3c5e | 188 | float controllerOutput_; |
| WarwickRacing | 0:9fe5d80c3c5e | 189 | float prevControllerOutput_; |
| WarwickRacing | 0:9fe5d80c3c5e | 190 | |
| WarwickRacing | 0:9fe5d80c3c5e | 191 | //We work in % for calculations so these will scale from |
| WarwickRacing | 0:9fe5d80c3c5e | 192 | //real world values to 0-100% and back again. |
| WarwickRacing | 0:9fe5d80c3c5e | 193 | float inMin_; |
| WarwickRacing | 0:9fe5d80c3c5e | 194 | float inMax_; |
| WarwickRacing | 0:9fe5d80c3c5e | 195 | float inSpan_; |
| WarwickRacing | 0:9fe5d80c3c5e | 196 | float outMin_; |
| WarwickRacing | 0:9fe5d80c3c5e | 197 | float outMax_; |
| WarwickRacing | 0:9fe5d80c3c5e | 198 | float outSpan_; |
| WarwickRacing | 0:9fe5d80c3c5e | 199 | |
| WarwickRacing | 0:9fe5d80c3c5e | 200 | //The accumulated error, i.e. integral. |
| WarwickRacing | 0:9fe5d80c3c5e | 201 | float accError_; |
| WarwickRacing | 0:9fe5d80c3c5e | 202 | //The controller output bias. |
| WarwickRacing | 0:9fe5d80c3c5e | 203 | float bias_; |
| WarwickRacing | 0:9fe5d80c3c5e | 204 | |
| WarwickRacing | 0:9fe5d80c3c5e | 205 | //The interval between samples. |
| WarwickRacing | 0:9fe5d80c3c5e | 206 | float tSample_; |
| WarwickRacing | 0:9fe5d80c3c5e | 207 | |
| WarwickRacing | 0:9fe5d80c3c5e | 208 | //Controller output as a real world value. |
| WarwickRacing | 0:9fe5d80c3c5e | 209 | volatile float realOutput_; |
| WarwickRacing | 0:9fe5d80c3c5e | 210 | |
| WarwickRacing | 0:9fe5d80c3c5e | 211 | }; |
| WarwickRacing | 0:9fe5d80c3c5e | 212 | |
| WarwickRacing | 0:9fe5d80c3c5e | 213 | #endif /* PID_H */ |