Hesu-eco / Mbed 2 deprecated limiteur_vitesse

premiere ebauche

Dependencies:   mbed PinDetect

speedlimiter.cpp@9:3e8bb4aba7c7, 2018-11-06 (annotated)

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