Hesu-eco
/
limiteur_vitesse
premiere ebauche
speedlimiter.cpp@15:e9560502e463, 2018-12-17 (annotated)
- Committer:
- LipSer
- Date:
- Mon Dec 17 20:15:19 2018 +0000
- Revision:
- 15:e9560502e463
- Parent:
- 14:70fc660d4386
fin
Who changed what in which revision?
User | Revision | Line number | New 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 | 13:47806f4dbfcd | 25 | void SpeedLimiter::controllerCallbackFunction() |
shovelcat | 13:47806f4dbfcd | 26 | { |
shovelcat | 10:f0b382368614 | 27 | float nextReferenceSpeed = getReferenceSpeed(true); |
shovelcat | 10:f0b382368614 | 28 | setReferenceSpeed(nextReferenceSpeed); |
shovelcat | 13:47806f4dbfcd | 29 | |
shovelcat | 10:f0b382368614 | 30 | |
shovelcat | 3:4da392d2bae8 | 31 | // write voltages at beginning of function to prevent jitter |
shovelcat | 3:4da392d2bae8 | 32 | // voltage will be delayed by 1 call which is okay. |
shovelcat | 4:a8c9f6a13633 | 33 | const float voutHi = getOutputPedalVoltageHi(); |
shovelcat | 4:a8c9f6a13633 | 34 | const float voutLo = getOutputPedalVoltageLo(); |
shovelcat | 4:a8c9f6a13633 | 35 | |
shovelcat | 4:a8c9f6a13633 | 36 | writeAdcPedalHi(voutHi); |
shovelcat | 4:a8c9f6a13633 | 37 | writeAdcPedalLo(voutLo); |
shovelcat | 4:a8c9f6a13633 | 38 | |
shovelcat | 3:4da392d2bae8 | 39 | // calculate voltage for next call |
shovelcat | 3:4da392d2bae8 | 40 | const float referenceSpeed = getReferenceSpeed(); |
shovelcat | 3:4da392d2bae8 | 41 | float outputAdcVoltageHi = 0; |
shovelcat | 3:4da392d2bae8 | 42 | float outputAdcVoltageLo = 0; |
shovelcat | 3:4da392d2bae8 | 43 | |
shovelcat | 3:4da392d2bae8 | 44 | if(referenceSpeed == DISABLE_ECO_ALGO_TRIGGER) { |
shovelcat | 3:4da392d2bae8 | 45 | outputAdcVoltageHi = ecoDisabledAlgorithm(); |
shovelcat | 13:47806f4dbfcd | 46 | } |
shovelcat | 13:47806f4dbfcd | 47 | else { |
shovelcat | 3:4da392d2bae8 | 48 | outputAdcVoltageHi = ecoEnabledAlgorithm(); |
shovelcat | 3:4da392d2bae8 | 49 | } |
shovelcat | 3:4da392d2bae8 | 50 | |
shovelcat | 3:4da392d2bae8 | 51 | outputAdcVoltageLo = outputAdcVoltageHi / 2; |
shovelcat | 3:4da392d2bae8 | 52 | |
shovelcat | 13:47806f4dbfcd | 53 | // pc->printf("Reference speed: %f\n\r", nextReferenceSpeed); |
shovelcat | 4:a8c9f6a13633 | 54 | // pc->printf("tmpHi: %f\t tmpLo: %f\n\r", outputAdcVoltageHi, outputAdcVoltageLo); |
shovelcat | 13:47806f4dbfcd | 55 | // pc->printf("Hi: %f\t Lo: %f\n\r", voutHi, voutLo); |
shovelcat | 4:a8c9f6a13633 | 56 | |
shovelcat | 3:4da392d2bae8 | 57 | setOutputPedalVoltageHi(outputAdcVoltageHi); |
shovelcat | 3:4da392d2bae8 | 58 | setOutputPedalVoltageLo(outputAdcVoltageLo); |
shovelcat | 3:4da392d2bae8 | 59 | } |
shovelcat | 3:4da392d2bae8 | 60 | |
shovelcat | 13:47806f4dbfcd | 61 | float SpeedLimiter::ecoDisabledAlgorithm() |
shovelcat | 13:47806f4dbfcd | 62 | { |
shovelcat | 13:47806f4dbfcd | 63 | const float value = readAdcPedalHi(); |
shovelcat | 13:47806f4dbfcd | 64 | return value; |
shovelcat | 13:47806f4dbfcd | 65 | } |
shovelcat | 13:47806f4dbfcd | 66 | |
shovelcat | 13:47806f4dbfcd | 67 | float SpeedLimiter::ecoEnabledAlgorithm() |
shovelcat | 13:47806f4dbfcd | 68 | { |
shovelcat | 13:47806f4dbfcd | 69 | static bool first_acquisition = true; |
shovelcat | 13:47806f4dbfcd | 70 | static time_t referenceTime = time(NULL); // seconds since January 1, 1970, a.k.a. since reset (time not set) |
shovelcat | 13:47806f4dbfcd | 71 | |
shovelcat | 13:47806f4dbfcd | 72 | // calculs |
shovelcat | 13:47806f4dbfcd | 73 | const double Vm = getMeasuredSpeed(); |
shovelcat | 13:47806f4dbfcd | 74 | const double Vd = getReferenceSpeed(); |
shovelcat | 13:47806f4dbfcd | 75 | |
shovelcat | 13:47806f4dbfcd | 76 | double out = 0.0; |
shovelcat | 13:47806f4dbfcd | 77 | if (readAdcPedalHi() > 1) { |
shovelcat | 13:47806f4dbfcd | 78 | |
LipSer | 15:e9560502e463 | 79 | // out = piControlFunction(Vd, Vm); |
LipSer | 15:e9560502e463 | 80 | out = ipControlFunction(Vd, Vm); |
shovelcat | 13:47806f4dbfcd | 81 | |
shovelcat | 13:47806f4dbfcd | 82 | // force to voltage conversion |
shovelcat | 13:47806f4dbfcd | 83 | out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE; |
shovelcat | 13:47806f4dbfcd | 84 | |
shovelcat | 13:47806f4dbfcd | 85 | if(first_acquisition) { |
shovelcat | 13:47806f4dbfcd | 86 | pc->printf("Acquisition start:\n\r"); |
shovelcat | 13:47806f4dbfcd | 87 | referenceTime = time(NULL); |
shovelcat | 13:47806f4dbfcd | 88 | first_acquisition = false; |
shovelcat | 13:47806f4dbfcd | 89 | } |
shovelcat | 13:47806f4dbfcd | 90 | } else { |
shovelcat | 13:47806f4dbfcd | 91 | const bool reset = true; |
LipSer | 15:e9560502e463 | 92 | // piControlFunction(0,reset); |
LipSer | 15:e9560502e463 | 93 | ipControlFunction(0,reset); |
shovelcat | 13:47806f4dbfcd | 94 | |
shovelcat | 13:47806f4dbfcd | 95 | first_acquisition = true; |
shovelcat | 13:47806f4dbfcd | 96 | } |
shovelcat | 13:47806f4dbfcd | 97 | |
shovelcat | 13:47806f4dbfcd | 98 | time_t elapsedTime = time(NULL) - referenceTime; |
shovelcat | 13:47806f4dbfcd | 99 | pc->printf("%d: Vd: %.2f\tVm: %.2f\t\n\r", elapsedTime, Vd, Vm); |
shovelcat | 13:47806f4dbfcd | 100 | |
shovelcat | 13:47806f4dbfcd | 101 | // char timestamp[32]; |
shovelcat | 13:47806f4dbfcd | 102 | // strftime(timestamp, 32, "%I:%M %p\n", localtime(&seconds)); |
shovelcat | 13:47806f4dbfcd | 103 | // pc->printf("%s: Vd: %.2f\tVm: %.2f\t\n\r", timestamp, Vd, Vm); |
shovelcat | 13:47806f4dbfcd | 104 | |
shovelcat | 13:47806f4dbfcd | 105 | return (float)out; |
shovelcat | 13:47806f4dbfcd | 106 | } |
shovelcat | 13:47806f4dbfcd | 107 | |
shovelcat | 13:47806f4dbfcd | 108 | double SpeedLimiter::piControlFunction(const double reference, const double measured, bool reset) { |
shovelcat | 13:47806f4dbfcd | 109 | // valeurs anterieures |
shovelcat | 13:47806f4dbfcd | 110 | static double ie = 0.0; |
shovelcat | 13:47806f4dbfcd | 111 | if(reset) { |
shovelcat | 13:47806f4dbfcd | 112 | ie = 0.000001; |
shovelcat | 13:47806f4dbfcd | 113 | } |
shovelcat | 13:47806f4dbfcd | 114 | |
shovelcat | 13:47806f4dbfcd | 115 | const double var = readAdcTest(); |
shovelcat | 13:47806f4dbfcd | 116 | |
shovelcat | 13:47806f4dbfcd | 117 | // constants calibrated in vehicle |
LipSer | 15:e9560502e463 | 118 | const double Kp = 13139; // * var ; |
LipSer | 15:e9560502e463 | 119 | const double Ki = 6849; |
LipSer | 15:e9560502e463 | 120 | // const double Kp = 8257; // * var ; |
LipSer | 15:e9560502e463 | 121 | // const double Ki = 19713; |
LipSer | 15:e9560502e463 | 122 | // const double Kp = 18000; // * var ; |
LipSer | 15:e9560502e463 | 123 | // const double Ki = 4300.0; |
shovelcat | 13:47806f4dbfcd | 124 | |
LipSer | 15:e9560502e463 | 125 | pc->printf("Kp: %f Ki: %f", Kp, Ki); |
shovelcat | 13:47806f4dbfcd | 126 | |
shovelcat | 13:47806f4dbfcd | 127 | // calculations |
shovelcat | 13:47806f4dbfcd | 128 | double error = reference - measured; |
LipSer | 15:e9560502e463 | 129 | // const double eMax = 4; |
LipSer | 15:e9560502e463 | 130 | // if(error > eMax) error = eMax; // upper bound the error |
shovelcat | 13:47806f4dbfcd | 131 | const double dt = TRANSFER_FUNCTION_PERIOD; |
shovelcat | 13:47806f4dbfcd | 132 | ie = ie + error*dt; |
shovelcat | 13:47806f4dbfcd | 133 | |
shovelcat | 13:47806f4dbfcd | 134 | double out = Kp*error + Ki*ie; |
shovelcat | 13:47806f4dbfcd | 135 | return out; |
shovelcat | 13:47806f4dbfcd | 136 | } |
shovelcat | 13:47806f4dbfcd | 137 | |
shovelcat | 13:47806f4dbfcd | 138 | double SpeedLimiter::ipControlFunction(const double reference, const double measured, bool reset) { |
shovelcat | 13:47806f4dbfcd | 139 | // valeurs anterieures |
shovelcat | 13:47806f4dbfcd | 140 | static double ie = 0.0; |
shovelcat | 13:47806f4dbfcd | 141 | if(reset) { |
shovelcat | 13:47806f4dbfcd | 142 | ie = 0.000001; |
shovelcat | 13:47806f4dbfcd | 143 | } |
shovelcat | 13:47806f4dbfcd | 144 | |
shovelcat | 13:47806f4dbfcd | 145 | // constants calibrated in vehicle |
LipSer | 15:e9560502e463 | 146 | const double Kp = 6217; |
LipSer | 15:e9560502e463 | 147 | const double Ki = 1.8142; |
LipSer | 15:e9560502e463 | 148 | |
LipSer | 15:e9560502e463 | 149 | pc->printf("Kp: %f Ki: %f", Kp, Ki); |
shovelcat | 13:47806f4dbfcd | 150 | |
shovelcat | 13:47806f4dbfcd | 151 | // calculations |
shovelcat | 13:47806f4dbfcd | 152 | double errorI = reference - measured; |
shovelcat | 13:47806f4dbfcd | 153 | |
shovelcat | 13:47806f4dbfcd | 154 | const double dt = TRANSFER_FUNCTION_PERIOD; |
shovelcat | 13:47806f4dbfcd | 155 | ie = ie + errorI*dt; |
shovelcat | 13:47806f4dbfcd | 156 | |
shovelcat | 13:47806f4dbfcd | 157 | double out = Ki*ie; |
shovelcat | 13:47806f4dbfcd | 158 | |
shovelcat | 13:47806f4dbfcd | 159 | double errorP = out - measured; |
shovelcat | 13:47806f4dbfcd | 160 | out = Kp*errorP; |
shovelcat | 13:47806f4dbfcd | 161 | |
shovelcat | 13:47806f4dbfcd | 162 | return out; |
shovelcat | 13:47806f4dbfcd | 163 | } |
shovelcat | 13:47806f4dbfcd | 164 | |
shovelcat | 3:4da392d2bae8 | 165 | // Returns voltage read on analog input port chosen for pedal input 1 |
shovelcat | 3:4da392d2bae8 | 166 | float SpeedLimiter::readAdcPedalHi() |
shovelcat | 3:4da392d2bae8 | 167 | { |
shovelcat | 3:4da392d2bae8 | 168 | const float decPcValue = _pedalInHi.read(); |
shovelcat | 3:4da392d2bae8 | 169 | const float voltage = decPcValue * ADC_INPUT_MAX_VALUE; |
shovelcat | 3:4da392d2bae8 | 170 | return voltage; |
shovelcat | 3:4da392d2bae8 | 171 | } |
shovelcat | 3:4da392d2bae8 | 172 | |
shovelcat | 3:4da392d2bae8 | 173 | // Returns voltage read on analog input port chosen for pedal input 2 |
shovelcat | 3:4da392d2bae8 | 174 | float SpeedLimiter::readAdcPedalLo() |
shovelcat | 3:4da392d2bae8 | 175 | { |
shovelcat | 3:4da392d2bae8 | 176 | const float decPcValue = _pedalInLo.read(); |
shovelcat | 3:4da392d2bae8 | 177 | const float voltage = decPcValue * ADC_INPUT_MAX_VALUE; |
shovelcat | 3:4da392d2bae8 | 178 | return voltage; |
shovelcat | 3:4da392d2bae8 | 179 | } |
shovelcat | 2:06f128641b62 | 180 | |
shovelcat | 3:4da392d2bae8 | 181 | // Accepts a value in volts, converts to % and sets ADC for pedal output 1 |
shovelcat | 3:4da392d2bae8 | 182 | void SpeedLimiter::writeAdcPedalHi(const float voltage) |
shovelcat | 3:4da392d2bae8 | 183 | { |
shovelcat | 3:4da392d2bae8 | 184 | const float boundedValue = boundValue(voltage, PEDAL_HI_MIN_VALUE, PEDAL_HI_MAX_VALUE); |
shovelcat | 3:4da392d2bae8 | 185 | const float decValue = voltageToDecimal(boundedValue, ADC_OUTPUT_MAX_VALUE); |
shovelcat | 3:4da392d2bae8 | 186 | _pedalOutHi.write(decValue); |
shovelcat | 3:4da392d2bae8 | 187 | } |
shovelcat | 3:4da392d2bae8 | 188 | |
shovelcat | 3:4da392d2bae8 | 189 | // Accepts a value in volts, converts to % and sets ADC for pedal output 2 |
shovelcat | 3:4da392d2bae8 | 190 | void SpeedLimiter::writeAdcPedalLo(const float voltage) |
shovelcat | 3:4da392d2bae8 | 191 | { |
shovelcat | 3:4da392d2bae8 | 192 | const float boundedValue = boundValue(voltage, PEDAL_LO_MIN_VALUE, PEDAL_LO_MAX_VALUE); |
shovelcat | 3:4da392d2bae8 | 193 | const float decValue = voltageToDecimal(boundedValue, ADC_OUTPUT_MAX_VALUE); |
shovelcat | 3:4da392d2bae8 | 194 | _pedalOutLo.write(decValue); |
shovelcat | 3:4da392d2bae8 | 195 | } |
shovelcat | 3:4da392d2bae8 | 196 | |
shovelcat | 3:4da392d2bae8 | 197 | // Returns 'value' bounded between 'lowerBound' and 'upperBound' |
shovelcat | 3:4da392d2bae8 | 198 | float SpeedLimiter::boundValue(float value, const float lowerBound, const float upperBound) |
shovelcat | 3:4da392d2bae8 | 199 | { |
shovelcat | 3:4da392d2bae8 | 200 | if(value < lowerBound) { |
shovelcat | 3:4da392d2bae8 | 201 | value = lowerBound; |
shovelcat | 3:4da392d2bae8 | 202 | } else if(value > upperBound) { |
shovelcat | 3:4da392d2bae8 | 203 | value = upperBound; |
shovelcat | 3:4da392d2bae8 | 204 | } |
shovelcat | 3:4da392d2bae8 | 205 | return value; |
shovelcat | 3:4da392d2bae8 | 206 | } |
shovelcat | 3:4da392d2bae8 | 207 | |
shovelcat | 3:4da392d2bae8 | 208 | // Returns "value/reference" as a percentage in decimal form (0.5 for 50%) |
shovelcat | 3:4da392d2bae8 | 209 | float SpeedLimiter::voltageToDecimal(const float voltage, const float reference) |
shovelcat | 3:4da392d2bae8 | 210 | { |
shovelcat | 3:4da392d2bae8 | 211 | return voltage/reference; |
shovelcat | 3:4da392d2bae8 | 212 | } |