Clark Lin / Mbed 2 deprecated BX-car

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of BX-car by kao yi

controller.cpp@12:f7acda4545ba, 2014-06-30 (annotated)

Committer:
physicsgood
Date:
Mon Jun 30 02:07:57 2014 +0000
Revision:
12:f7acda4545ba
Parent:
10:d2401a243e8d
Child:
15:3fa780990a6a
QQQ

Who changed what in which revision?

UserRevisionLine numberNew contents of line
backman 7:fd976e1ced33 1 #include "mbed.h"
backman 7:fd976e1ced33 2 #include "controller.h"
backman 7:fd976e1ced33 3
backman 7:fd976e1ced33 4
physicsgood 12:f7acda4545ba 5 PID::PID(float in_min,float in_max,float out_min,float out_max,float Kp, float Ki, float Kd) {
physicsgood 12:f7acda4545ba 6 //in_min in_out camera array
physicsgood 12:f7acda4545ba 7 //out_min out_max servo range
physicsgood 12:f7acda4545ba 8 //
backman 7:fd976e1ced33 9 usingFeedForward = false;
backman 7:fd976e1ced33 10 //inAuto = false;
backman 7:fd976e1ced33 11
backman 7:fd976e1ced33 12 //Default the limits to the full range of I/O.
backman 7:fd976e1ced33 13 //Make sure to set these to more appropriate limits for your application.
backman 9:33b99cb45e99 14
backman 9:33b99cb45e99 15 //BX tune
physicsgood 12:f7acda4545ba 16 /*setInputLimits(in_min,in_max);
backman 9:33b99cb45e99 17 setOutputLimits(out_min,out_max);
backman 7:fd976e1ced33 18
physicsgood 12:f7acda4545ba 19 tSample_ = interval;// init is 10
backman 7:fd976e1ced33 20
backman 7:fd976e1ced33 21 setTunings(Kc, tauI, tauD);
backman 7:fd976e1ced33 22
backman 7:fd976e1ced33 23 setPoint_ = 0.0;
backman 7:fd976e1ced33 24 processVariable_ = 0.0;
backman 7:fd976e1ced33 25 prevProcessVariable_ = 0.0;
backman 7:fd976e1ced33 26 controllerOutput_ = 0.0;
backman 7:fd976e1ced33 27 prevControllerOutput_ = 0.0;
backman 7:fd976e1ced33 28
backman 7:fd976e1ced33 29 accError_ = 0.0;
backman 7:fd976e1ced33 30 bias_ = 0.0;
backman 7:fd976e1ced33 31
physicsgood 12:f7acda4545ba 32 realOutput_ = 0.0;*/
backman 7:fd976e1ced33 33
backman 7:fd976e1ced33 34 }
backman 7:fd976e1ced33 35
backman 7:fd976e1ced33 36 void PID::setInputLimits(float inMin, float inMax) {
backman 7:fd976e1ced33 37
backman 7:fd976e1ced33 38 //Make sure we haven't been given impossible values.
backman 7:fd976e1ced33 39 if (inMin >= inMax) {
backman 7:fd976e1ced33 40 return;
backman 7:fd976e1ced33 41 }
backman 7:fd976e1ced33 42
backman 7:fd976e1ced33 43 //Rescale the working variables to reflect the changes.
backman 7:fd976e1ced33 44 prevProcessVariable_ *= (inMax - inMin) / inSpan_;
backman 7:fd976e1ced33 45 accError_ *= (inMax - inMin) / inSpan_;
backman 7:fd976e1ced33 46
backman 7:fd976e1ced33 47 //Make sure the working variables are within the new limits.
backman 7:fd976e1ced33 48 if (prevProcessVariable_ > 1) {
backman 7:fd976e1ced33 49 prevProcessVariable_ = 1;
backman 7:fd976e1ced33 50 }
backman 7:fd976e1ced33 51 else if (prevProcessVariable_ < 0) {
backman 7:fd976e1ced33 52 prevProcessVariable_ = 0;
backman 7:fd976e1ced33 53 }
backman 7:fd976e1ced33 54
backman 7:fd976e1ced33 55 inMin_ = inMin;
backman 7:fd976e1ced33 56 inMax_ = inMax;
backman 7:fd976e1ced33 57 inSpan_ = inMax - inMin;
backman 7:fd976e1ced33 58
backman 7:fd976e1ced33 59 }
backman 7:fd976e1ced33 60
backman 7:fd976e1ced33 61 void PID::setOutputLimits(float outMin, float outMax) {
backman 7:fd976e1ced33 62
backman 7:fd976e1ced33 63 //Make sure we haven't been given impossible values.
backman 7:fd976e1ced33 64 if (outMin >= outMax) {
backman 7:fd976e1ced33 65 return;
backman 7:fd976e1ced33 66 }
backman 7:fd976e1ced33 67
backman 7:fd976e1ced33 68 //Rescale the working variables to reflect the changes.
backman 7:fd976e1ced33 69 prevControllerOutput_ *= (outMax - outMin) / outSpan_;
backman 7:fd976e1ced33 70
backman 7:fd976e1ced33 71 //Make sure the working variables are within the new limits.
backman 7:fd976e1ced33 72 if (prevControllerOutput_ > 1) {
backman 7:fd976e1ced33 73 prevControllerOutput_ = 1;
backman 7:fd976e1ced33 74 }
backman 7:fd976e1ced33 75 else if (prevControllerOutput_ < 0) {
backman 7:fd976e1ced33 76 prevControllerOutput_ = 0;
backman 7:fd976e1ced33 77 }
backman 7:fd976e1ced33 78
backman 7:fd976e1ced33 79 outMin_ = outMin;
backman 7:fd976e1ced33 80 outMax_ = outMax;
backman 7:fd976e1ced33 81 outSpan_ = outMax - outMin;
backman 7:fd976e1ced33 82
backman 7:fd976e1ced33 83 }
backman 7:fd976e1ced33 84
backman 7:fd976e1ced33 85
backman 7:fd976e1ced33 86
backman 7:fd976e1ced33 87 //--------------------------------------------------
backman 7:fd976e1ced33 88
backman 7:fd976e1ced33 89
backman 7:fd976e1ced33 90
backman 7:fd976e1ced33 91 void PID::setTunings(float Kc, float tauI, float tauD) {
backman 7:fd976e1ced33 92
backman 7:fd976e1ced33 93 //Verify that the tunings make sense.
backman 7:fd976e1ced33 94 if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
backman 7:fd976e1ced33 95 return;
backman 7:fd976e1ced33 96 }
backman 7:fd976e1ced33 97
backman 7:fd976e1ced33 98 //Store raw values to hand back to user on request.
backman 7:fd976e1ced33 99 pParam_ = Kc;
backman 7:fd976e1ced33 100 iParam_ = tauI;
backman 7:fd976e1ced33 101 dParam_ = tauD;
backman 7:fd976e1ced33 102
backman 7:fd976e1ced33 103 float tempTauR;
backman 7:fd976e1ced33 104
backman 7:fd976e1ced33 105 if (tauI == 0.0) {
backman 7:fd976e1ced33 106 tempTauR = 0.0;
backman 7:fd976e1ced33 107 }
backman 7:fd976e1ced33 108 else {
backman 7:fd976e1ced33 109 tempTauR = (1.0 / tauI) * tSample_;
backman 7:fd976e1ced33 110 }
backman 7:fd976e1ced33 111
backman 7:fd976e1ced33 112 //For "bumpless transfer" we need to rescale the accumulated error.
backman 7:fd976e1ced33 113 //if (inAuto) {
backman 7:fd976e1ced33 114 //if (tempTauR == 0.0) {
backman 7:fd976e1ced33 115 //accError_ = 0.0;
backman 7:fd976e1ced33 116 //}
backman 7:fd976e1ced33 117 //else {
backman 7:fd976e1ced33 118 accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
backman 7:fd976e1ced33 119 //}
backman 7:fd976e1ced33 120 //}
backman 7:fd976e1ced33 121
backman 7:fd976e1ced33 122 Kc_ = Kc;
backman 7:fd976e1ced33 123 tauR_ = tempTauR;
backman 7:fd976e1ced33 124 tauD_ = tauD / tSample_;
backman 7:fd976e1ced33 125
backman 7:fd976e1ced33 126 }
backman 7:fd976e1ced33 127
backman 7:fd976e1ced33 128 void PID::reset(void) {
backman 7:fd976e1ced33 129
backman 7:fd976e1ced33 130 float scaledBias = 0.0;
backman 7:fd976e1ced33 131
backman 7:fd976e1ced33 132 if (usingFeedForward) {
backman 7:fd976e1ced33 133 scaledBias = (bias_ - outMin_) / outSpan_;
backman 7:fd976e1ced33 134 }
backman 7:fd976e1ced33 135 else {
backman 7:fd976e1ced33 136 scaledBias = (realOutput_ - outMin_) / outSpan_;
backman 7:fd976e1ced33 137 }
backman 7:fd976e1ced33 138
backman 7:fd976e1ced33 139 prevControllerOutput_ = scaledBias;
backman 7:fd976e1ced33 140 prevProcessVariable_ = (processVariable_ - inMin_) / inSpan_;
backman 7:fd976e1ced33 141
backman 7:fd976e1ced33 142 //Clear any error in the integral.
backman 7:fd976e1ced33 143 accError_ = 0;
backman 7:fd976e1ced33 144
backman 7:fd976e1ced33 145 }
backman 7:fd976e1ced33 146 /*
backman 7:fd976e1ced33 147 void PID::setMode(int mode) {
backman 7:fd976e1ced33 148
backman 7:fd976e1ced33 149 //We were in manual, and we just got set to auto.
backman 7:fd976e1ced33 150 //Reset the controller internals.
backman 7:fd976e1ced33 151 if (mode != 0 && !inAuto) {
backman 7:fd976e1ced33 152 reset();
backman 7:fd976e1ced33 153 }
backman 7:fd976e1ced33 154
backman 7:fd976e1ced33 155 inAuto = (mode != 0);
backman 7:fd976e1ced33 156
backman 7:fd976e1ced33 157 }*/
backman 7:fd976e1ced33 158
backman 7:fd976e1ced33 159 void PID::setInterval(float interval) {
backman 7:fd976e1ced33 160
backman 7:fd976e1ced33 161 if (interval > 0) {
backman 7:fd976e1ced33 162 //Convert the time-based tunings to reflect this change.
backman 7:fd976e1ced33 163 tauR_ *= (interval / tSample_);
backman 7:fd976e1ced33 164 accError_ *= (tSample_ / interval);
backman 7:fd976e1ced33 165 tauD_ *= (interval / tSample_);
backman 7:fd976e1ced33 166 tSample_ = interval;
backman 7:fd976e1ced33 167 }
backman 7:fd976e1ced33 168
backman 7:fd976e1ced33 169 }
backman 7:fd976e1ced33 170 /*
backman 7:fd976e1ced33 171 void PID::setSetPoint(float sp) {
backman 7:fd976e1ced33 172
backman 7:fd976e1ced33 173 setPoint_ = sp;
backman 7:fd976e1ced33 174
backman 7:fd976e1ced33 175 }
backman 7:fd976e1ced33 176
backman 7:fd976e1ced33 177 void PID::setProcessValue(float pv) {
backman 7:fd976e1ced33 178
backman 7:fd976e1ced33 179 processVariable_ = pv;
backman 7:fd976e1ced33 180
backman 7:fd976e1ced33 181 }
backman 7:fd976e1ced33 182 */
backman 7:fd976e1ced33 183 void PID::setBias(float bias){
backman 7:fd976e1ced33 184
backman 7:fd976e1ced33 185 bias_ = bias;
backman 7:fd976e1ced33 186 usingFeedForward = 1;
backman 7:fd976e1ced33 187
backman 7:fd976e1ced33 188 }
backman 7:fd976e1ced33 189
physicsgood 12:f7acda4545ba 190 float PID::compute(float R ,float L, float sp) {
physicsgood 10:d2401a243e8d 191 //pv centerR centerL
backman 7:fd976e1ced33 192 //enregistrer variables dans var interne
physicsgood 12:f7acda4545ba 193 float centerR = R; //(right + left) / 2
physicsgood 12:f7acda4545ba 194 float centerL = L;
physicsgood 12:f7acda4545ba 195 float centerB = sp; // center of black
physicsgood 12:f7acda4545ba 196 float error = centerB - centerR;
physicsgood 12:f7acda4545ba 197 if(error < -8 && error >8){
physicsgood 12:f7acda4545ba 198 return 0.085;
backman 7:fd976e1ced33 199 }
physicsgood 12:f7acda4545ba 200 else if((error < -8 && error > -20)||(error > 8 && error < 40)){
physicsgood 12:f7acda4545ba 201 return 0.085 + Kp*error;
backman 7:fd976e1ced33 202 }
physicsgood 12:f7acda4545ba 203 else return 0.085;
backman 8:8e49e21d80a2 204
physicsgood 12:f7acda4545ba 205
physicsgood 12:f7acda4545ba 206
backman 7:fd976e1ced33 207
backman 7:fd976e1ced33 208 }
backman 7:fd976e1ced33 209
backman 7:fd976e1ced33 210 float PID::getInMin() {
backman 7:fd976e1ced33 211
backman 7:fd976e1ced33 212 return inMin_;
backman 7:fd976e1ced33 213
backman 7:fd976e1ced33 214 }
backman 7:fd976e1ced33 215
backman 7:fd976e1ced33 216 float PID::getInMax() {
backman 7:fd976e1ced33 217
backman 7:fd976e1ced33 218 return inMax_;
backman 7:fd976e1ced33 219
backman 7:fd976e1ced33 220 }
backman 7:fd976e1ced33 221
backman 7:fd976e1ced33 222 float PID::getOutMin() {
backman 7:fd976e1ced33 223
backman 7:fd976e1ced33 224 return outMin_;
backman 7:fd976e1ced33 225
backman 7:fd976e1ced33 226 }
backman 7:fd976e1ced33 227
backman 7:fd976e1ced33 228 float PID::getOutMax() {
backman 7:fd976e1ced33 229
backman 7:fd976e1ced33 230 return outMax_;
backman 7:fd976e1ced33 231
backman 7:fd976e1ced33 232 }
backman 7:fd976e1ced33 233
backman 7:fd976e1ced33 234 float PID::getInterval() {
backman 7:fd976e1ced33 235
backman 7:fd976e1ced33 236 return tSample_;
backman 7:fd976e1ced33 237
backman 7:fd976e1ced33 238 }
backman 7:fd976e1ced33 239
backman 7:fd976e1ced33 240 float PID::getPParam() {
backman 7:fd976e1ced33 241
backman 7:fd976e1ced33 242 return pParam_;
backman 7:fd976e1ced33 243
backman 7:fd976e1ced33 244 }
backman 7:fd976e1ced33 245
backman 7:fd976e1ced33 246 float PID::getIParam() {
backman 7:fd976e1ced33 247
backman 7:fd976e1ced33 248 return iParam_;
backman 7:fd976e1ced33 249
backman 7:fd976e1ced33 250 }
backman 7:fd976e1ced33 251
backman 7:fd976e1ced33 252 float PID::getDParam() {
backman 7:fd976e1ced33 253
backman 7:fd976e1ced33 254 return dParam_;
backman 7:fd976e1ced33 255
backman 7:fd976e1ced33 256 }
backman 7:fd976e1ced33 257