quadcopter cufe / Mbed 2 deprecated FollowWall2

Go to goal robot with follow wall algo

Dependencies:   mbed

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

Committer:
khaledelmadawi
Date:
Tue Apr 08 13:21:17 2014 +0000
Revision:
0:efef62b55c86
Follow wall

Who changed what in which revision?

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