quadcopter cufe / Mbed 2 deprecated FollowWall

follow wall 1

Dependencies:   mbed

PID/PID.h@0:b703833f6795, 2014-04-08 (annotated)

Committer:
khaledelmadawi
Date:
Tue Apr 08 13:23:06 2014 +0000
Revision:
0:b703833f6795
follow wall 1

Who changed what in which revision?

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