Hesu-eco / Mbed 2 deprecated limiteur_vitesse

premiere ebauche

Dependencies:   mbed PinDetect

speedlimiter.cpp@3:4da392d2bae8, 2018-10-18 (annotated)

Committer:
shovelcat
Date:
Thu Oct 18 17:31:03 2018 +0000
Revision:
3:4da392d2bae8
Parent:
2:06f128641b62
Child:
4:a8c9f6a13633
version avec Objet. version sans eco-conduite (eco disabled) fonctionne sur le vehicule. Reste a valider la fonction de transfert et fonctionner sur le vehicule.

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 3:4da392d2bae8 8 Serial* SpeedLimiter::pc = new Serial(USBTX, USBRX);
shovelcat 3:4da392d2bae8 9
shovelcat 2:06f128641b62 10 SpeedLimiter::SpeedLimiter(const PinName& pedalInHi, const PinName& pedalInLo, const PinName& pedalOutHi, const PinName& pedalOutLo)
shovelcat 2:06f128641b62 11 : _pedalInHi(pedalInHi)
shovelcat 2:06f128641b62 12 , _pedalInLo(pedalInLo)
shovelcat 2:06f128641b62 13 , _pedalOutHi(pedalOutHi)
shovelcat 2:06f128641b62 14 , _pedalOutLo(pedalOutLo)
shovelcat 3:4da392d2bae8 15 , _referenceSpeed(DISABLE_ECO_ALGO_TRIGGER)
shovelcat 3:4da392d2bae8 16 , _measuredSpeed(0.0)
shovelcat 3:4da392d2bae8 17 , _ticker(new Ticker)
shovelcat 3:4da392d2bae8 18 , _mutex(new Mutex)
shovelcat 2:06f128641b62 19 {
shovelcat 3:4da392d2bae8 20 }
shovelcat 3:4da392d2bae8 21
shovelcat 3:4da392d2bae8 22 SpeedLimiter::~SpeedLimiter()
shovelcat 3:4da392d2bae8 23 {
shovelcat 3:4da392d2bae8 24 delete _ticker;
shovelcat 3:4da392d2bae8 25 delete _mutex;
shovelcat 3:4da392d2bae8 26 }
shovelcat 3:4da392d2bae8 27
shovelcat 3:4da392d2bae8 28 void SpeedLimiter::start()
shovelcat 3:4da392d2bae8 29 {
shovelcat 3:4da392d2bae8 30 _ticker->attach(callback(this, &SpeedLimiter::tickerCallback), TRANSFER_FUNCTION_PERIOD);
shovelcat 3:4da392d2bae8 31 }
shovelcat 3:4da392d2bae8 32
shovelcat 3:4da392d2bae8 33 void SpeedLimiter::tickerCallback()
shovelcat 3:4da392d2bae8 34 {
shovelcat 3:4da392d2bae8 35 ipControllerTransferFunction();
shovelcat 3:4da392d2bae8 36 }
shovelcat 3:4da392d2bae8 37
shovelcat 3:4da392d2bae8 38 void SpeedLimiter::ipControllerTransferFunction()
shovelcat 3:4da392d2bae8 39 {
shovelcat 3:4da392d2bae8 40 // write voltages at beginning of function to prevent jitter
shovelcat 3:4da392d2bae8 41 // voltage will be delayed by 1 call which is okay.
shovelcat 3:4da392d2bae8 42 writeAdcPedalHi(getOutputPedalVoltageHi());
shovelcat 3:4da392d2bae8 43 writeAdcPedalLo(getOutputPedalVoltageLo());
shovelcat 3:4da392d2bae8 44
shovelcat 3:4da392d2bae8 45 debug("hello world\n\r");
shovelcat 3:4da392d2bae8 46
shovelcat 3:4da392d2bae8 47 // calculate voltage for next call
shovelcat 3:4da392d2bae8 48 const float referenceSpeed = getReferenceSpeed();
shovelcat 3:4da392d2bae8 49 float outputAdcVoltageHi = 0;
shovelcat 3:4da392d2bae8 50 float outputAdcVoltageLo = 0;
shovelcat 3:4da392d2bae8 51
shovelcat 3:4da392d2bae8 52 if(referenceSpeed == DISABLE_ECO_ALGO_TRIGGER) {
shovelcat 3:4da392d2bae8 53 outputAdcVoltageHi = ecoDisabledAlgorithm();
shovelcat 3:4da392d2bae8 54 } else {
shovelcat 3:4da392d2bae8 55 outputAdcVoltageHi = ecoEnabledAlgorithm();
shovelcat 3:4da392d2bae8 56 }
shovelcat 3:4da392d2bae8 57
shovelcat 3:4da392d2bae8 58 outputAdcVoltageLo = outputAdcVoltageHi / 2;
shovelcat 3:4da392d2bae8 59
shovelcat 3:4da392d2bae8 60 setOutputPedalVoltageHi(outputAdcVoltageHi);
shovelcat 3:4da392d2bae8 61 setOutputPedalVoltageLo(outputAdcVoltageLo);
shovelcat 3:4da392d2bae8 62 }
shovelcat 3:4da392d2bae8 63
shovelcat 3:4da392d2bae8 64 // Returns voltage read on analog input port chosen for pedal input 1
shovelcat 3:4da392d2bae8 65 float SpeedLimiter::readAdcPedalHi()
shovelcat 3:4da392d2bae8 66 {
shovelcat 3:4da392d2bae8 67 const float decPcValue = _pedalInHi.read();
shovelcat 3:4da392d2bae8 68 const float voltage = decPcValue * ADC_INPUT_MAX_VALUE;
shovelcat 3:4da392d2bae8 69 return voltage;
shovelcat 3:4da392d2bae8 70 }
shovelcat 3:4da392d2bae8 71
shovelcat 3:4da392d2bae8 72 // Returns voltage read on analog input port chosen for pedal input 2
shovelcat 3:4da392d2bae8 73 float SpeedLimiter::readAdcPedalLo()
shovelcat 3:4da392d2bae8 74 {
shovelcat 3:4da392d2bae8 75 const float decPcValue = _pedalInLo.read();
shovelcat 3:4da392d2bae8 76 const float voltage = decPcValue * ADC_INPUT_MAX_VALUE;
shovelcat 3:4da392d2bae8 77 return voltage;
shovelcat 3:4da392d2bae8 78 }
shovelcat 2:06f128641b62 79
shovelcat 3:4da392d2bae8 80 // Accepts a value in volts, converts to % and sets ADC for pedal output 1
shovelcat 3:4da392d2bae8 81 void SpeedLimiter::writeAdcPedalHi(const float voltage)
shovelcat 3:4da392d2bae8 82 {
shovelcat 3:4da392d2bae8 83 const float boundedValue = boundValue(voltage, PEDAL_HI_MIN_VALUE, PEDAL_HI_MAX_VALUE);
shovelcat 3:4da392d2bae8 84 const float decValue = voltageToDecimal(boundedValue, ADC_OUTPUT_MAX_VALUE);
shovelcat 3:4da392d2bae8 85 _pedalOutHi.write(decValue);
shovelcat 3:4da392d2bae8 86 }
shovelcat 3:4da392d2bae8 87
shovelcat 3:4da392d2bae8 88 // Accepts a value in volts, converts to % and sets ADC for pedal output 2
shovelcat 3:4da392d2bae8 89 void SpeedLimiter::writeAdcPedalLo(const float voltage)
shovelcat 3:4da392d2bae8 90 {
shovelcat 3:4da392d2bae8 91 const float boundedValue = boundValue(voltage, PEDAL_LO_MIN_VALUE, PEDAL_LO_MAX_VALUE);
shovelcat 3:4da392d2bae8 92 const float decValue = voltageToDecimal(boundedValue, ADC_OUTPUT_MAX_VALUE);
shovelcat 3:4da392d2bae8 93 _pedalOutLo.write(decValue);
shovelcat 3:4da392d2bae8 94 }
shovelcat 3:4da392d2bae8 95
shovelcat 3:4da392d2bae8 96 float SpeedLimiter::ecoDisabledAlgorithm()
shovelcat 3:4da392d2bae8 97 {
shovelcat 3:4da392d2bae8 98 const float value = readAdcPedalHi();
shovelcat 3:4da392d2bae8 99 return value;
shovelcat 3:4da392d2bae8 100 }
shovelcat 3:4da392d2bae8 101
shovelcat 3:4da392d2bae8 102 float SpeedLimiter::ecoEnabledAlgorithm()
shovelcat 3:4da392d2bae8 103 {
shovelcat 3:4da392d2bae8 104 // valeurs anterieures
shovelcat 3:4da392d2bae8 105 static float x1 = 0.0;
shovelcat 3:4da392d2bae8 106 static float y1 = 0.0;
shovelcat 3:4da392d2bae8 107
shovelcat 3:4da392d2bae8 108 // constantes calcules avec matlab
shovelcat 3:4da392d2bae8 109 const float b0 = 10793.0;
shovelcat 3:4da392d2bae8 110 const float b1 = -8513.5;
shovelcat 3:4da392d2bae8 111 const float a1 = -1.0;
shovelcat 3:4da392d2bae8 112
shovelcat 3:4da392d2bae8 113 const float TRANS_k_gear = 20.5; //Gearbox ratio
shovelcat 3:4da392d2bae8 114 const float TRANS_eff = 0.91; // Efficiency of the transmission
shovelcat 3:4da392d2bae8 115 const float TRANS_R_wheel = 0.3175; // Radius of the wheel [m]
shovelcat 3:4da392d2bae8 116 const float TRANS_k = TRANS_k_gear/TRANS_R_wheel; // Global ratio of the transmission [m]
shovelcat 3:4da392d2bae8 117
shovelcat 3:4da392d2bae8 118 const float G_forceToTorque = (1/TRANS_k) * TRANS_eff;
shovelcat 3:4da392d2bae8 119
shovelcat 3:4da392d2bae8 120 // calculs
shovelcat 3:4da392d2bae8 121 const float x0 = getMeasuredSpeed();
shovelcat 3:4da392d2bae8 122 const float y0 = ( (-a1*y1) + (b0*x0) + (b1*x1) ) * G_forceToTorque;
shovelcat 3:4da392d2bae8 123
shovelcat 3:4da392d2bae8 124 // update des valeurs anterieurs
shovelcat 3:4da392d2bae8 125 x1 = x0;
shovelcat 3:4da392d2bae8 126 y1 = y0;
shovelcat 3:4da392d2bae8 127
shovelcat 3:4da392d2bae8 128 return y0;
shovelcat 3:4da392d2bae8 129 }
shovelcat 3:4da392d2bae8 130
shovelcat 3:4da392d2bae8 131 // Returns 'value' bounded between 'lowerBound' and 'upperBound'
shovelcat 3:4da392d2bae8 132 float SpeedLimiter::boundValue(float value, const float lowerBound, const float upperBound)
shovelcat 3:4da392d2bae8 133 {
shovelcat 3:4da392d2bae8 134 if(value < lowerBound) {
shovelcat 3:4da392d2bae8 135 value = lowerBound;
shovelcat 3:4da392d2bae8 136 } else if(value > upperBound) {
shovelcat 3:4da392d2bae8 137 value = upperBound;
shovelcat 3:4da392d2bae8 138 }
shovelcat 3:4da392d2bae8 139 return value;
shovelcat 3:4da392d2bae8 140 }
shovelcat 3:4da392d2bae8 141
shovelcat 3:4da392d2bae8 142 // Returns "value/reference" as a percentage in decimal form (0.5 for 50%)
shovelcat 3:4da392d2bae8 143 float SpeedLimiter::voltageToDecimal(const float voltage, const float reference)
shovelcat 3:4da392d2bae8 144 {
shovelcat 3:4da392d2bae8 145 return voltage/reference;
shovelcat 3:4da392d2bae8 146 }