Clark Lin / Mbed 2 deprecated BX-car_s

QQQ

Dependencies:   mbed-rtos mbed

Fork of BX-car_s by Tony Lin

controller.cpp@23:d6d4e8adf7fe, 2014-07-02 (annotated)

Committer:
physicsgood
Date:
Wed Jul 02 13:33:49 2014 +0000
Revision:
23:d6d4e8adf7fe
Parent:
22:1464a3f0a290 
QQQQ

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
backman 11:03d5aa2511c4 5
backman 11:03d5aa2511c4 6
backman 9:33b99cb45e99 7 PID::PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval) {
backman 7:fd976e1ced33 8
backman 9:33b99cb45e99 9 //BX tune
physicsgood 23:d6d4e8adf7fe 10 Kp = 0.0005;
physicsgood 21:5f7efc1ca8ad 11 Ki = 0.0;
physicsgood 21:5f7efc1ca8ad 12 Kd = 0.0;
backman 9:33b99cb45e99 13 setInputLimits(in_min,in_max);
backman 9:33b99cb45e99 14 setOutputLimits(out_min,out_max);
backman 7:fd976e1ced33 15
backman 7:fd976e1ced33 16 tSample_ = interval;
backman 7:fd976e1ced33 17
backman 7:fd976e1ced33 18 setTunings(Kc, tauI, tauD);
backman 7:fd976e1ced33 19
backman 7:fd976e1ced33 20 setPoint_ = 0.0;
backman 7:fd976e1ced33 21 processVariable_ = 0.0;
backman 7:fd976e1ced33 22 prevProcessVariable_ = 0.0;
backman 7:fd976e1ced33 23 controllerOutput_ = 0.0;
backman 7:fd976e1ced33 24 prevControllerOutput_ = 0.0;
backman 7:fd976e1ced33 25
backman 11:03d5aa2511c4 26
backman 7:fd976e1ced33 27
backman 7:fd976e1ced33 28 }
backman 7:fd976e1ced33 29
backman 7:fd976e1ced33 30 void PID::setInputLimits(float inMin, float inMax) {
backman 7:fd976e1ced33 31
backman 7:fd976e1ced33 32 //Make sure we haven't been given impossible values.
backman 7:fd976e1ced33 33 if (inMin >= inMax) {
backman 7:fd976e1ced33 34 return;
backman 7:fd976e1ced33 35 }
backman 7:fd976e1ced33 36
backman 11:03d5aa2511c4 37
backman 7:fd976e1ced33 38
backman 11:03d5aa2511c4 39
backman 7:fd976e1ced33 40
backman 7:fd976e1ced33 41 inMin_ = inMin;
backman 7:fd976e1ced33 42 inMax_ = inMax;
backman 11:03d5aa2511c4 43 inSpan_ = (inMax - inMin);
backman 11:03d5aa2511c4 44
backman 11:03d5aa2511c4 45
backman 11:03d5aa2511c4 46
backman 7:fd976e1ced33 47
backman 7:fd976e1ced33 48 }
backman 7:fd976e1ced33 49
backman 7:fd976e1ced33 50 void PID::setOutputLimits(float outMin, float outMax) {
backman 7:fd976e1ced33 51
backman 7:fd976e1ced33 52 //Make sure we haven't been given impossible values.
backman 7:fd976e1ced33 53 if (outMin >= outMax) {
backman 7:fd976e1ced33 54 return;
backman 7:fd976e1ced33 55 }
backman 7:fd976e1ced33 56
backman 11:03d5aa2511c4 57
backman 7:fd976e1ced33 58
backman 11:03d5aa2511c4 59 //ppp
backman 7:fd976e1ced33 60
backman 7:fd976e1ced33 61 outMin_ = outMin;
backman 7:fd976e1ced33 62 outMax_ = outMax;
backman 11:03d5aa2511c4 63 outMid_ = (outMin+outMax)/2;
backman 11:03d5aa2511c4 64
backman 11:03d5aa2511c4 65 outSpan_ = (outMax - outMin);
backman 11:03d5aa2511c4 66
backman 11:03d5aa2511c4 67
backman 11:03d5aa2511c4 68
backman 11:03d5aa2511c4 69
backman 11:03d5aa2511c4 70
backman 7:fd976e1ced33 71
backman 7:fd976e1ced33 72 }
backman 7:fd976e1ced33 73
backman 7:fd976e1ced33 74
backman 7:fd976e1ced33 75
backman 7:fd976e1ced33 76 //--------------------------------------------------
backman 7:fd976e1ced33 77
backman 7:fd976e1ced33 78
backman 7:fd976e1ced33 79
backman 7:fd976e1ced33 80 void PID::setTunings(float Kc, float tauI, float tauD) {
backman 7:fd976e1ced33 81
backman 7:fd976e1ced33 82 //Verify that the tunings make sense.
backman 7:fd976e1ced33 83 if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
backman 7:fd976e1ced33 84 return;
backman 7:fd976e1ced33 85 }
backman 7:fd976e1ced33 86
backman 7:fd976e1ced33 87 //Store raw values to hand back to user on request.
backman 7:fd976e1ced33 88 pParam_ = Kc;
backman 7:fd976e1ced33 89 iParam_ = tauI;
backman 7:fd976e1ced33 90 dParam_ = tauD;
backman 7:fd976e1ced33 91
backman 11:03d5aa2511c4 92
backman 7:fd976e1ced33 93
backman 11:03d5aa2511c4 94
backman 7:fd976e1ced33 95
backman 7:fd976e1ced33 96 Kc_ = Kc;
backman 11:03d5aa2511c4 97 tauI_=tauI;
backman 11:03d5aa2511c4 98 tauD_=tauD;
backman 7:fd976e1ced33 99
backman 7:fd976e1ced33 100 }
backman 11:03d5aa2511c4 101
backman 11:03d5aa2511c4 102
physicsgood 21:5f7efc1ca8ad 103
backman 7:fd976e1ced33 104
physicsgood 21:5f7efc1ca8ad 105
physicsgood 21:5f7efc1ca8ad 106
backman 11:03d5aa2511c4 107
backman 8:8e49e21d80a2 108
backman 11:03d5aa2511c4 109
backman 11:03d5aa2511c4 110
backman 11:03d5aa2511c4 111
backman 11:03d5aa2511c4 112
backman 11:03d5aa2511c4 113
backman 11:03d5aa2511c4 114
backman 11:03d5aa2511c4 115
backman 11:03d5aa2511c4 116
physicsgood 21:5f7efc1ca8ad 117
backman 11:03d5aa2511c4 118
backman 11:03d5aa2511c4 119
physicsgood 21:5f7efc1ca8ad 120
backman 8:8e49e21d80a2 121
backman 8:8e49e21d80a2 122
physicsgood 23:d6d4e8adf7fe 123 float PID::compute(int centerL, int centerR , int sp) {
physicsgood 22:1464a3f0a290 124 //turn right 122~64 122
physicsgood 22:1464a3f0a290 125 //turn left 64~6 8
physicsgood 23:d6d4e8adf7fe 126 int errorR = sp - centerR;
physicsgood 23:d6d4e8adf7fe 127 int errorL = sp - centerL;
physicsgood 23:d6d4e8adf7fe 128 int error;
physicsgood 23:d6d4e8adf7fe 129 if(centerL ==128 && centerR == 0){
physicsgood 23:d6d4e8adf7fe 130 error = 0;
physicsgood 23:d6d4e8adf7fe 131 }
physicsgood 23:d6d4e8adf7fe 132 if (centerL == 128 && centerR != 0){
physicsgood 23:d6d4e8adf7fe 133 error = -30;
physicsgood 23:d6d4e8adf7fe 134 }
physicsgood 23:d6d4e8adf7fe 135 else if (centerL != 128 && centerR == 0){
physicsgood 23:d6d4e8adf7fe 136 error = 30;
physicsgood 23:d6d4e8adf7fe 137 }
physicsgood 23:d6d4e8adf7fe 138 else{
physicsgood 23:d6d4e8adf7fe 139 if(errorR >= 0 && errorL >=0){
physicsgood 23:d6d4e8adf7fe 140 if(errorR > errorL){
physicsgood 23:d6d4e8adf7fe 141 if(errorR > -5 && errorR < 5)
physicsgood 23:d6d4e8adf7fe 142 error = 0;
physicsgood 23:d6d4e8adf7fe 143 else
physicsgood 23:d6d4e8adf7fe 144 error = errorR;
physicsgood 23:d6d4e8adf7fe 145 }
physicsgood 23:d6d4e8adf7fe 146 else{
physicsgood 23:d6d4e8adf7fe 147 if(errorL > -5 && errorL < 5)
physicsgood 23:d6d4e8adf7fe 148 error = 0;
physicsgood 23:d6d4e8adf7fe 149 else
physicsgood 23:d6d4e8adf7fe 150 error = errorL;
physicsgood 23:d6d4e8adf7fe 151 }
physicsgood 23:d6d4e8adf7fe 152
physicsgood 23:d6d4e8adf7fe 153 }
physicsgood 23:d6d4e8adf7fe 154 else if(errorR < 0 && errorL >=0){
physicsgood 23:d6d4e8adf7fe 155 if(-errorR > errorL){
physicsgood 23:d6d4e8adf7fe 156 if(errorR > -5 && errorR < 5)
physicsgood 23:d6d4e8adf7fe 157 error = 0;
physicsgood 23:d6d4e8adf7fe 158 else
physicsgood 23:d6d4e8adf7fe 159 error = errorR;
physicsgood 23:d6d4e8adf7fe 160 }
physicsgood 23:d6d4e8adf7fe 161 else{
physicsgood 23:d6d4e8adf7fe 162 if(errorL > -5 && errorL < 5)
physicsgood 23:d6d4e8adf7fe 163 error = 0;
physicsgood 23:d6d4e8adf7fe 164 else
physicsgood 23:d6d4e8adf7fe 165 error = errorL;
physicsgood 23:d6d4e8adf7fe 166 }
physicsgood 23:d6d4e8adf7fe 167 }
physicsgood 23:d6d4e8adf7fe 168 else if(errorR >= 0 && errorL <0){
physicsgood 23:d6d4e8adf7fe 169 if(errorR > -errorL){
physicsgood 23:d6d4e8adf7fe 170 if(errorR > -5 && errorR < 5)
physicsgood 23:d6d4e8adf7fe 171 error = 0;
physicsgood 23:d6d4e8adf7fe 172 else
physicsgood 23:d6d4e8adf7fe 173 error = errorR;
physicsgood 23:d6d4e8adf7fe 174 }
physicsgood 23:d6d4e8adf7fe 175 else{
physicsgood 23:d6d4e8adf7fe 176 if(errorL > -5 && errorL < 5)
physicsgood 23:d6d4e8adf7fe 177 error = 0;
physicsgood 23:d6d4e8adf7fe 178 else
physicsgood 23:d6d4e8adf7fe 179 error = errorL;
physicsgood 23:d6d4e8adf7fe 180 }
physicsgood 23:d6d4e8adf7fe 181 }
physicsgood 23:d6d4e8adf7fe 182 else{
physicsgood 23:d6d4e8adf7fe 183 if(errorR > errorL){
physicsgood 23:d6d4e8adf7fe 184 if(errorL > -5 && errorL < 5)
physicsgood 23:d6d4e8adf7fe 185 error = 0;
physicsgood 23:d6d4e8adf7fe 186 else
physicsgood 23:d6d4e8adf7fe 187 error = errorL;
physicsgood 23:d6d4e8adf7fe 188 }
physicsgood 23:d6d4e8adf7fe 189 else{
physicsgood 23:d6d4e8adf7fe 190 if(errorR > -5 && errorR < 5)
physicsgood 23:d6d4e8adf7fe 191 error = 0;
physicsgood 23:d6d4e8adf7fe 192 else
physicsgood 23:d6d4e8adf7fe 193 error = errorR;
physicsgood 23:d6d4e8adf7fe 194 }
physicsgood 23:d6d4e8adf7fe 195 }
physicsgood 23:d6d4e8adf7fe 196 }
physicsgood 23:d6d4e8adf7fe 197 return 0.069 + Kp*error;
physicsgood 23:d6d4e8adf7fe 198 }
physicsgood 23:d6d4e8adf7fe 199
physicsgood 23:d6d4e8adf7fe 200 int PID::getCenter(int centerL, int centerR) {
physicsgood 23:d6d4e8adf7fe 201 //turn right 122~64 122
physicsgood 23:d6d4e8adf7fe 202 //turn left 64~6 8
physicsgood 23:d6d4e8adf7fe 203
physicsgood 23:d6d4e8adf7fe 204 int result = 64;
physicsgood 23:d6d4e8adf7fe 205 int errorR = 64 - centerR;
physicsgood 23:d6d4e8adf7fe 206 int errorL = 64 - centerL;
physicsgood 23:d6d4e8adf7fe 207 int negL = 1 , negR = 1;
physicsgood 23:d6d4e8adf7fe 208
physicsgood 23:d6d4e8adf7fe 209
physicsgood 23:d6d4e8adf7fe 210
physicsgood 23:d6d4e8adf7fe 211
physicsgood 23:d6d4e8adf7fe 212 if(centerL == 128 && centerR == 0){// no black line
physicsgood 23:d6d4e8adf7fe 213
physicsgood 23:d6d4e8adf7fe 214 result = 64;
physicsgood 23:d6d4e8adf7fe 215
physicsgood 23:d6d4e8adf7fe 216 } else if(centerL == 128 && centerR != 0){// no left line
physicsgood 23:d6d4e8adf7fe 217
physicsgood 23:d6d4e8adf7fe 218 result = centerR;
physicsgood 23:d6d4e8adf7fe 219
physicsgood 23:d6d4e8adf7fe 220
physicsgood 23:d6d4e8adf7fe 221 } else if(centerL != 128 && centerR == 0){// no right line
physicsgood 23:d6d4e8adf7fe 222
physicsgood 23:d6d4e8adf7fe 223 result = centerL;
physicsgood 23:d6d4e8adf7fe 224
physicsgood 23:d6d4e8adf7fe 225 } else{// two black lines
physicsgood 23:d6d4e8adf7fe 226
physicsgood 23:d6d4e8adf7fe 227 negL = (errorL >= 0) ? 1 : -1;
physicsgood 23:d6d4e8adf7fe 228 negR = (errorR >= 0) ? 1 : -1;
physicsgood 23:d6d4e8adf7fe 229 errorL *= negL;
physicsgood 23:d6d4e8adf7fe 230 errorR *= negR;
physicsgood 23:d6d4e8adf7fe 231
physicsgood 23:d6d4e8adf7fe 232 result = (errorL >= errorR) ? centerL : centerR;
physicsgood 23:d6d4e8adf7fe 233 }
backman 7:fd976e1ced33 234 }
backman 7:fd976e1ced33 235
backman 7:fd976e1ced33 236 float PID::getInMin() {
backman 7:fd976e1ced33 237
backman 7:fd976e1ced33 238 return inMin_;
backman 7:fd976e1ced33 239
backman 7:fd976e1ced33 240 }
backman 7:fd976e1ced33 241
backman 7:fd976e1ced33 242 float PID::getInMax() {
backman 7:fd976e1ced33 243
backman 7:fd976e1ced33 244 return inMax_;
backman 7:fd976e1ced33 245
backman 7:fd976e1ced33 246 }
backman 7:fd976e1ced33 247
backman 7:fd976e1ced33 248 float PID::getOutMin() {
backman 7:fd976e1ced33 249
backman 7:fd976e1ced33 250 return outMin_;
backman 7:fd976e1ced33 251
backman 7:fd976e1ced33 252 }
backman 7:fd976e1ced33 253
backman 7:fd976e1ced33 254 float PID::getOutMax() {
backman 7:fd976e1ced33 255
backman 7:fd976e1ced33 256 return outMax_;
backman 7:fd976e1ced33 257
backman 7:fd976e1ced33 258 }
backman 7:fd976e1ced33 259
backman 7:fd976e1ced33 260 float PID::getInterval() {
backman 7:fd976e1ced33 261
backman 7:fd976e1ced33 262 return tSample_;
backman 7:fd976e1ced33 263
backman 7:fd976e1ced33 264 }
backman 7:fd976e1ced33 265
backman 7:fd976e1ced33 266 float PID::getPParam() {
backman 7:fd976e1ced33 267
backman 7:fd976e1ced33 268 return pParam_;
backman 7:fd976e1ced33 269
backman 7:fd976e1ced33 270 }
backman 7:fd976e1ced33 271
backman 7:fd976e1ced33 272 float PID::getIParam() {
backman 7:fd976e1ced33 273
backman 7:fd976e1ced33 274 return iParam_;
backman 7:fd976e1ced33 275
backman 7:fd976e1ced33 276 }
backman 7:fd976e1ced33 277
backman 7:fd976e1ced33 278 float PID::getDParam() {
backman 7:fd976e1ced33 279
backman 7:fd976e1ced33 280 return dParam_;
backman 7:fd976e1ced33 281
backman 7:fd976e1ced33 282 }
backman 7:fd976e1ced33 283