Hesu-eco / Mbed 2 deprecated limiteur_vitesse

premiere ebauche

Dependencies:   mbed PinDetect

speedlimiter.cpp@13:47806f4dbfcd, 2018-12-13 (annotated)

Committer:
shovelcat
Date:
Thu Dec 13 19:18:39 2018 +0000
Revision:
13:47806f4dbfcd
Parent:
12:cf97590d9df2
Child:
14:70fc660d4386
modifs pour essais Kp et Ki pour Joao

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 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
shovelcat 13:47806f4dbfcd 79 out = piControlFunction(Vd, Vm);
shovelcat 13:47806f4dbfcd 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;
shovelcat 13:47806f4dbfcd 92 piControlFunction(0,reset);
shovelcat 13:47806f4dbfcd 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
shovelcat 13:47806f4dbfcd 118 const double Kp = 18000 * var ;
shovelcat 13:47806f4dbfcd 119 const double Ki = 4300.0;
shovelcat 13:47806f4dbfcd 120
shovelcat 13:47806f4dbfcd 121 pc->printf("Current Kp value: %f\n\r\n\r", Kp);
shovelcat 13:47806f4dbfcd 122
shovelcat 13:47806f4dbfcd 123 // calculations
shovelcat 13:47806f4dbfcd 124 double error = reference - measured;
shovelcat 13:47806f4dbfcd 125 const double eMax = 4;
shovelcat 13:47806f4dbfcd 126 if(error > eMax) error = eMax; // upper bound the error
shovelcat 13:47806f4dbfcd 127 const double dt = TRANSFER_FUNCTION_PERIOD;
shovelcat 13:47806f4dbfcd 128 ie = ie + error*dt;
shovelcat 13:47806f4dbfcd 129
shovelcat 13:47806f4dbfcd 130 double out = Kp*error + Ki*ie;
shovelcat 13:47806f4dbfcd 131 return out;
shovelcat 13:47806f4dbfcd 132 }
shovelcat 13:47806f4dbfcd 133
shovelcat 13:47806f4dbfcd 134 double SpeedLimiter::ipControlFunction(const double reference, const double measured, bool reset) {
shovelcat 13:47806f4dbfcd 135 // valeurs anterieures
shovelcat 13:47806f4dbfcd 136 static double ie = 0.0;
shovelcat 13:47806f4dbfcd 137 if(reset) {
shovelcat 13:47806f4dbfcd 138 ie = 0.000001;
shovelcat 13:47806f4dbfcd 139 }
shovelcat 13:47806f4dbfcd 140
shovelcat 13:47806f4dbfcd 141 // constants calibrated in vehicle
shovelcat 13:47806f4dbfcd 142 const double Kp = 18000 ;
shovelcat 13:47806f4dbfcd 143 const double Ki = 4300.0;
shovelcat 13:47806f4dbfcd 144
shovelcat 13:47806f4dbfcd 145 // calculations
shovelcat 13:47806f4dbfcd 146 double errorI = reference - measured;
shovelcat 13:47806f4dbfcd 147
shovelcat 13:47806f4dbfcd 148 const double dt = TRANSFER_FUNCTION_PERIOD;
shovelcat 13:47806f4dbfcd 149 ie = ie + errorI*dt;
shovelcat 13:47806f4dbfcd 150
shovelcat 13:47806f4dbfcd 151 double out = Ki*ie;
shovelcat 13:47806f4dbfcd 152
shovelcat 13:47806f4dbfcd 153 double errorP = out - measured;
shovelcat 13:47806f4dbfcd 154 out = Kp*errorP;
shovelcat 13:47806f4dbfcd 155
shovelcat 13:47806f4dbfcd 156 return out;
shovelcat 13:47806f4dbfcd 157 }
shovelcat 13:47806f4dbfcd 158
shovelcat 3:4da392d2bae8 159 // Returns voltage read on analog input port chosen for pedal input 1
shovelcat 3:4da392d2bae8 160 float SpeedLimiter::readAdcPedalHi()
shovelcat 3:4da392d2bae8 161 {
shovelcat 3:4da392d2bae8 162 const float decPcValue = _pedalInHi.read();
shovelcat 3:4da392d2bae8 163 const float voltage = decPcValue * ADC_INPUT_MAX_VALUE;
shovelcat 3:4da392d2bae8 164 return voltage;
shovelcat 3:4da392d2bae8 165 }
shovelcat 3:4da392d2bae8 166
shovelcat 3:4da392d2bae8 167 // Returns voltage read on analog input port chosen for pedal input 2
shovelcat 3:4da392d2bae8 168 float SpeedLimiter::readAdcPedalLo()
shovelcat 3:4da392d2bae8 169 {
shovelcat 3:4da392d2bae8 170 const float decPcValue = _pedalInLo.read();
shovelcat 3:4da392d2bae8 171 const float voltage = decPcValue * ADC_INPUT_MAX_VALUE;
shovelcat 3:4da392d2bae8 172 return voltage;
shovelcat 3:4da392d2bae8 173 }
shovelcat 2:06f128641b62 174
shovelcat 3:4da392d2bae8 175 // Accepts a value in volts, converts to % and sets ADC for pedal output 1
shovelcat 3:4da392d2bae8 176 void SpeedLimiter::writeAdcPedalHi(const float voltage)
shovelcat 3:4da392d2bae8 177 {
shovelcat 3:4da392d2bae8 178 const float boundedValue = boundValue(voltage, PEDAL_HI_MIN_VALUE, PEDAL_HI_MAX_VALUE);
shovelcat 3:4da392d2bae8 179 const float decValue = voltageToDecimal(boundedValue, ADC_OUTPUT_MAX_VALUE);
shovelcat 3:4da392d2bae8 180 _pedalOutHi.write(decValue);
shovelcat 3:4da392d2bae8 181 }
shovelcat 3:4da392d2bae8 182
shovelcat 3:4da392d2bae8 183 // Accepts a value in volts, converts to % and sets ADC for pedal output 2
shovelcat 3:4da392d2bae8 184 void SpeedLimiter::writeAdcPedalLo(const float voltage)
shovelcat 3:4da392d2bae8 185 {
shovelcat 3:4da392d2bae8 186 const float boundedValue = boundValue(voltage, PEDAL_LO_MIN_VALUE, PEDAL_LO_MAX_VALUE);
shovelcat 3:4da392d2bae8 187 const float decValue = voltageToDecimal(boundedValue, ADC_OUTPUT_MAX_VALUE);
shovelcat 3:4da392d2bae8 188 _pedalOutLo.write(decValue);
shovelcat 3:4da392d2bae8 189 }
shovelcat 3:4da392d2bae8 190
shovelcat 3:4da392d2bae8 191 // Returns 'value' bounded between 'lowerBound' and 'upperBound'
shovelcat 3:4da392d2bae8 192 float SpeedLimiter::boundValue(float value, const float lowerBound, const float upperBound)
shovelcat 3:4da392d2bae8 193 {
shovelcat 3:4da392d2bae8 194 if(value < lowerBound) {
shovelcat 3:4da392d2bae8 195 value = lowerBound;
shovelcat 3:4da392d2bae8 196 } else if(value > upperBound) {
shovelcat 3:4da392d2bae8 197 value = upperBound;
shovelcat 3:4da392d2bae8 198 }
shovelcat 3:4da392d2bae8 199 return value;
shovelcat 3:4da392d2bae8 200 }
shovelcat 3:4da392d2bae8 201
shovelcat 3:4da392d2bae8 202 // Returns "value/reference" as a percentage in decimal form (0.5 for 50%)
shovelcat 3:4da392d2bae8 203 float SpeedLimiter::voltageToDecimal(const float voltage, const float reference)
shovelcat 3:4da392d2bae8 204 {
shovelcat 3:4da392d2bae8 205 return voltage/reference;
shovelcat 3:4da392d2bae8 206 }