Keegan Hu / Mbed 2 deprecated MPU6050_Driver_Balance

fsaf

Dependencies:   mbed

Fork of MPU6050_Driver_Balanceddddd by Keegan Hu

PID.cpp@2:3a7eb05dbc72, 2018-04-20 (annotated)

Committer:
glintligo
Date:
Fri Apr 20 10:56:57 2018 +0000
Revision:
2:3a7eb05dbc72
Parent:
1:f9658c7309ef 
2018.4.20;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
glintligo 1:f9658c7309ef 1 /**************************************************************************************************************************************************************
glintligo 1:f9658c7309ef 2 // This is a modified version of mbed /users/aberk/code/PID/ for LSM303DLHC
glintligo 1:f9658c7309ef 3 //
glintligo 1:f9658c7309ef 4 // Changes made by Ryan Spillman:
glintligo 1:f9658c7309ef 5 // Fixed a mathematical issue (need to better document said fix)
glintligo 1:f9658c7309ef 6 //
glintligo 1:f9658c7309ef 7 // Need to better document scaling and other mathematical issues
glintligo 1:f9658c7309ef 8 **************************************************************************************************************************************************************/
glintligo 1:f9658c7309ef 9 #include "PID.h"
glintligo 1:f9658c7309ef 10
glintligo 1:f9658c7309ef 11 PID::PID(float Kc, float tauI, float tauD, float interval)
glintligo 1:f9658c7309ef 12 {
glintligo 1:f9658c7309ef 13
glintligo 1:f9658c7309ef 14 usingFeedForward = false;
glintligo 1:f9658c7309ef 15 inAuto = false;
glintligo 1:f9658c7309ef 16
glintligo 1:f9658c7309ef 17 //Default the limits to the full range of I/O: 3.3V
glintligo 1:f9658c7309ef 18 //Make sure to set these to more appropriate limits for
glintligo 1:f9658c7309ef 19 //your application.
glintligo 1:f9658c7309ef 20 setInputLimits(0.0, 3.3);
glintligo 1:f9658c7309ef 21 setOutputLimits(0.0, 3.3);
glintligo 1:f9658c7309ef 22
glintligo 1:f9658c7309ef 23 tSample_ = interval;
glintligo 1:f9658c7309ef 24
glintligo 1:f9658c7309ef 25 setTunings(Kc, tauI, tauD);
glintligo 1:f9658c7309ef 26
glintligo 1:f9658c7309ef 27 setPoint_ = 0.0;
glintligo 1:f9658c7309ef 28 processVariable_ = 0.0;
glintligo 1:f9658c7309ef 29 prevProcessVariable_ = 0.0;
glintligo 1:f9658c7309ef 30 controllerOutput_ = 0.0;
glintligo 1:f9658c7309ef 31 prevControllerOutput_ = 0.0;
glintligo 1:f9658c7309ef 32
glintligo 1:f9658c7309ef 33 accError_ = 0.0;
glintligo 1:f9658c7309ef 34 bias_ = 0.0;
glintligo 1:f9658c7309ef 35
glintligo 1:f9658c7309ef 36 realOutput_ = 0.0;
glintligo 1:f9658c7309ef 37
glintligo 1:f9658c7309ef 38 }
glintligo 1:f9658c7309ef 39
glintligo 1:f9658c7309ef 40 void PID::setInputLimits(float inMin, float inMax)
glintligo 1:f9658c7309ef 41 {
glintligo 1:f9658c7309ef 42 inMin_ = inMin;
glintligo 1:f9658c7309ef 43 inMax_ = inMax;
glintligo 1:f9658c7309ef 44 inSpan_ = inMax - inMin;
glintligo 1:f9658c7309ef 45
glintligo 1:f9658c7309ef 46 }
glintligo 1:f9658c7309ef 47
glintligo 1:f9658c7309ef 48 void PID::setOutputLimits(float outMin, float outMax)
glintligo 1:f9658c7309ef 49 {
glintligo 1:f9658c7309ef 50
glintligo 1:f9658c7309ef 51 outMin_ = outMin;
glintligo 1:f9658c7309ef 52 outMax_ = outMax;
glintligo 1:f9658c7309ef 53 outSpan_ = outMax - outMin;
glintligo 1:f9658c7309ef 54
glintligo 1:f9658c7309ef 55 }
glintligo 1:f9658c7309ef 56
glintligo 1:f9658c7309ef 57 void PID::setTunings(float Kc, float tauI, float tauD)
glintligo 1:f9658c7309ef 58 {
glintligo 1:f9658c7309ef 59 //Store raw values to hand back to user on request.
glintligo 1:f9658c7309ef 60 pParam_ = Kc;
glintligo 1:f9658c7309ef 61 iParam_ = tauI;
glintligo 1:f9658c7309ef 62 dParam_ = tauD;
glintligo 1:f9658c7309ef 63
glintligo 1:f9658c7309ef 64 float tempTauR;
glintligo 1:f9658c7309ef 65
glintligo 1:f9658c7309ef 66 if (tauI == 0.0f) {
glintligo 1:f9658c7309ef 67 tempTauR = 0.0f;
glintligo 1:f9658c7309ef 68 } else {
glintligo 1:f9658c7309ef 69 tempTauR = (1.0f / tauI) * tSample_;
glintligo 1:f9658c7309ef 70 }
glintligo 1:f9658c7309ef 71
glintligo 1:f9658c7309ef 72 //For "bumpless transfer" we need to rescale the accumulated error.
glintligo 1:f9658c7309ef 73 if (inAuto) {
glintligo 1:f9658c7309ef 74 if (tempTauR == 0.0f) {
glintligo 1:f9658c7309ef 75 accError_ = 0.0f;
glintligo 1:f9658c7309ef 76 } else {
glintligo 1:f9658c7309ef 77 accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
glintligo 1:f9658c7309ef 78 }
glintligo 1:f9658c7309ef 79 }
glintligo 1:f9658c7309ef 80
glintligo 1:f9658c7309ef 81 Kc_ = Kc;
glintligo 1:f9658c7309ef 82 tauR_ = tempTauR;
glintligo 1:f9658c7309ef 83 tauD_ = tauD / tSample_;
glintligo 1:f9658c7309ef 84
glintligo 1:f9658c7309ef 85 }
glintligo 1:f9658c7309ef 86
glintligo 1:f9658c7309ef 87 void PID::reset(void)
glintligo 1:f9658c7309ef 88 {
glintligo 1:f9658c7309ef 89 prevControllerOutput_ = 0.0f;
glintligo 1:f9658c7309ef 90
glintligo 1:f9658c7309ef 91 //Clear any error in the integral.
glintligo 1:f9658c7309ef 92 accError_ = 0.0f;
glintligo 1:f9658c7309ef 93
glintligo 1:f9658c7309ef 94 }
glintligo 1:f9658c7309ef 95
glintligo 1:f9658c7309ef 96 void PID::setMode(int mode) {
glintligo 1:f9658c7309ef 97
glintligo 1:f9658c7309ef 98 //We were in manual, and we just got set to auto.
glintligo 1:f9658c7309ef 99 //Reset the controller internals.
glintligo 1:f9658c7309ef 100 if (mode != 0 && !inAuto) {
glintligo 1:f9658c7309ef 101 reset();
glintligo 1:f9658c7309ef 102 }
glintligo 1:f9658c7309ef 103
glintligo 1:f9658c7309ef 104 inAuto = (mode != 0);
glintligo 1:f9658c7309ef 105
glintligo 1:f9658c7309ef 106 }
glintligo 1:f9658c7309ef 107
glintligo 1:f9658c7309ef 108 void PID::setInterval(float interval) {
glintligo 1:f9658c7309ef 109
glintligo 1:f9658c7309ef 110 if (interval > 0) {
glintligo 1:f9658c7309ef 111 //Convert the time-based tunings to reflect this change.
glintligo 1:f9658c7309ef 112 tauR_ *= (interval / tSample_);
glintligo 1:f9658c7309ef 113 accError_ *= (tSample_ / interval);
glintligo 1:f9658c7309ef 114 tauD_ *= (interval / tSample_);
glintligo 1:f9658c7309ef 115 tSample_ = interval;
glintligo 1:f9658c7309ef 116 }
glintligo 1:f9658c7309ef 117
glintligo 1:f9658c7309ef 118 }
glintligo 1:f9658c7309ef 119
glintligo 1:f9658c7309ef 120 void PID::setSetPoint(float sp) {
glintligo 1:f9658c7309ef 121
glintligo 1:f9658c7309ef 122 setPoint_ = sp;
glintligo 1:f9658c7309ef 123
glintligo 1:f9658c7309ef 124 }
glintligo 1:f9658c7309ef 125
glintligo 1:f9658c7309ef 126 void PID::setProcessValue(float pv) {
glintligo 1:f9658c7309ef 127
glintligo 1:f9658c7309ef 128 processVariable_ = pv;
glintligo 1:f9658c7309ef 129
glintligo 1:f9658c7309ef 130 }
glintligo 1:f9658c7309ef 131
glintligo 1:f9658c7309ef 132 void PID::setBias(float bias){
glintligo 1:f9658c7309ef 133
glintligo 1:f9658c7309ef 134 bias_ = bias;
glintligo 1:f9658c7309ef 135 usingFeedForward = 1;
glintligo 1:f9658c7309ef 136
glintligo 1:f9658c7309ef 137 }
glintligo 1:f9658c7309ef 138
glintligo 1:f9658c7309ef 139 float PID::compute()
glintligo 1:f9658c7309ef 140 {
glintligo 1:f9658c7309ef 141 float error = setPoint_ - processVariable_;
glintligo 1:f9658c7309ef 142
glintligo 1:f9658c7309ef 143 //Check and see if the output is pegged at a limit and only
glintligo 1:f9658c7309ef 144 //integrate if it is not. This is to prevent reset-windup.
glintligo 1:f9658c7309ef 145 if (!(prevControllerOutput_ >= outMax_ && error > 0) && !(prevControllerOutput_ <= outMin_ && error < 0)) {
glintligo 1:f9658c7309ef 146 accError_ += error;
glintligo 1:f9658c7309ef 147 }
glintligo 1:f9658c7309ef 148
glintligo 1:f9658c7309ef 149 //Compute the current slope of the input signal.
glintligo 1:f9658c7309ef 150 float dMeas = (processVariable_ - prevProcessVariable_) / tSample_;
glintligo 1:f9658c7309ef 151
glintligo 1:f9658c7309ef 152 //Perform the PID calculation.
glintligo 1:f9658c7309ef 153 controllerOutput_ = Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
glintligo 1:f9658c7309ef 154
glintligo 1:f9658c7309ef 155 //scale PID output based on user provided input and output restraints
glintligo 1:f9658c7309ef 156 controllerOutput_ = ((outMax_ - outMin_) * ((Kc_ * controllerOutput_) - inMin_)) / (inMax_ - inMin_) + outMin_;
glintligo 1:f9658c7309ef 157
glintligo 1:f9658c7309ef 158 //Make sure the computed output is within output constraints.
glintligo 1:f9658c7309ef 159 if (controllerOutput_ < outMin_) {
glintligo 1:f9658c7309ef 160 controllerOutput_ = outMin_;
glintligo 1:f9658c7309ef 161 } else if (controllerOutput_ > outMax_) {
glintligo 1:f9658c7309ef 162 controllerOutput_ = outMax_;
glintligo 1:f9658c7309ef 163 }
glintligo 1:f9658c7309ef 164
glintligo 1:f9658c7309ef 165 //Remember this output for the windup check next time.
glintligo 1:f9658c7309ef 166 prevControllerOutput_ = controllerOutput_;
glintligo 1:f9658c7309ef 167 //Remember the input for the derivative calculation next time.
glintligo 1:f9658c7309ef 168 prevProcessVariable_ = processVariable_;
glintligo 1:f9658c7309ef 169
glintligo 1:f9658c7309ef 170 //Scale the output from percent span back out to a real world number.
glintligo 1:f9658c7309ef 171 return controllerOutput_;
glintligo 1:f9658c7309ef 172 }
glintligo 1:f9658c7309ef 173
glintligo 1:f9658c7309ef 174 float PID::getInMin() {
glintligo 1:f9658c7309ef 175
glintligo 1:f9658c7309ef 176 return inMin_;
glintligo 1:f9658c7309ef 177
glintligo 1:f9658c7309ef 178 }
glintligo 1:f9658c7309ef 179
glintligo 1:f9658c7309ef 180 float PID::getInMax() {
glintligo 1:f9658c7309ef 181
glintligo 1:f9658c7309ef 182 return inMax_;
glintligo 1:f9658c7309ef 183
glintligo 1:f9658c7309ef 184 }
glintligo 1:f9658c7309ef 185
glintligo 1:f9658c7309ef 186 float PID::getOutMin() {
glintligo 1:f9658c7309ef 187
glintligo 1:f9658c7309ef 188 return outMin_;
glintligo 1:f9658c7309ef 189
glintligo 1:f9658c7309ef 190 }
glintligo 1:f9658c7309ef 191
glintligo 1:f9658c7309ef 192 float PID::getOutMax() {
glintligo 1:f9658c7309ef 193
glintligo 1:f9658c7309ef 194 return outMax_;
glintligo 1:f9658c7309ef 195
glintligo 1:f9658c7309ef 196 }
glintligo 1:f9658c7309ef 197
glintligo 1:f9658c7309ef 198 float PID::getInterval() {
glintligo 1:f9658c7309ef 199
glintligo 1:f9658c7309ef 200 return tSample_;
glintligo 1:f9658c7309ef 201
glintligo 1:f9658c7309ef 202 }
glintligo 1:f9658c7309ef 203
glintligo 1:f9658c7309ef 204 float PID::getPParam() {
glintligo 1:f9658c7309ef 205
glintligo 1:f9658c7309ef 206 return pParam_;
glintligo 1:f9658c7309ef 207
glintligo 1:f9658c7309ef 208 }
glintligo 1:f9658c7309ef 209
glintligo 1:f9658c7309ef 210 float PID::getIParam() {
glintligo 1:f9658c7309ef 211
glintligo 1:f9658c7309ef 212 return iParam_;
glintligo 1:f9658c7309ef 213
glintligo 1:f9658c7309ef 214 }
glintligo 1:f9658c7309ef 215
glintligo 1:f9658c7309ef 216 float PID::getDParam() {
glintligo 1:f9658c7309ef 217
glintligo 1:f9658c7309ef 218 return dParam_;
glintligo 1:f9658c7309ef 219
glintligo 1:f9658c7309ef 220 }