Tony Lin / Mbed 2 deprecated BX-car

hello

Dependencies:   mbed-rtos mbed

Fork of BX-car by kao yi

controller.cpp@14:303b22b76d7a, 2014-06-28 (annotated)

Committer:
TonyLin
Date:
Sat Jun 28 07:32:39 2014 +0000
Revision:
14:303b22b76d7a
Parent:
11:03d5aa2511c4 
Dynamic kp!!!!

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
backman 9:33b99cb45e99 10 setInputLimits(in_min,in_max);
backman 9:33b99cb45e99 11 setOutputLimits(out_min,out_max);
backman 7:fd976e1ced33 12
backman 7:fd976e1ced33 13 tSample_ = interval;
backman 7:fd976e1ced33 14
backman 7:fd976e1ced33 15 setTunings(Kc, tauI, tauD);
backman 7:fd976e1ced33 16
backman 7:fd976e1ced33 17 setPoint_ = 0.0;
backman 7:fd976e1ced33 18 processVariable_ = 0.0;
backman 7:fd976e1ced33 19 prevProcessVariable_ = 0.0;
backman 7:fd976e1ced33 20 controllerOutput_ = 0.0;
backman 7:fd976e1ced33 21 prevControllerOutput_ = 0.0;
backman 7:fd976e1ced33 22
backman 11:03d5aa2511c4 23
backman 7:fd976e1ced33 24
backman 7:fd976e1ced33 25 }
backman 7:fd976e1ced33 26
backman 7:fd976e1ced33 27 void PID::setInputLimits(float inMin, float inMax) {
backman 7:fd976e1ced33 28
backman 7:fd976e1ced33 29 //Make sure we haven't been given impossible values.
backman 7:fd976e1ced33 30 if (inMin >= inMax) {
backman 7:fd976e1ced33 31 return;
backman 7:fd976e1ced33 32 }
backman 7:fd976e1ced33 33
backman 11:03d5aa2511c4 34
backman 7:fd976e1ced33 35
backman 11:03d5aa2511c4 36
backman 7:fd976e1ced33 37
backman 7:fd976e1ced33 38 inMin_ = inMin;
backman 7:fd976e1ced33 39 inMax_ = inMax;
backman 11:03d5aa2511c4 40 inSpan_ = (inMax - inMin);
backman 11:03d5aa2511c4 41
backman 11:03d5aa2511c4 42
backman 11:03d5aa2511c4 43
backman 7:fd976e1ced33 44
backman 7:fd976e1ced33 45 }
backman 7:fd976e1ced33 46
backman 7:fd976e1ced33 47 void PID::setOutputLimits(float outMin, float outMax) {
backman 7:fd976e1ced33 48
backman 7:fd976e1ced33 49 //Make sure we haven't been given impossible values.
backman 7:fd976e1ced33 50 if (outMin >= outMax) {
backman 7:fd976e1ced33 51 return;
backman 7:fd976e1ced33 52 }
backman 7:fd976e1ced33 53
backman 11:03d5aa2511c4 54
backman 7:fd976e1ced33 55
backman 11:03d5aa2511c4 56 //ppp
backman 7:fd976e1ced33 57
backman 7:fd976e1ced33 58 outMin_ = outMin;
backman 7:fd976e1ced33 59 outMax_ = outMax;
backman 11:03d5aa2511c4 60 outMid_ = (outMin+outMax)/2;
backman 11:03d5aa2511c4 61
backman 11:03d5aa2511c4 62 outSpan_ = (outMax - outMin);
backman 11:03d5aa2511c4 63
backman 11:03d5aa2511c4 64
backman 11:03d5aa2511c4 65
backman 11:03d5aa2511c4 66
backman 11:03d5aa2511c4 67
backman 7:fd976e1ced33 68
backman 7:fd976e1ced33 69 }
backman 7:fd976e1ced33 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 void PID::setTunings(float Kc, float tauI, float tauD) {
backman 7:fd976e1ced33 78
backman 7:fd976e1ced33 79 //Verify that the tunings make sense.
backman 7:fd976e1ced33 80 if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
backman 7:fd976e1ced33 81 return;
backman 7:fd976e1ced33 82 }
backman 7:fd976e1ced33 83
backman 7:fd976e1ced33 84 //Store raw values to hand back to user on request.
backman 7:fd976e1ced33 85 pParam_ = Kc;
backman 7:fd976e1ced33 86 iParam_ = tauI;
backman 7:fd976e1ced33 87 dParam_ = tauD;
backman 7:fd976e1ced33 88
backman 11:03d5aa2511c4 89
backman 7:fd976e1ced33 90
backman 11:03d5aa2511c4 91
backman 7:fd976e1ced33 92
backman 7:fd976e1ced33 93 Kc_ = Kc;
backman 11:03d5aa2511c4 94 tauI_=tauI;
backman 11:03d5aa2511c4 95 tauD_=tauD;
backman 7:fd976e1ced33 96
backman 7:fd976e1ced33 97 }
backman 11:03d5aa2511c4 98
backman 11:03d5aa2511c4 99
backman 7:fd976e1ced33 100 float PID::compute(float pv, float sp) {
backman 7:fd976e1ced33 101
backman 11:03d5aa2511c4 102
backman 7:fd976e1ced33 103 processVariable_ = pv; //ce que l'on mesure
backman 7:fd976e1ced33 104 setPoint_ = sp; // ce que l'on veut atteindre
backman 7:fd976e1ced33 105
backman 11:03d5aa2511c4 106
backman 11:03d5aa2511c4 107
backman 11:03d5aa2511c4 108
backman 11:03d5aa2511c4 109
backman 11:03d5aa2511c4 110
backman 7:fd976e1ced33 111 //Pull in the input and setpoint, and scale them into percent span.
backman 7:fd976e1ced33 112 float scaledPV = (processVariable_ - inMin_) / inSpan_;
backman 7:fd976e1ced33 113
backman 7:fd976e1ced33 114 if (scaledPV > 1.0) {
backman 7:fd976e1ced33 115 scaledPV = 1.0;
backman 7:fd976e1ced33 116 }
backman 7:fd976e1ced33 117 else if (scaledPV < 0.0) {
backman 7:fd976e1ced33 118 scaledPV = 0.0;
backman 7:fd976e1ced33 119 }
backman 7:fd976e1ced33 120
backman 7:fd976e1ced33 121 float scaledSP = (setPoint_ - inMin_) / inSpan_;
backman 7:fd976e1ced33 122 if (scaledSP > 1.0) {
backman 7:fd976e1ced33 123 scaledSP = 1;
backman 7:fd976e1ced33 124 }
backman 7:fd976e1ced33 125 else if (scaledSP < 0.0) {
backman 7:fd976e1ced33 126 scaledSP = 0;
backman 7:fd976e1ced33 127 }
backman 7:fd976e1ced33 128
backman 11:03d5aa2511c4 129 float error = (scaledSP - scaledPV)*2;
backman 11:03d5aa2511c4 130 // 100~ -100%
backman 11:03d5aa2511c4 131
backman 11:03d5aa2511c4 132
backman 8:8e49e21d80a2 133
backman 11:03d5aa2511c4 134
backman 11:03d5aa2511c4 135 // if error add
backman 11:03d5aa2511c4 136 if ( (-1<error && error <1) ) {
backman 11:03d5aa2511c4 137 accError_ = (accError_*0.66)+ error;
backman 7:fd976e1ced33 138 }
backman 11:03d5aa2511c4 139
backman 11:03d5aa2511c4 140 float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
backman 11:03d5aa2511c4 141
backman 11:03d5aa2511c4 142 controllerOutput_= Kc_* error+(tauI_*accError_)+tauD_*dMeas+outMid_;
backman 11:03d5aa2511c4 143 de_ip=accError_;
backman 11:03d5aa2511c4 144 de_dp=dMeas;
backman 11:03d5aa2511c4 145 de_kp=error;
backman 11:03d5aa2511c4 146
backman 11:03d5aa2511c4 147
backman 11:03d5aa2511c4 148
backman 11:03d5aa2511c4 149
backman 11:03d5aa2511c4 150
backman 11:03d5aa2511c4 151
backman 11:03d5aa2511c4 152 if (controllerOutput_ < outMin_) {
backman 9:33b99cb45e99 153 controllerOutput_ = outMin_;
backman 7:fd976e1ced33 154 }
backman 11:03d5aa2511c4 155 else if (controllerOutput_ > outMax_) {
backman 9:33b99cb45e99 156 controllerOutput_ = outMax_;
backman 7:fd976e1ced33 157 }
backman 11:03d5aa2511c4 158
backman 11:03d5aa2511c4 159
backman 11:03d5aa2511c4 160
backman 11:03d5aa2511c4 161
backman 11:03d5aa2511c4 162 prevProcessVariable_ = scaledPV;
backman 8:8e49e21d80a2 163
backman 8:8e49e21d80a2 164
backman 11:03d5aa2511c4 165 return (controllerOutput_);
backman 7:fd976e1ced33 166
backman 7:fd976e1ced33 167 }
backman 7:fd976e1ced33 168
backman 7:fd976e1ced33 169 float PID::getInMin() {
backman 7:fd976e1ced33 170
backman 7:fd976e1ced33 171 return inMin_;
backman 7:fd976e1ced33 172
backman 7:fd976e1ced33 173 }
backman 7:fd976e1ced33 174
backman 7:fd976e1ced33 175 float PID::getInMax() {
backman 7:fd976e1ced33 176
backman 7:fd976e1ced33 177 return inMax_;
backman 7:fd976e1ced33 178
backman 7:fd976e1ced33 179 }
backman 7:fd976e1ced33 180
backman 7:fd976e1ced33 181 float PID::getOutMin() {
backman 7:fd976e1ced33 182
backman 7:fd976e1ced33 183 return outMin_;
backman 7:fd976e1ced33 184
backman 7:fd976e1ced33 185 }
backman 7:fd976e1ced33 186
backman 7:fd976e1ced33 187 float PID::getOutMax() {
backman 7:fd976e1ced33 188
backman 7:fd976e1ced33 189 return outMax_;
backman 7:fd976e1ced33 190
backman 7:fd976e1ced33 191 }
backman 7:fd976e1ced33 192
backman 7:fd976e1ced33 193 float PID::getInterval() {
backman 7:fd976e1ced33 194
backman 7:fd976e1ced33 195 return tSample_;
backman 7:fd976e1ced33 196
backman 7:fd976e1ced33 197 }
backman 7:fd976e1ced33 198
backman 7:fd976e1ced33 199 float PID::getPParam() {
backman 7:fd976e1ced33 200
backman 7:fd976e1ced33 201 return pParam_;
backman 7:fd976e1ced33 202
backman 7:fd976e1ced33 203 }
backman 7:fd976e1ced33 204
backman 7:fd976e1ced33 205 float PID::getIParam() {
backman 7:fd976e1ced33 206
backman 7:fd976e1ced33 207 return iParam_;
backman 7:fd976e1ced33 208
backman 7:fd976e1ced33 209 }
backman 7:fd976e1ced33 210
backman 7:fd976e1ced33 211 float PID::getDParam() {
backman 7:fd976e1ced33 212
backman 7:fd976e1ced33 213 return dParam_;
backman 7:fd976e1ced33 214
backman 7:fd976e1ced33 215 }
backman 7:fd976e1ced33 216