Hesu-eco / Mbed 2 deprecated limiteur_vitesse

premiere ebauche

Dependencies:   mbed PinDetect

speedlimiter.cpp@4:a8c9f6a13633, 2018-10-23 (annotated)

Committer:
shovelcat
Date:
Tue Oct 23 14:19:54 2018 +0000
Revision:
4:a8c9f6a13633
Parent:
3:4da392d2bae8
Child:
5:aef1fc6c0df1
updated

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 3:4da392d2bae8 98 // valeurs anterieures
shovelcat 4:a8c9f6a13633 99 static double x1 = 0.0;
shovelcat 4:a8c9f6a13633 100 static double y1 = 0.0;
shovelcat 3:4da392d2bae8 101
shovelcat 3:4da392d2bae8 102 // constantes calcules avec matlab
shovelcat 4:a8c9f6a13633 103 const double b0 = 10793.0;
shovelcat 4:a8c9f6a13633 104 const double b1 = -8513.5;
shovelcat 4:a8c9f6a13633 105 const double a1 = -1.0;
shovelcat 3:4da392d2bae8 106
shovelcat 4:a8c9f6a13633 107 const double TRANS_k_gear = 20.5; //Gearbox ratio
shovelcat 4:a8c9f6a13633 108 const double TRANS_eff = 0.91; // Efficiency of the transmission
shovelcat 4:a8c9f6a13633 109 const double TRANS_R_wheel = 0.3175; // Radius of the wheel [m]
shovelcat 4:a8c9f6a13633 110 const double TRANS_k = TRANS_k_gear/TRANS_R_wheel; // Global ratio of the transmission [m]
shovelcat 3:4da392d2bae8 111
shovelcat 4:a8c9f6a13633 112 const double G_forceToTorque = (1/TRANS_k) * TRANS_eff;
shovelcat 3:4da392d2bae8 113
shovelcat 3:4da392d2bae8 114 // calculs
shovelcat 4:a8c9f6a13633 115 double y0 = 0.0;
shovelcat 4:a8c9f6a13633 116 if (readAdcPedalHi() > 1) {
shovelcat 4:a8c9f6a13633 117
shovelcat 4:a8c9f6a13633 118 const float Vm = getMeasuredSpeed();
shovelcat 4:a8c9f6a13633 119 const float Vd = getReferenceSpeed();
shovelcat 4:a8c9f6a13633 120 const double x0 = Vd - Vm;
shovelcat 4:a8c9f6a13633 121 y0 = ( (-a1*y1) + (b0*x0) + (b1*x1) ) * G_forceToTorque;
shovelcat 3:4da392d2bae8 122
shovelcat 4:a8c9f6a13633 123 // update des valeurs anterieurs
shovelcat 4:a8c9f6a13633 124 x1 = x0;
shovelcat 4:a8c9f6a13633 125 y1 = y0;
shovelcat 4:a8c9f6a13633 126 }
shovelcat 3:4da392d2bae8 127
shovelcat 4:a8c9f6a13633 128 pc->printf("Vm: %f\t Vd: %f\t Eh: %f\t El\n\r", Vm, Vd, y0, y0/2);
shovelcat 4:a8c9f6a13633 129
shovelcat 4:a8c9f6a13633 130 return (float)y0;
shovelcat 3:4da392d2bae8 131 }
shovelcat 3:4da392d2bae8 132
shovelcat 3:4da392d2bae8 133 // Returns 'value' bounded between 'lowerBound' and 'upperBound'
shovelcat 3:4da392d2bae8 134 float SpeedLimiter::boundValue(float value, const float lowerBound, const float upperBound)
shovelcat 3:4da392d2bae8 135 {
shovelcat 3:4da392d2bae8 136 if(value < lowerBound) {
shovelcat 3:4da392d2bae8 137 value = lowerBound;
shovelcat 3:4da392d2bae8 138 } else if(value > upperBound) {
shovelcat 3:4da392d2bae8 139 value = upperBound;
shovelcat 3:4da392d2bae8 140 }
shovelcat 3:4da392d2bae8 141 return value;
shovelcat 3:4da392d2bae8 142 }
shovelcat 3:4da392d2bae8 143
shovelcat 3:4da392d2bae8 144 // Returns "value/reference" as a percentage in decimal form (0.5 for 50%)
shovelcat 3:4da392d2bae8 145 float SpeedLimiter::voltageToDecimal(const float voltage, const float reference)
shovelcat 3:4da392d2bae8 146 {
shovelcat 3:4da392d2bae8 147 return voltage/reference;
shovelcat 3:4da392d2bae8 148 }