Hesu-eco / Mbed 2 deprecated limiteur_vitesse

premiere ebauche

Dependencies:   mbed PinDetect

speedlimiter.cpp@6:1a4ed88c9a4b, 2018-10-25 (annotated)

Committer:
shovelcat
Date:
Thu Oct 25 22:30:50 2018 +0000
Revision:
6:1a4ed88c9a4b
Parent:
5:aef1fc6c0df1
Child:
7:d636d073b942
PI calibr? et fonctionnel ! :D

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shovelcat 2:06f128641b62 1 /*
shovelcat 2:06f128641b62 2 author: Sebastian Pelchat
shovelcat 2:06f128641b62 3 date: october 2018
shovelcat 2:06f128641b62 4 */
shovelcat 2:06f128641b62 5 #include "speedlimiter.hpp"
shovelcat 2:06f128641b62 6
shovelcat 3:4da392d2bae8 7 #define debug SpeedLimiter::pc->printf
shovelcat 4:a8c9f6a13633 8
shovelcat 3:4da392d2bae8 9 Serial* SpeedLimiter::pc = new Serial(USBTX, USBRX);
shovelcat 3:4da392d2bae8 10
shovelcat 2:06f128641b62 11 SpeedLimiter::SpeedLimiter(const PinName& pedalInHi, const PinName& pedalInLo, const PinName& pedalOutHi, const PinName& pedalOutLo)
shovelcat 2:06f128641b62 12 : _pedalInHi(pedalInHi)
shovelcat 2:06f128641b62 13 , _pedalInLo(pedalInLo)
shovelcat 2:06f128641b62 14 , _pedalOutHi(pedalOutHi)
shovelcat 2:06f128641b62 15 , _pedalOutLo(pedalOutLo)
shovelcat 3:4da392d2bae8 16 , _referenceSpeed(DISABLE_ECO_ALGO_TRIGGER)
shovelcat 3:4da392d2bae8 17 , _measuredSpeed(0.0)
shovelcat 2:06f128641b62 18 {
shovelcat 3:4da392d2bae8 19 }
shovelcat 3:4da392d2bae8 20
shovelcat 3:4da392d2bae8 21 SpeedLimiter::~SpeedLimiter()
shovelcat 3:4da392d2bae8 22 {
shovelcat 3:4da392d2bae8 23 }
shovelcat 3:4da392d2bae8 24
shovelcat 3:4da392d2bae8 25 void SpeedLimiter::ipControllerTransferFunction()
shovelcat 3:4da392d2bae8 26 {
shovelcat 3:4da392d2bae8 27 // write voltages at beginning of function to prevent jitter
shovelcat 3:4da392d2bae8 28 // voltage will be delayed by 1 call which is okay.
shovelcat 4:a8c9f6a13633 29 // pc->printf("H\n\r");
shovelcat 4:a8c9f6a13633 30 const float voutHi = getOutputPedalVoltageHi();
shovelcat 4:a8c9f6a13633 31 const float voutLo = getOutputPedalVoltageLo();
shovelcat 4:a8c9f6a13633 32
shovelcat 4:a8c9f6a13633 33 // pc->printf("Hi: %f\t Lo: %f\n\r", voutHi, voutLo);
shovelcat 4:a8c9f6a13633 34
shovelcat 4:a8c9f6a13633 35 writeAdcPedalHi(voutHi);
shovelcat 4:a8c9f6a13633 36 writeAdcPedalLo(voutLo);
shovelcat 4:a8c9f6a13633 37
shovelcat 3:4da392d2bae8 38 // calculate voltage for next call
shovelcat 3:4da392d2bae8 39 const float referenceSpeed = getReferenceSpeed();
shovelcat 3:4da392d2bae8 40 float outputAdcVoltageHi = 0;
shovelcat 3:4da392d2bae8 41 float outputAdcVoltageLo = 0;
shovelcat 3:4da392d2bae8 42
shovelcat 3:4da392d2bae8 43 if(referenceSpeed == DISABLE_ECO_ALGO_TRIGGER) {
shovelcat 3:4da392d2bae8 44 outputAdcVoltageHi = ecoDisabledAlgorithm();
shovelcat 3:4da392d2bae8 45 } else {
shovelcat 3:4da392d2bae8 46 outputAdcVoltageHi = ecoEnabledAlgorithm();
shovelcat 3:4da392d2bae8 47 }
shovelcat 3:4da392d2bae8 48
shovelcat 4:a8c9f6a13633 49 // outputAdcVoltageHi = ADC_OUTPUT_MAX_VALUE / 2;
shovelcat 3:4da392d2bae8 50 outputAdcVoltageLo = outputAdcVoltageHi / 2;
shovelcat 3:4da392d2bae8 51
shovelcat 4:a8c9f6a13633 52 // pc->printf("tmpHi: %f\t tmpLo: %f\n\r", outputAdcVoltageHi, outputAdcVoltageLo);
shovelcat 4:a8c9f6a13633 53
shovelcat 3:4da392d2bae8 54 setOutputPedalVoltageHi(outputAdcVoltageHi);
shovelcat 3:4da392d2bae8 55 setOutputPedalVoltageLo(outputAdcVoltageLo);
shovelcat 3:4da392d2bae8 56 }
shovelcat 3:4da392d2bae8 57
shovelcat 3:4da392d2bae8 58 // Returns voltage read on analog input port chosen for pedal input 1
shovelcat 3:4da392d2bae8 59 float SpeedLimiter::readAdcPedalHi()
shovelcat 3:4da392d2bae8 60 {
shovelcat 3:4da392d2bae8 61 const float decPcValue = _pedalInHi.read();
shovelcat 3:4da392d2bae8 62 const float voltage = decPcValue * ADC_INPUT_MAX_VALUE;
shovelcat 3:4da392d2bae8 63 return voltage;
shovelcat 3:4da392d2bae8 64 }
shovelcat 3:4da392d2bae8 65
shovelcat 3:4da392d2bae8 66 // Returns voltage read on analog input port chosen for pedal input 2
shovelcat 3:4da392d2bae8 67 float SpeedLimiter::readAdcPedalLo()
shovelcat 3:4da392d2bae8 68 {
shovelcat 3:4da392d2bae8 69 const float decPcValue = _pedalInLo.read();
shovelcat 3:4da392d2bae8 70 const float voltage = decPcValue * ADC_INPUT_MAX_VALUE;
shovelcat 3:4da392d2bae8 71 return voltage;
shovelcat 3:4da392d2bae8 72 }
shovelcat 2:06f128641b62 73
shovelcat 3:4da392d2bae8 74 // Accepts a value in volts, converts to % and sets ADC for pedal output 1
shovelcat 3:4da392d2bae8 75 void SpeedLimiter::writeAdcPedalHi(const float voltage)
shovelcat 3:4da392d2bae8 76 {
shovelcat 3:4da392d2bae8 77 const float boundedValue = boundValue(voltage, PEDAL_HI_MIN_VALUE, PEDAL_HI_MAX_VALUE);
shovelcat 3:4da392d2bae8 78 const float decValue = voltageToDecimal(boundedValue, ADC_OUTPUT_MAX_VALUE);
shovelcat 3:4da392d2bae8 79 _pedalOutHi.write(decValue);
shovelcat 3:4da392d2bae8 80 }
shovelcat 3:4da392d2bae8 81
shovelcat 3:4da392d2bae8 82 // Accepts a value in volts, converts to % and sets ADC for pedal output 2
shovelcat 3:4da392d2bae8 83 void SpeedLimiter::writeAdcPedalLo(const float voltage)
shovelcat 3:4da392d2bae8 84 {
shovelcat 3:4da392d2bae8 85 const float boundedValue = boundValue(voltage, PEDAL_LO_MIN_VALUE, PEDAL_LO_MAX_VALUE);
shovelcat 3:4da392d2bae8 86 const float decValue = voltageToDecimal(boundedValue, ADC_OUTPUT_MAX_VALUE);
shovelcat 3:4da392d2bae8 87 _pedalOutLo.write(decValue);
shovelcat 3:4da392d2bae8 88 }
shovelcat 3:4da392d2bae8 89
shovelcat 3:4da392d2bae8 90 float SpeedLimiter::ecoDisabledAlgorithm()
shovelcat 3:4da392d2bae8 91 {
shovelcat 3:4da392d2bae8 92 const float value = readAdcPedalHi();
shovelcat 3:4da392d2bae8 93 return value;
shovelcat 3:4da392d2bae8 94 }
shovelcat 3:4da392d2bae8 95
shovelcat 6:1a4ed88c9a4b 96 //// Proportionnel 3s
shovelcat 6:1a4ed88c9a4b 97 //float SpeedLimiter::ecoEnabledAlgorithm()
shovelcat 6:1a4ed88c9a4b 98 //{
shovelcat 6:1a4ed88c9a4b 99 // static bool first_acquisition = true;
shovelcat 6:1a4ed88c9a4b 100 //
shovelcat 6:1a4ed88c9a4b 101 // // constantes calcules dans MATLAB
shovelcat 6:1a4ed88c9a4b 102 // double Kp = 1.6783e+03; //3.1848e+03
shovelcat 6:1a4ed88c9a4b 103 //
shovelcat 6:1a4ed88c9a4b 104 // // valeurs anterieures
shovelcat 6:1a4ed88c9a4b 105 // static double ie = 0.0;
shovelcat 6:1a4ed88c9a4b 106 //
shovelcat 6:1a4ed88c9a4b 107 // // calculs
shovelcat 6:1a4ed88c9a4b 108 // double Vm = getMeasuredSpeed();
shovelcat 6:1a4ed88c9a4b 109 // double Vd = getReferenceSpeed();
shovelcat 6:1a4ed88c9a4b 110 // double out = 0.0;
shovelcat 6:1a4ed88c9a4b 111 // if (readAdcPedalHi() > 1) {
shovelcat 6:1a4ed88c9a4b 112 // double e = Vd - Vm;
shovelcat 6:1a4ed88c9a4b 113 // const double dt = TRANSFER_FUNCTION_PERIOD;
shovelcat 6:1a4ed88c9a4b 114 // ie = ie + e*dt;
shovelcat 6:1a4ed88c9a4b 115 //
shovelcat 6:1a4ed88c9a4b 116 // out = Kp*e;
shovelcat 6:1a4ed88c9a4b 117 //
shovelcat 6:1a4ed88c9a4b 118 // // out est maintenant en 'force'
shovelcat 6:1a4ed88c9a4b 119 // out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
shovelcat 6:1a4ed88c9a4b 120 //
shovelcat 6:1a4ed88c9a4b 121 // if(first_acquisition) {
shovelcat 6:1a4ed88c9a4b 122 // pc->printf("Acquisition start:\n\r");
shovelcat 6:1a4ed88c9a4b 123 // first_acquisition = false;
shovelcat 6:1a4ed88c9a4b 124 // }
shovelcat 6:1a4ed88c9a4b 125 // } else {
shovelcat 6:1a4ed88c9a4b 126 // ie = 0.00000000001;
shovelcat 6:1a4ed88c9a4b 127 // first_acquisition = true;
shovelcat 6:1a4ed88c9a4b 128 // }
shovelcat 6:1a4ed88c9a4b 129 //
shovelcat 6:1a4ed88c9a4b 130 // pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
shovelcat 6:1a4ed88c9a4b 131 //
shovelcat 6:1a4ed88c9a4b 132 // return (float)out;
shovelcat 6:1a4ed88c9a4b 133 //}
shovelcat 6:1a4ed88c9a4b 134
shovelcat 6:1a4ed88c9a4b 135 // PI 3s (meilleur a date)
shovelcat 3:4da392d2bae8 136 float SpeedLimiter::ecoEnabledAlgorithm()
shovelcat 3:4da392d2bae8 137 {
shovelcat 5:aef1fc6c0df1 138 static bool first_acquisition = true;
shovelcat 6:1a4ed88c9a4b 139
shovelcat 5:aef1fc6c0df1 140 // constantes calcules dans MATLAB
shovelcat 6:1a4ed88c9a4b 141 double var = readAdcTest();
shovelcat 6:1a4ed88c9a4b 142 double Kp = 18000 ; //3.1848e+03
shovelcat 6:1a4ed88c9a4b 143 double Ki = 4300.0; // 2.5181e+03
shovelcat 3:4da392d2bae8 144
shovelcat 5:aef1fc6c0df1 145 // valeurs anterieures
shovelcat 6:1a4ed88c9a4b 146 static double ie = 0.0;
shovelcat 3:4da392d2bae8 147
shovelcat 3:4da392d2bae8 148 // calculs
shovelcat 5:aef1fc6c0df1 149 double Vm = getMeasuredSpeed();
shovelcat 6:1a4ed88c9a4b 150 double Vd = 40 * var;
shovelcat 5:aef1fc6c0df1 151 double out = 0.0;
shovelcat 4:a8c9f6a13633 152 if (readAdcPedalHi() > 1) {
shovelcat 6:1a4ed88c9a4b 153 double e = Vd - Vm;
shovelcat 6:1a4ed88c9a4b 154 const double dt = TRANSFER_FUNCTION_PERIOD;
shovelcat 6:1a4ed88c9a4b 155 ie = ie + e*dt;
shovelcat 6:1a4ed88c9a4b 156
shovelcat 6:1a4ed88c9a4b 157 out = Kp*e + Ki*ie;
shovelcat 5:aef1fc6c0df1 158
shovelcat 5:aef1fc6c0df1 159 // out est maintenant en 'force'
shovelcat 5:aef1fc6c0df1 160 out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
shovelcat 3:4da392d2bae8 161
shovelcat 5:aef1fc6c0df1 162 if(first_acquisition) {
shovelcat 5:aef1fc6c0df1 163 pc->printf("Acquisition start:\n\r");
shovelcat 5:aef1fc6c0df1 164 first_acquisition = false;
shovelcat 5:aef1fc6c0df1 165 }
shovelcat 5:aef1fc6c0df1 166 } else {
shovelcat 6:1a4ed88c9a4b 167 ie = 0.00000000001;
shovelcat 5:aef1fc6c0df1 168 first_acquisition = true;
shovelcat 4:a8c9f6a13633 169 }
shovelcat 3:4da392d2bae8 170
shovelcat 6:1a4ed88c9a4b 171 pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t Kp: %.2f\tVar: %.2f\n\r", Vm, Vd, out, Ki, var);
shovelcat 5:aef1fc6c0df1 172
shovelcat 5:aef1fc6c0df1 173 return (float)out;
shovelcat 5:aef1fc6c0df1 174 }
shovelcat 5:aef1fc6c0df1 175
shovelcat 6:1a4ed88c9a4b 176 //// essai routier: lim = 10 kmh, overshoot de 50%, un seul ronflement
shovelcat 6:1a4ed88c9a4b 177 //// max 15 kmh, min 7 kmh.
shovelcat 6:1a4ed88c9a4b 178 //// temps de stabilisation 3s
shovelcat 5:aef1fc6c0df1 179 //float SpeedLimiter::ecoEnabledAlgorithm()
shovelcat 5:aef1fc6c0df1 180 //{
shovelcat 6:1a4ed88c9a4b 181 // static bool first_acquisition = true;
shovelcat 5:aef1fc6c0df1 182 //
shovelcat 6:1a4ed88c9a4b 183 // // constantes calcules dans MATLAB
shovelcat 6:1a4ed88c9a4b 184 // const double b0 = 3312.0;
shovelcat 6:1a4ed88c9a4b 185 // const double b1 = -3061.0;
shovelcat 5:aef1fc6c0df1 186 // const double a1 = -1.0;
shovelcat 5:aef1fc6c0df1 187 //
shovelcat 6:1a4ed88c9a4b 188 // // valeurs anterieures
shovelcat 6:1a4ed88c9a4b 189 // static double lastE = 0.0;
shovelcat 6:1a4ed88c9a4b 190 // static double lastOut = 0.0;
shovelcat 6:1a4ed88c9a4b 191 //
shovelcat 6:1a4ed88c9a4b 192 // // calculs
shovelcat 6:1a4ed88c9a4b 193 // double Vm = getMeasuredSpeed();
shovelcat 6:1a4ed88c9a4b 194 // double Vd = getReferenceSpeed();
shovelcat 6:1a4ed88c9a4b 195 // double out = 0.0;
shovelcat 6:1a4ed88c9a4b 196 // if (readAdcPedalHi() > 1) {
shovelcat 6:1a4ed88c9a4b 197 // const double e = Vd - Vm;
shovelcat 6:1a4ed88c9a4b 198 //
shovelcat 6:1a4ed88c9a4b 199 // // y(n) = b0 * x(n) + b1 * x(n-1) - a1 * y(n-1)
shovelcat 6:1a4ed88c9a4b 200 // out = (b0 * e) + (b1 * lastE) - (a1 * lastOut);
shovelcat 6:1a4ed88c9a4b 201 //
shovelcat 6:1a4ed88c9a4b 202 // lastE = e;
shovelcat 6:1a4ed88c9a4b 203 // lastOut = out;
shovelcat 6:1a4ed88c9a4b 204 //
shovelcat 6:1a4ed88c9a4b 205 // // out est maintenant en 'force'
shovelcat 6:1a4ed88c9a4b 206 // out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
shovelcat 5:aef1fc6c0df1 207 //
shovelcat 6:1a4ed88c9a4b 208 // if(first_acquisition) {
shovelcat 6:1a4ed88c9a4b 209 // pc->printf("Acquisition start:\n\r");
shovelcat 6:1a4ed88c9a4b 210 // first_acquisition = false;
shovelcat 6:1a4ed88c9a4b 211 // }
shovelcat 6:1a4ed88c9a4b 212 // } else {
shovelcat 6:1a4ed88c9a4b 213 // lastE = 0.00000000001;
shovelcat 6:1a4ed88c9a4b 214 // lastOut = 0.00000000001;
shovelcat 6:1a4ed88c9a4b 215 // first_acquisition = true;
shovelcat 6:1a4ed88c9a4b 216 // }
shovelcat 6:1a4ed88c9a4b 217 //
shovelcat 6:1a4ed88c9a4b 218 // pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
shovelcat 6:1a4ed88c9a4b 219 //
shovelcat 6:1a4ed88c9a4b 220 // return (float)out;
shovelcat 6:1a4ed88c9a4b 221 //}
shovelcat 6:1a4ed88c9a4b 222
shovelcat 6:1a4ed88c9a4b 223 //// temps de stabilisation 1s
shovelcat 6:1a4ed88c9a4b 224 //// oscillation, grand overshoot (20 kmh), consigne 10 kmh
shovelcat 6:1a4ed88c9a4b 225 //float SpeedLimiter::ecoEnabledAlgorithm()
shovelcat 6:1a4ed88c9a4b 226 //{
shovelcat 6:1a4ed88c9a4b 227 // static bool first_acquisition = true;
shovelcat 6:1a4ed88c9a4b 228 //
shovelcat 6:1a4ed88c9a4b 229 // // constantes calcules dans MATLAB
shovelcat 6:1a4ed88c9a4b 230 // const double b0 = 10790.0;
shovelcat 6:1a4ed88c9a4b 231 // const double b1 = -8514.0;
shovelcat 6:1a4ed88c9a4b 232 // const double a1 = -1.0;
shovelcat 6:1a4ed88c9a4b 233 //
shovelcat 6:1a4ed88c9a4b 234 // // valeurs anterieures
shovelcat 6:1a4ed88c9a4b 235 // static double lastE = 0.0;
shovelcat 6:1a4ed88c9a4b 236 // static double lastOut = 0.0;
shovelcat 5:aef1fc6c0df1 237 //
shovelcat 5:aef1fc6c0df1 238 // // calculs
shovelcat 6:1a4ed88c9a4b 239 // double Vm = getMeasuredSpeed();
shovelcat 6:1a4ed88c9a4b 240 // double Vd = getReferenceSpeed();
shovelcat 6:1a4ed88c9a4b 241 // double out = 0.0;
shovelcat 5:aef1fc6c0df1 242 // if (readAdcPedalHi() > 1) {
shovelcat 6:1a4ed88c9a4b 243 // const double e = Vd - Vm;
shovelcat 6:1a4ed88c9a4b 244 //
shovelcat 6:1a4ed88c9a4b 245 // // y(n) = b0 * x(n) + b1 * x(n-1) - a1 * y(n-1)
shovelcat 6:1a4ed88c9a4b 246 // out = (b0 * e) + (b1 * lastE) - (a1 * lastOut);
shovelcat 6:1a4ed88c9a4b 247 //
shovelcat 6:1a4ed88c9a4b 248 // lastE = e;
shovelcat 6:1a4ed88c9a4b 249 // lastOut = out;
shovelcat 5:aef1fc6c0df1 250 //
shovelcat 6:1a4ed88c9a4b 251 // // out est maintenant en 'force'
shovelcat 6:1a4ed88c9a4b 252 // out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
shovelcat 5:aef1fc6c0df1 253 //
shovelcat 6:1a4ed88c9a4b 254 // if(first_acquisition) {
shovelcat 6:1a4ed88c9a4b 255 // pc->printf("Acquisition start:\n\r");
shovelcat 6:1a4ed88c9a4b 256 // first_acquisition = false;
shovelcat 6:1a4ed88c9a4b 257 // }
shovelcat 6:1a4ed88c9a4b 258 // } else {
shovelcat 6:1a4ed88c9a4b 259 // lastE = 0.00000000001;
shovelcat 6:1a4ed88c9a4b 260 // lastOut = 0.00000000001;
shovelcat 6:1a4ed88c9a4b 261 // first_acquisition = true;
shovelcat 5:aef1fc6c0df1 262 // }
shovelcat 5:aef1fc6c0df1 263 //
shovelcat 6:1a4ed88c9a4b 264 // pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
shovelcat 6:1a4ed88c9a4b 265 //
shovelcat 6:1a4ed88c9a4b 266 // return (float)out;
shovelcat 6:1a4ed88c9a4b 267 //}
shovelcat 6:1a4ed88c9a4b 268
shovelcat 6:1a4ed88c9a4b 269 //// implementation du PID 2s de stab
shovelcat 6:1a4ed88c9a4b 270 //// overshoot significatif, ronflement constant
shovelcat 6:1a4ed88c9a4b 271 //float SpeedLimiter::ecoEnabledAlgorithm()
shovelcat 6:1a4ed88c9a4b 272 //{
shovelcat 6:1a4ed88c9a4b 273 // static bool first_acquisition = true;
shovelcat 6:1a4ed88c9a4b 274 //
shovelcat 6:1a4ed88c9a4b 275 // // constantes calcules dans MATLAB
shovelcat 6:1a4ed88c9a4b 276 // const double b0 = 4.246e04;
shovelcat 6:1a4ed88c9a4b 277 // const double b1 = -7.958e04;
shovelcat 6:1a4ed88c9a4b 278 // const double b2 = 3.768e04;
shovelcat 6:1a4ed88c9a4b 279 // const double a1 = -1.0;
shovelcat 6:1a4ed88c9a4b 280 //
shovelcat 6:1a4ed88c9a4b 281 // // valeurs anterieures
shovelcat 6:1a4ed88c9a4b 282 // static double lastE = 0.0;
shovelcat 6:1a4ed88c9a4b 283 // static double lastlastE = 0.0;
shovelcat 6:1a4ed88c9a4b 284 // static double lastOut = 0.0;
shovelcat 6:1a4ed88c9a4b 285 // static double lastlastOut = 0.0;
shovelcat 6:1a4ed88c9a4b 286 //
shovelcat 6:1a4ed88c9a4b 287 // // calculs
shovelcat 6:1a4ed88c9a4b 288 // double Vm = getMeasuredSpeed();
shovelcat 6:1a4ed88c9a4b 289 // double Vd = getReferenceSpeed();
shovelcat 6:1a4ed88c9a4b 290 // double out = 0.0;
shovelcat 6:1a4ed88c9a4b 291 // if (readAdcPedalHi() > 1) {
shovelcat 6:1a4ed88c9a4b 292 // const double e = Vd - Vm;
shovelcat 6:1a4ed88c9a4b 293 //
shovelcat 6:1a4ed88c9a4b 294 // // y(n) = b0 * x(n) + b1 * x(n-1) - a1 * y(n-1)
shovelcat 6:1a4ed88c9a4b 295 // out = (b0 * e) + (b1 * lastE) + (b2 * lastlastE) - (a1 * lastOut);
shovelcat 6:1a4ed88c9a4b 296 //
shovelcat 6:1a4ed88c9a4b 297 // lastlastE = lastE;
shovelcat 6:1a4ed88c9a4b 298 // lastE = e;
shovelcat 6:1a4ed88c9a4b 299 // lastlastOut = lastOut;
shovelcat 6:1a4ed88c9a4b 300 // lastOut = out;
shovelcat 6:1a4ed88c9a4b 301 //
shovelcat 6:1a4ed88c9a4b 302 // // out est maintenant en 'force'
shovelcat 6:1a4ed88c9a4b 303 // out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
shovelcat 6:1a4ed88c9a4b 304 //
shovelcat 6:1a4ed88c9a4b 305 // if(first_acquisition) {
shovelcat 6:1a4ed88c9a4b 306 // pc->printf("Acquisition start:\n\r");
shovelcat 6:1a4ed88c9a4b 307 // first_acquisition = false;
shovelcat 6:1a4ed88c9a4b 308 // }
shovelcat 6:1a4ed88c9a4b 309 // } else {
shovelcat 6:1a4ed88c9a4b 310 // lastE = 0.00000000001;
shovelcat 6:1a4ed88c9a4b 311 // lastOut = 0.00000000001;
shovelcat 6:1a4ed88c9a4b 312 // first_acquisition = true;
shovelcat 6:1a4ed88c9a4b 313 // }
shovelcat 5:aef1fc6c0df1 314 //
shovelcat 6:1a4ed88c9a4b 315 // pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
shovelcat 6:1a4ed88c9a4b 316 //
shovelcat 6:1a4ed88c9a4b 317 // return (float)out;
shovelcat 5:aef1fc6c0df1 318 //}
shovelcat 3:4da392d2bae8 319
shovelcat 6:1a4ed88c9a4b 320 //// overshoot ~17, oscillation / ronflement
shovelcat 6:1a4ed88c9a4b 321 //float SpeedLimiter::ecoEnabledAlgorithm()
shovelcat 6:1a4ed88c9a4b 322 //{
shovelcat 6:1a4ed88c9a4b 323 // static bool first_acquisition = true;
shovelcat 6:1a4ed88c9a4b 324 //
shovelcat 6:1a4ed88c9a4b 325 // // constantes calcules dans MATLAB
shovelcat 6:1a4ed88c9a4b 326 // double Kp = 4.7772e+03;
shovelcat 6:1a4ed88c9a4b 327 // double Ki = 5.6658e+03;
shovelcat 6:1a4ed88c9a4b 328 // double Kd = 4000.0;
shovelcat 6:1a4ed88c9a4b 329 //
shovelcat 6:1a4ed88c9a4b 330 // // valeurs anterieures
shovelcat 6:1a4ed88c9a4b 331 // static double ie = 0.0;
shovelcat 6:1a4ed88c9a4b 332 // static double lastE = 0.0;
shovelcat 6:1a4ed88c9a4b 333 //
shovelcat 6:1a4ed88c9a4b 334 // // calculs
shovelcat 6:1a4ed88c9a4b 335 // double Vm = getMeasuredSpeed();
shovelcat 6:1a4ed88c9a4b 336 // double Vd = getReferenceSpeed();
shovelcat 6:1a4ed88c9a4b 337 // double out = 0.0;
shovelcat 6:1a4ed88c9a4b 338 // if (readAdcPedalHi() > 1) {
shovelcat 6:1a4ed88c9a4b 339 // double e = Vd - Vm;
shovelcat 6:1a4ed88c9a4b 340 // const double dt = TRANSFER_FUNCTION_PERIOD;
shovelcat 6:1a4ed88c9a4b 341 // ie = ie + e*dt;
shovelcat 6:1a4ed88c9a4b 342 // double de = (e - lastE)/dt;
shovelcat 6:1a4ed88c9a4b 343 //
shovelcat 6:1a4ed88c9a4b 344 // out = Kp*e + Ki*ie + Kd*de;
shovelcat 6:1a4ed88c9a4b 345 //
shovelcat 6:1a4ed88c9a4b 346 // lastE = e;
shovelcat 6:1a4ed88c9a4b 347 //
shovelcat 6:1a4ed88c9a4b 348 // // out est maintenant en 'force'
shovelcat 6:1a4ed88c9a4b 349 // out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
shovelcat 6:1a4ed88c9a4b 350 //
shovelcat 6:1a4ed88c9a4b 351 // if(first_acquisition) {
shovelcat 6:1a4ed88c9a4b 352 // pc->printf("Acquisition start:\n\r");
shovelcat 6:1a4ed88c9a4b 353 // first_acquisition = false;
shovelcat 6:1a4ed88c9a4b 354 // }
shovelcat 6:1a4ed88c9a4b 355 // } else {
shovelcat 6:1a4ed88c9a4b 356 // lastE = 0.00000000001;
shovelcat 6:1a4ed88c9a4b 357 // ie = 0.00000000001;
shovelcat 6:1a4ed88c9a4b 358 // first_acquisition = true;
shovelcat 6:1a4ed88c9a4b 359 // }
shovelcat 6:1a4ed88c9a4b 360 //
shovelcat 6:1a4ed88c9a4b 361 // pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
shovelcat 6:1a4ed88c9a4b 362 //
shovelcat 6:1a4ed88c9a4b 363 // return (float)out;
shovelcat 6:1a4ed88c9a4b 364 //}
shovelcat 6:1a4ed88c9a4b 365
shovelcat 6:1a4ed88c9a4b 366
shovelcat 3:4da392d2bae8 367 // Returns 'value' bounded between 'lowerBound' and 'upperBound'
shovelcat 3:4da392d2bae8 368 float SpeedLimiter::boundValue(float value, const float lowerBound, const float upperBound)
shovelcat 3:4da392d2bae8 369 {
shovelcat 3:4da392d2bae8 370 if(value < lowerBound) {
shovelcat 3:4da392d2bae8 371 value = lowerBound;
shovelcat 3:4da392d2bae8 372 } else if(value > upperBound) {
shovelcat 3:4da392d2bae8 373 value = upperBound;
shovelcat 3:4da392d2bae8 374 }
shovelcat 3:4da392d2bae8 375 return value;
shovelcat 3:4da392d2bae8 376 }
shovelcat 3:4da392d2bae8 377
shovelcat 3:4da392d2bae8 378 // Returns "value/reference" as a percentage in decimal form (0.5 for 50%)
shovelcat 3:4da392d2bae8 379 float SpeedLimiter::voltageToDecimal(const float voltage, const float reference)
shovelcat 3:4da392d2bae8 380 {
shovelcat 3:4da392d2bae8 381 return voltage/reference;
shovelcat 3:4da392d2bae8 382 }