Keegan Hu / Mbed 2 deprecated MPU6050_Driver_Balance

fsaf

Dependencies:   mbed

Fork of MPU6050_Driver_Balanceddddd by Keegan Hu

PID.h@1:f9658c7309ef, 2018-04-20 (annotated)

Committer:
glintligo
Date:
Fri Apr 20 08:27:50 2018 +0000
Revision:
1:f9658c7309ef
fdd

Who changed what in which revision?

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