Hesu-eco / Mbed 2 deprecated limiteur_vitesse

premiere ebauche

Dependencies:   mbed PinDetect

speedlimiter.cpp@8:51549d7108dd, 2018-11-06 (annotated)

Committer:
shovelcat
Date:
Tue Nov 06 22:07:05 2018 +0000
Revision:
8:51549d7108dd
Parent:
7:d636d073b942
Child:
9:3e8bb4aba7c7
update pour les tests a Myriam et Sophia

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 8:51549d7108dd 9 const float valeursVitessesRecommandes[20] = {0,2.7546,5.5052,8.2467,10.974,13.682,16.365,19.23,22.058,24.846,27.587,29.858,29.981,29.876,29.968,29.833,29.895,29.941,29.888,29.937};
shovelcat 8:51549d7108dd 10
shovelcat 8:51549d7108dd 11 float getNextReferenceSpeed() {
shovelcat 8:51549d7108dd 12 static int index = 0;
shovelcat 8:51549d7108dd 13 return valeursVitessesRecommandes[index++];
shovelcat 8:51549d7108dd 14 }
shovelcat 8:51549d7108dd 15
shovelcat 8:51549d7108dd 16
shovelcat 8:51549d7108dd 17
shovelcat 3:4da392d2bae8 18 Serial* SpeedLimiter::pc = new Serial(USBTX, USBRX);
shovelcat 3:4da392d2bae8 19
shovelcat 2:06f128641b62 20 SpeedLimiter::SpeedLimiter(const PinName& pedalInHi, const PinName& pedalInLo, const PinName& pedalOutHi, const PinName& pedalOutLo)
shovelcat 2:06f128641b62 21 : _pedalInHi(pedalInHi)
shovelcat 2:06f128641b62 22 , _pedalInLo(pedalInLo)
shovelcat 2:06f128641b62 23 , _pedalOutHi(pedalOutHi)
shovelcat 2:06f128641b62 24 , _pedalOutLo(pedalOutLo)
shovelcat 3:4da392d2bae8 25 , _referenceSpeed(DISABLE_ECO_ALGO_TRIGGER)
shovelcat 3:4da392d2bae8 26 , _measuredSpeed(0.0)
shovelcat 2:06f128641b62 27 {
shovelcat 3:4da392d2bae8 28 }
shovelcat 3:4da392d2bae8 29
shovelcat 3:4da392d2bae8 30 SpeedLimiter::~SpeedLimiter()
shovelcat 3:4da392d2bae8 31 {
shovelcat 3:4da392d2bae8 32 }
shovelcat 3:4da392d2bae8 33
shovelcat 3:4da392d2bae8 34 void SpeedLimiter::ipControllerTransferFunction()
shovelcat 3:4da392d2bae8 35 {
shovelcat 8:51549d7108dd 36 static int counter = 0;
shovelcat 8:51549d7108dd 37 // une fois par 10 * 0.1 secondes -> 1 seconde
shovelcat 8:51549d7108dd 38 if (counter < 10 - 1) {
shovelcat 8:51549d7108dd 39 counter++;
shovelcat 8:51549d7108dd 40 }
shovelcat 8:51549d7108dd 41 else {
shovelcat 8:51549d7108dd 42 counter = 0;
shovelcat 8:51549d7108dd 43 float nextReferenceSpeed = getNextReferenceSpeed();
shovelcat 8:51549d7108dd 44 setReferenceSpeed(getNextReferenceSpeed());
shovelcat 8:51549d7108dd 45 pc->printf("%f\n\r", nextReferenceSpeed);
shovelcat 8:51549d7108dd 46 }
shovelcat 3:4da392d2bae8 47 // write voltages at beginning of function to prevent jitter
shovelcat 3:4da392d2bae8 48 // voltage will be delayed by 1 call which is okay.
shovelcat 4:a8c9f6a13633 49 // pc->printf("H\n\r");
shovelcat 4:a8c9f6a13633 50 const float voutHi = getOutputPedalVoltageHi();
shovelcat 4:a8c9f6a13633 51 const float voutLo = getOutputPedalVoltageLo();
shovelcat 4:a8c9f6a13633 52
shovelcat 4:a8c9f6a13633 53 // pc->printf("Hi: %f\t Lo: %f\n\r", voutHi, voutLo);
shovelcat 4:a8c9f6a13633 54
shovelcat 4:a8c9f6a13633 55 writeAdcPedalHi(voutHi);
shovelcat 4:a8c9f6a13633 56 writeAdcPedalLo(voutLo);
shovelcat 4:a8c9f6a13633 57
shovelcat 3:4da392d2bae8 58 // calculate voltage for next call
shovelcat 3:4da392d2bae8 59 const float referenceSpeed = getReferenceSpeed();
shovelcat 3:4da392d2bae8 60 float outputAdcVoltageHi = 0;
shovelcat 3:4da392d2bae8 61 float outputAdcVoltageLo = 0;
shovelcat 3:4da392d2bae8 62
shovelcat 3:4da392d2bae8 63 if(referenceSpeed == DISABLE_ECO_ALGO_TRIGGER) {
shovelcat 3:4da392d2bae8 64 outputAdcVoltageHi = ecoDisabledAlgorithm();
shovelcat 3:4da392d2bae8 65 } else {
shovelcat 3:4da392d2bae8 66 outputAdcVoltageHi = ecoEnabledAlgorithm();
shovelcat 3:4da392d2bae8 67 }
shovelcat 3:4da392d2bae8 68
shovelcat 4:a8c9f6a13633 69 // outputAdcVoltageHi = ADC_OUTPUT_MAX_VALUE / 2;
shovelcat 3:4da392d2bae8 70 outputAdcVoltageLo = outputAdcVoltageHi / 2;
shovelcat 3:4da392d2bae8 71
shovelcat 4:a8c9f6a13633 72 // pc->printf("tmpHi: %f\t tmpLo: %f\n\r", outputAdcVoltageHi, outputAdcVoltageLo);
shovelcat 4:a8c9f6a13633 73
shovelcat 3:4da392d2bae8 74 setOutputPedalVoltageHi(outputAdcVoltageHi);
shovelcat 3:4da392d2bae8 75 setOutputPedalVoltageLo(outputAdcVoltageLo);
shovelcat 3:4da392d2bae8 76 }
shovelcat 3:4da392d2bae8 77
shovelcat 3:4da392d2bae8 78 // Returns voltage read on analog input port chosen for pedal input 1
shovelcat 3:4da392d2bae8 79 float SpeedLimiter::readAdcPedalHi()
shovelcat 3:4da392d2bae8 80 {
shovelcat 3:4da392d2bae8 81 const float decPcValue = _pedalInHi.read();
shovelcat 3:4da392d2bae8 82 const float voltage = decPcValue * ADC_INPUT_MAX_VALUE;
shovelcat 3:4da392d2bae8 83 return voltage;
shovelcat 3:4da392d2bae8 84 }
shovelcat 3:4da392d2bae8 85
shovelcat 3:4da392d2bae8 86 // Returns voltage read on analog input port chosen for pedal input 2
shovelcat 3:4da392d2bae8 87 float SpeedLimiter::readAdcPedalLo()
shovelcat 3:4da392d2bae8 88 {
shovelcat 3:4da392d2bae8 89 const float decPcValue = _pedalInLo.read();
shovelcat 3:4da392d2bae8 90 const float voltage = decPcValue * ADC_INPUT_MAX_VALUE;
shovelcat 3:4da392d2bae8 91 return voltage;
shovelcat 3:4da392d2bae8 92 }
shovelcat 2:06f128641b62 93
shovelcat 3:4da392d2bae8 94 // Accepts a value in volts, converts to % and sets ADC for pedal output 1
shovelcat 3:4da392d2bae8 95 void SpeedLimiter::writeAdcPedalHi(const float voltage)
shovelcat 3:4da392d2bae8 96 {
shovelcat 3:4da392d2bae8 97 const float boundedValue = boundValue(voltage, PEDAL_HI_MIN_VALUE, PEDAL_HI_MAX_VALUE);
shovelcat 3:4da392d2bae8 98 const float decValue = voltageToDecimal(boundedValue, ADC_OUTPUT_MAX_VALUE);
shovelcat 3:4da392d2bae8 99 _pedalOutHi.write(decValue);
shovelcat 3:4da392d2bae8 100 }
shovelcat 3:4da392d2bae8 101
shovelcat 3:4da392d2bae8 102 // Accepts a value in volts, converts to % and sets ADC for pedal output 2
shovelcat 3:4da392d2bae8 103 void SpeedLimiter::writeAdcPedalLo(const float voltage)
shovelcat 3:4da392d2bae8 104 {
shovelcat 3:4da392d2bae8 105 const float boundedValue = boundValue(voltage, PEDAL_LO_MIN_VALUE, PEDAL_LO_MAX_VALUE);
shovelcat 3:4da392d2bae8 106 const float decValue = voltageToDecimal(boundedValue, ADC_OUTPUT_MAX_VALUE);
shovelcat 3:4da392d2bae8 107 _pedalOutLo.write(decValue);
shovelcat 3:4da392d2bae8 108 }
shovelcat 3:4da392d2bae8 109
shovelcat 3:4da392d2bae8 110 float SpeedLimiter::ecoDisabledAlgorithm()
shovelcat 3:4da392d2bae8 111 {
shovelcat 3:4da392d2bae8 112 const float value = readAdcPedalHi();
shovelcat 3:4da392d2bae8 113 return value;
shovelcat 3:4da392d2bae8 114 }
shovelcat 3:4da392d2bae8 115
shovelcat 3:4da392d2bae8 116 float SpeedLimiter::ecoEnabledAlgorithm()
shovelcat 3:4da392d2bae8 117 {
shovelcat 5:aef1fc6c0df1 118 static bool first_acquisition = true;
shovelcat 6:1a4ed88c9a4b 119
shovelcat 7:d636d073b942 120 // constants calibrated in vehicul
shovelcat 6:1a4ed88c9a4b 121 double var = readAdcTest();
shovelcat 7:d636d073b942 122 double Kp = 18000 ;
shovelcat 7:d636d073b942 123 double Ki = 4300.0;
shovelcat 3:4da392d2bae8 124
shovelcat 5:aef1fc6c0df1 125 // valeurs anterieures
shovelcat 6:1a4ed88c9a4b 126 static double ie = 0.0;
shovelcat 3:4da392d2bae8 127
shovelcat 3:4da392d2bae8 128 // calculs
shovelcat 5:aef1fc6c0df1 129 double Vm = getMeasuredSpeed();
shovelcat 7:d636d073b942 130 double Vd = 20;
shovelcat 5:aef1fc6c0df1 131 double out = 0.0;
shovelcat 4:a8c9f6a13633 132 if (readAdcPedalHi() > 1) {
shovelcat 6:1a4ed88c9a4b 133 double e = Vd - Vm;
shovelcat 7:d636d073b942 134 double eMax = 4;
shovelcat 7:d636d073b942 135 if(e > eMax) e = eMax;
shovelcat 6:1a4ed88c9a4b 136 const double dt = TRANSFER_FUNCTION_PERIOD;
shovelcat 6:1a4ed88c9a4b 137 ie = ie + e*dt;
shovelcat 6:1a4ed88c9a4b 138
shovelcat 6:1a4ed88c9a4b 139 out = Kp*e + Ki*ie;
shovelcat 5:aef1fc6c0df1 140
shovelcat 5:aef1fc6c0df1 141 // out est maintenant en 'force'
shovelcat 5:aef1fc6c0df1 142 out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
shovelcat 3:4da392d2bae8 143
shovelcat 5:aef1fc6c0df1 144 if(first_acquisition) {
shovelcat 5:aef1fc6c0df1 145 pc->printf("Acquisition start:\n\r");
shovelcat 5:aef1fc6c0df1 146 first_acquisition = false;
shovelcat 5:aef1fc6c0df1 147 }
shovelcat 5:aef1fc6c0df1 148 } else {
shovelcat 6:1a4ed88c9a4b 149 ie = 0.00000000001;
shovelcat 5:aef1fc6c0df1 150 first_acquisition = true;
shovelcat 4:a8c9f6a13633 151 }
shovelcat 3:4da392d2bae8 152
shovelcat 6:1a4ed88c9a4b 153 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 154
shovelcat 5:aef1fc6c0df1 155 return (float)out;
shovelcat 5:aef1fc6c0df1 156 }
shovelcat 5:aef1fc6c0df1 157
shovelcat 3:4da392d2bae8 158 // Returns 'value' bounded between 'lowerBound' and 'upperBound'
shovelcat 3:4da392d2bae8 159 float SpeedLimiter::boundValue(float value, const float lowerBound, const float upperBound)
shovelcat 3:4da392d2bae8 160 {
shovelcat 3:4da392d2bae8 161 if(value < lowerBound) {
shovelcat 3:4da392d2bae8 162 value = lowerBound;
shovelcat 3:4da392d2bae8 163 } else if(value > upperBound) {
shovelcat 3:4da392d2bae8 164 value = upperBound;
shovelcat 3:4da392d2bae8 165 }
shovelcat 3:4da392d2bae8 166 return value;
shovelcat 3:4da392d2bae8 167 }
shovelcat 3:4da392d2bae8 168
shovelcat 3:4da392d2bae8 169 // Returns "value/reference" as a percentage in decimal form (0.5 for 50%)
shovelcat 3:4da392d2bae8 170 float SpeedLimiter::voltageToDecimal(const float voltage, const float reference)
shovelcat 3:4da392d2bae8 171 {
shovelcat 3:4da392d2bae8 172 return voltage/reference;
shovelcat 3:4da392d2bae8 173 }