Hesu-eco / Mbed 2 deprecated limiteur_vitesse

premiere ebauche

Dependencies:   mbed PinDetect

speedlimiter.cpp@5:aef1fc6c0df1, 2018-10-25 (annotated)

Committer:
shovelcat
Date:
Thu Oct 25 17:43:21 2018 +0000
Revision:
5:aef1fc6c0df1
Parent:
4:a8c9f6a13633
Child:
6:1a4ed88c9a4b
fonctionnel avec ~50% d'overshoot

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 3:4da392d2bae8 96 float SpeedLimiter::ecoEnabledAlgorithm()
shovelcat 3:4da392d2bae8 97 {
shovelcat 5:aef1fc6c0df1 98 static bool first_acquisition = true;
shovelcat 3:4da392d2bae8 99
shovelcat 5:aef1fc6c0df1 100 // constantes calcules dans MATLAB
shovelcat 5:aef1fc6c0df1 101 const double b0 = 3312.0;
shovelcat 5:aef1fc6c0df1 102 const double b1 = -3061.0;
shovelcat 4:a8c9f6a13633 103 const double a1 = -1.0;
shovelcat 3:4da392d2bae8 104
shovelcat 5:aef1fc6c0df1 105 // valeurs anterieures
shovelcat 5:aef1fc6c0df1 106 static double lastE = 0.0;
shovelcat 5:aef1fc6c0df1 107 static double lastOut = 0.0;
shovelcat 3:4da392d2bae8 108
shovelcat 3:4da392d2bae8 109 // calculs
shovelcat 5:aef1fc6c0df1 110 double Vm = getMeasuredSpeed();
shovelcat 5:aef1fc6c0df1 111 double Vd = getReferenceSpeed();
shovelcat 5:aef1fc6c0df1 112 double out = 0.0;
shovelcat 4:a8c9f6a13633 113 if (readAdcPedalHi() > 1) {
shovelcat 4:a8c9f6a13633 114
shovelcat 5:aef1fc6c0df1 115 const double e = Vd - Vm;
shovelcat 5:aef1fc6c0df1 116
shovelcat 5:aef1fc6c0df1 117 // y(n) = b0 * x(n) + b1 * x(n-1) - a1 * y(n-1)
shovelcat 5:aef1fc6c0df1 118 out = (b0 * e) + (b1 * lastE) - (a1 * lastOut);
shovelcat 5:aef1fc6c0df1 119
shovelcat 5:aef1fc6c0df1 120 lastE = e;
shovelcat 5:aef1fc6c0df1 121 lastOut = out;
shovelcat 5:aef1fc6c0df1 122
shovelcat 5:aef1fc6c0df1 123 // out est maintenant en 'force'
shovelcat 5:aef1fc6c0df1 124 out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
shovelcat 3:4da392d2bae8 125
shovelcat 5:aef1fc6c0df1 126 if(first_acquisition) {
shovelcat 5:aef1fc6c0df1 127 pc->printf("Acquisition start:\n\r");
shovelcat 5:aef1fc6c0df1 128 first_acquisition = false;
shovelcat 5:aef1fc6c0df1 129 }
shovelcat 5:aef1fc6c0df1 130 // pc->printf("%f;", Vm);
shovelcat 5:aef1fc6c0df1 131 // wait(5);
shovelcat 5:aef1fc6c0df1 132
shovelcat 5:aef1fc6c0df1 133 } else {
shovelcat 5:aef1fc6c0df1 134 lastE = 0.00000000001;
shovelcat 5:aef1fc6c0df1 135 lastOut = 0.00000000001;
shovelcat 5:aef1fc6c0df1 136 first_acquisition = true;
shovelcat 4:a8c9f6a13633 137 }
shovelcat 3:4da392d2bae8 138
shovelcat 5:aef1fc6c0df1 139 pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
shovelcat 5:aef1fc6c0df1 140
shovelcat 5:aef1fc6c0df1 141 return (float)out;
shovelcat 5:aef1fc6c0df1 142 }
shovelcat 5:aef1fc6c0df1 143
shovelcat 5:aef1fc6c0df1 144
shovelcat 4:a8c9f6a13633 145
shovelcat 5:aef1fc6c0df1 146 //float SpeedLimiter::ecoEnabledAlgorithm()
shovelcat 5:aef1fc6c0df1 147 //{
shovelcat 5:aef1fc6c0df1 148 // // valeurs anterieures
shovelcat 5:aef1fc6c0df1 149 // static double x1 = 0.0;
shovelcat 5:aef1fc6c0df1 150 // static double y1 = 0.0;
shovelcat 5:aef1fc6c0df1 151 //
shovelcat 5:aef1fc6c0df1 152 // // constantes calcules avec matlab
shovelcat 5:aef1fc6c0df1 153 // const double b0 = 10793.0;
shovelcat 5:aef1fc6c0df1 154 // const double b1 = -8513.5;
shovelcat 5:aef1fc6c0df1 155 // const double a1 = -1.0;
shovelcat 5:aef1fc6c0df1 156 //
shovelcat 5:aef1fc6c0df1 157 // const double TRANS_k_gear = 20.5; //Gearbox ratio
shovelcat 5:aef1fc6c0df1 158 // const double TRANS_eff = 0.91; // Efficiency of the transmission
shovelcat 5:aef1fc6c0df1 159 // const double TRANS_R_wheel = 0.3175; // Radius of the wheel [m]
shovelcat 5:aef1fc6c0df1 160 // const double TRANS_k = TRANS_k_gear/TRANS_R_wheel; // Global ratio of the transmission [m]
shovelcat 5:aef1fc6c0df1 161 //
shovelcat 5:aef1fc6c0df1 162 // const double G_forceToTorque = (1/TRANS_k) * TRANS_eff;
shovelcat 5:aef1fc6c0df1 163 //
shovelcat 5:aef1fc6c0df1 164 // // calculs
shovelcat 5:aef1fc6c0df1 165 // const float Vm = getMeasuredSpeed();
shovelcat 5:aef1fc6c0df1 166 // const float Vd = getReferenceSpeed();
shovelcat 5:aef1fc6c0df1 167 // double y0 = 0.0;
shovelcat 5:aef1fc6c0df1 168 // if (readAdcPedalHi() > 1) {
shovelcat 5:aef1fc6c0df1 169 // const double x0 = Vd - Vm;
shovelcat 5:aef1fc6c0df1 170 // y0 = ( (-a1*y1) + (b0*x0) + (b1*x1) ) * G_forceToTorque;
shovelcat 5:aef1fc6c0df1 171 //
shovelcat 5:aef1fc6c0df1 172 // const double deltaT = 20.0;
shovelcat 5:aef1fc6c0df1 173 // const double deltaE = PEDAL_HI_MAX_VALUE - PEDAL_HI_MIN_VALUE;
shovelcat 5:aef1fc6c0df1 174 // const double ratioTtoE = deltaE / deltaT / 159.63 * deltaE;
shovelcat 5:aef1fc6c0df1 175 // y0 = y0 * ratioTtoE + PEDAL_HI_MIN_VALUE;
shovelcat 5:aef1fc6c0df1 176 //
shovelcat 5:aef1fc6c0df1 177 // // update des valeurs anterieurs
shovelcat 5:aef1fc6c0df1 178 // x1 = x0;
shovelcat 5:aef1fc6c0df1 179 // y1 = y0;
shovelcat 5:aef1fc6c0df1 180 // }
shovelcat 5:aef1fc6c0df1 181 //
shovelcat 5:aef1fc6c0df1 182 // pc->printf("Vm: %f\t Vd: %f\t Eh: %f\t El\n\r", Vm, Vd, y0, y0/2);
shovelcat 5:aef1fc6c0df1 183 //
shovelcat 5:aef1fc6c0df1 184 // return (float)y0;
shovelcat 5:aef1fc6c0df1 185 //}
shovelcat 3:4da392d2bae8 186
shovelcat 3:4da392d2bae8 187 // Returns 'value' bounded between 'lowerBound' and 'upperBound'
shovelcat 3:4da392d2bae8 188 float SpeedLimiter::boundValue(float value, const float lowerBound, const float upperBound)
shovelcat 3:4da392d2bae8 189 {
shovelcat 3:4da392d2bae8 190 if(value < lowerBound) {
shovelcat 3:4da392d2bae8 191 value = lowerBound;
shovelcat 3:4da392d2bae8 192 } else if(value > upperBound) {
shovelcat 3:4da392d2bae8 193 value = upperBound;
shovelcat 3:4da392d2bae8 194 }
shovelcat 3:4da392d2bae8 195 return value;
shovelcat 3:4da392d2bae8 196 }
shovelcat 3:4da392d2bae8 197
shovelcat 3:4da392d2bae8 198 // Returns "value/reference" as a percentage in decimal form (0.5 for 50%)
shovelcat 3:4da392d2bae8 199 float SpeedLimiter::voltageToDecimal(const float voltage, const float reference)
shovelcat 3:4da392d2bae8 200 {
shovelcat 3:4da392d2bae8 201 return voltage/reference;
shovelcat 3:4da392d2bae8 202 }