Hesu-eco / Mbed 2 deprecated limiteur_vitesse

premiere ebauche

Dependencies:   mbed PinDetect

speedlimiter.cpp@10:f0b382368614, 2018-11-16 (annotated)

Committer:
shovelcat
Date:
Fri Nov 16 17:47:35 2018 +0000
Revision:
10:f0b382368614
Parent:
9:3e8bb4aba7c7
Child:
12:cf97590d9df2
"Plan B" pour tests routiers

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