Hesu-eco / Mbed 2 deprecated limiteur_vitesse

premiere ebauche

Dependencies:   mbed PinDetect

speedlimiter.cpp@12:cf97590d9df2, 2018-11-18 (annotated)

Committer:
shovelcat
Date:
Sun Nov 18 03:30:57 2018 +0000
Revision:
12:cf97590d9df2
Parent:
10:f0b382368614
Child:
13:47806f4dbfcd
"Plan B" samedi avec Sophia. Ajoute le timestamp, ajouter du code pas encore test pour le CAN (Energy SoC, motorCurrent)

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 12:cf97590d9df2 115 static time_t referenceTime = time(NULL); // seconds since January 1, 1970, a.k.a. since reset (time not set)
shovelcat 6:1a4ed88c9a4b 116
shovelcat 7:d636d073b942 117 // constants calibrated in vehicul
shovelcat 6:1a4ed88c9a4b 118 double var = readAdcTest();
shovelcat 7:d636d073b942 119 double Kp = 18000 ;
shovelcat 7:d636d073b942 120 double Ki = 4300.0;
shovelcat 3:4da392d2bae8 121
shovelcat 5:aef1fc6c0df1 122 // valeurs anterieures
shovelcat 6:1a4ed88c9a4b 123 static double ie = 0.0;
shovelcat 3:4da392d2bae8 124
shovelcat 3:4da392d2bae8 125 // calculs
shovelcat 5:aef1fc6c0df1 126 double Vm = getMeasuredSpeed();
shovelcat 9:3e8bb4aba7c7 127 double Vd = getReferenceSpeed();
shovelcat 9:3e8bb4aba7c7 128 //double Vd = 20;
shovelcat 5:aef1fc6c0df1 129 double out = 0.0;
shovelcat 4:a8c9f6a13633 130 if (readAdcPedalHi() > 1) {
shovelcat 6:1a4ed88c9a4b 131 double e = Vd - Vm;
shovelcat 7:d636d073b942 132 double eMax = 4;
shovelcat 7:d636d073b942 133 if(e > eMax) e = eMax;
shovelcat 6:1a4ed88c9a4b 134 const double dt = TRANSFER_FUNCTION_PERIOD;
shovelcat 6:1a4ed88c9a4b 135 ie = ie + e*dt;
shovelcat 6:1a4ed88c9a4b 136
shovelcat 6:1a4ed88c9a4b 137 out = Kp*e + Ki*ie;
shovelcat 5:aef1fc6c0df1 138
shovelcat 5:aef1fc6c0df1 139 // out est maintenant en 'force'
shovelcat 5:aef1fc6c0df1 140 out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
shovelcat 3:4da392d2bae8 141
shovelcat 5:aef1fc6c0df1 142 if(first_acquisition) {
shovelcat 5:aef1fc6c0df1 143 pc->printf("Acquisition start:\n\r");
shovelcat 12:cf97590d9df2 144 referenceTime = time(NULL);
shovelcat 5:aef1fc6c0df1 145 first_acquisition = false;
shovelcat 5:aef1fc6c0df1 146 }
shovelcat 5:aef1fc6c0df1 147 } else {
shovelcat 6:1a4ed88c9a4b 148 ie = 0.00000000001;
shovelcat 5:aef1fc6c0df1 149 first_acquisition = true;
shovelcat 4:a8c9f6a13633 150 }
shovelcat 12:cf97590d9df2 151
shovelcat 12:cf97590d9df2 152 time_t elapsedTime = time(NULL) - referenceTime;
shovelcat 12:cf97590d9df2 153 pc->printf("%d: Vd: %.2f\tVm: %.2f\t\n\r", elapsedTime, Vd, Vm);
shovelcat 3:4da392d2bae8 154
shovelcat 12:cf97590d9df2 155 // char timestamp[32];
shovelcat 12:cf97590d9df2 156 // strftime(timestamp, 32, "%I:%M %p\n", localtime(&seconds));
shovelcat 12:cf97590d9df2 157 // pc->printf("%s: Vd: %.2f\tVm: %.2f\t\n\r", timestamp, Vd, Vm);
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 }