Hesu-eco / Mbed 2 deprecated limiteur_vitesse

premiere ebauche

Dependencies:   mbed PinDetect

speedlimiter.cpp@7:d636d073b942, 2018-10-26 (annotated)

Committer:
shovelcat
Date:
Fri Oct 26 17:50:47 2018 +0000
Revision:
7:d636d073b942
Parent:
6:1a4ed88c9a4b
Child:
8:51549d7108dd
sature l'erreur a une valeur maximum pour limiter l'acceleration de depart

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 6:1a4ed88c9a4b 99
shovelcat 7:d636d073b942 100 // constants calibrated in vehicul
shovelcat 6:1a4ed88c9a4b 101 double var = readAdcTest();
shovelcat 7:d636d073b942 102 double Kp = 18000 ;
shovelcat 7:d636d073b942 103 double Ki = 4300.0;
shovelcat 3:4da392d2bae8 104
shovelcat 5:aef1fc6c0df1 105 // valeurs anterieures
shovelcat 6:1a4ed88c9a4b 106 static double ie = 0.0;
shovelcat 3:4da392d2bae8 107
shovelcat 3:4da392d2bae8 108 // calculs
shovelcat 5:aef1fc6c0df1 109 double Vm = getMeasuredSpeed();
shovelcat 7:d636d073b942 110 double Vd = 20;
shovelcat 5:aef1fc6c0df1 111 double out = 0.0;
shovelcat 4:a8c9f6a13633 112 if (readAdcPedalHi() > 1) {
shovelcat 6:1a4ed88c9a4b 113 double e = Vd - Vm;
shovelcat 7:d636d073b942 114 double eMax = 4;
shovelcat 7:d636d073b942 115 if(e > eMax) e = eMax;
shovelcat 6:1a4ed88c9a4b 116 const double dt = TRANSFER_FUNCTION_PERIOD;
shovelcat 6:1a4ed88c9a4b 117 ie = ie + e*dt;
shovelcat 6:1a4ed88c9a4b 118
shovelcat 6:1a4ed88c9a4b 119 out = Kp*e + Ki*ie;
shovelcat 5:aef1fc6c0df1 120
shovelcat 5:aef1fc6c0df1 121 // out est maintenant en 'force'
shovelcat 5:aef1fc6c0df1 122 out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
shovelcat 3:4da392d2bae8 123
shovelcat 5:aef1fc6c0df1 124 if(first_acquisition) {
shovelcat 5:aef1fc6c0df1 125 pc->printf("Acquisition start:\n\r");
shovelcat 5:aef1fc6c0df1 126 first_acquisition = false;
shovelcat 5:aef1fc6c0df1 127 }
shovelcat 5:aef1fc6c0df1 128 } else {
shovelcat 6:1a4ed88c9a4b 129 ie = 0.00000000001;
shovelcat 5:aef1fc6c0df1 130 first_acquisition = true;
shovelcat 4:a8c9f6a13633 131 }
shovelcat 3:4da392d2bae8 132
shovelcat 6:1a4ed88c9a4b 133 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 134
shovelcat 5:aef1fc6c0df1 135 return (float)out;
shovelcat 5:aef1fc6c0df1 136 }
shovelcat 5:aef1fc6c0df1 137
shovelcat 3:4da392d2bae8 138 // Returns 'value' bounded between 'lowerBound' and 'upperBound'
shovelcat 3:4da392d2bae8 139 float SpeedLimiter::boundValue(float value, const float lowerBound, const float upperBound)
shovelcat 3:4da392d2bae8 140 {
shovelcat 3:4da392d2bae8 141 if(value < lowerBound) {
shovelcat 3:4da392d2bae8 142 value = lowerBound;
shovelcat 3:4da392d2bae8 143 } else if(value > upperBound) {
shovelcat 3:4da392d2bae8 144 value = upperBound;
shovelcat 3:4da392d2bae8 145 }
shovelcat 3:4da392d2bae8 146 return value;
shovelcat 3:4da392d2bae8 147 }
shovelcat 3:4da392d2bae8 148
shovelcat 3:4da392d2bae8 149 // Returns "value/reference" as a percentage in decimal form (0.5 for 50%)
shovelcat 3:4da392d2bae8 150 float SpeedLimiter::voltageToDecimal(const float voltage, const float reference)
shovelcat 3:4da392d2bae8 151 {
shovelcat 3:4da392d2bae8 152 return voltage/reference;
shovelcat 3:4da392d2bae8 153 }