Hesu-eco
/
limiteur_vitesse
premiere ebauche
Diff: speedlimiter.cpp
- Revision:
- 3:4da392d2bae8
- Parent:
- 2:06f128641b62
- Child:
- 4:a8c9f6a13633
diff -r 06f128641b62 -r 4da392d2bae8 speedlimiter.cpp --- a/speedlimiter.cpp Thu Oct 18 02:20:14 2018 +0000 +++ b/speedlimiter.cpp Thu Oct 18 17:31:03 2018 +0000 @@ -4,11 +4,143 @@ */ #include "speedlimiter.hpp" +#define debug SpeedLimiter::pc->printf +Serial* SpeedLimiter::pc = new Serial(USBTX, USBRX); + SpeedLimiter::SpeedLimiter(const PinName& pedalInHi, const PinName& pedalInLo, const PinName& pedalOutHi, const PinName& pedalOutLo) : _pedalInHi(pedalInHi) , _pedalInLo(pedalInLo) , _pedalOutHi(pedalOutHi) , _pedalOutLo(pedalOutLo) + , _referenceSpeed(DISABLE_ECO_ALGO_TRIGGER) + , _measuredSpeed(0.0) + , _ticker(new Ticker) + , _mutex(new Mutex) { +} + +SpeedLimiter::~SpeedLimiter() +{ + delete _ticker; + delete _mutex; +} + +void SpeedLimiter::start() +{ + _ticker->attach(callback(this, &SpeedLimiter::tickerCallback), TRANSFER_FUNCTION_PERIOD); +} + +void SpeedLimiter::tickerCallback() +{ + ipControllerTransferFunction(); +} + +void SpeedLimiter::ipControllerTransferFunction() +{ + // write voltages at beginning of function to prevent jitter + // voltage will be delayed by 1 call which is okay. + writeAdcPedalHi(getOutputPedalVoltageHi()); + writeAdcPedalLo(getOutputPedalVoltageLo()); + + debug("hello world\n\r"); + + // calculate voltage for next call + const float referenceSpeed = getReferenceSpeed(); + float outputAdcVoltageHi = 0; + float outputAdcVoltageLo = 0; + + if(referenceSpeed == DISABLE_ECO_ALGO_TRIGGER) { + outputAdcVoltageHi = ecoDisabledAlgorithm(); + } else { + outputAdcVoltageHi = ecoEnabledAlgorithm(); + } + + outputAdcVoltageLo = outputAdcVoltageHi / 2; + + setOutputPedalVoltageHi(outputAdcVoltageHi); + setOutputPedalVoltageLo(outputAdcVoltageLo); +} + +// Returns voltage read on analog input port chosen for pedal input 1 +float SpeedLimiter::readAdcPedalHi() +{ + const float decPcValue = _pedalInHi.read(); + const float voltage = decPcValue * ADC_INPUT_MAX_VALUE; + return voltage; +} + +// Returns voltage read on analog input port chosen for pedal input 2 +float SpeedLimiter::readAdcPedalLo() +{ + const float decPcValue = _pedalInLo.read(); + const float voltage = decPcValue * ADC_INPUT_MAX_VALUE; + return voltage; +} -} \ No newline at end of file +// Accepts a value in volts, converts to % and sets ADC for pedal output 1 +void SpeedLimiter::writeAdcPedalHi(const float voltage) +{ + const float boundedValue = boundValue(voltage, PEDAL_HI_MIN_VALUE, PEDAL_HI_MAX_VALUE); + const float decValue = voltageToDecimal(boundedValue, ADC_OUTPUT_MAX_VALUE); + _pedalOutHi.write(decValue); +} + +// Accepts a value in volts, converts to % and sets ADC for pedal output 2 +void SpeedLimiter::writeAdcPedalLo(const float voltage) +{ + const float boundedValue = boundValue(voltage, PEDAL_LO_MIN_VALUE, PEDAL_LO_MAX_VALUE); + const float decValue = voltageToDecimal(boundedValue, ADC_OUTPUT_MAX_VALUE); + _pedalOutLo.write(decValue); +} + +float SpeedLimiter::ecoDisabledAlgorithm() +{ + const float value = readAdcPedalHi(); + return value; +} + +float SpeedLimiter::ecoEnabledAlgorithm() +{ + // valeurs anterieures + static float x1 = 0.0; + static float y1 = 0.0; + + // constantes calcules avec matlab + const float b0 = 10793.0; + const float b1 = -8513.5; + const float a1 = -1.0; + + const float TRANS_k_gear = 20.5; //Gearbox ratio + const float TRANS_eff = 0.91; // Efficiency of the transmission + const float TRANS_R_wheel = 0.3175; // Radius of the wheel [m] + const float TRANS_k = TRANS_k_gear/TRANS_R_wheel; // Global ratio of the transmission [m] + + const float G_forceToTorque = (1/TRANS_k) * TRANS_eff; + + // calculs + const float x0 = getMeasuredSpeed(); + const float y0 = ( (-a1*y1) + (b0*x0) + (b1*x1) ) * G_forceToTorque; + + // update des valeurs anterieurs + x1 = x0; + y1 = y0; + + return y0; +} + +// Returns 'value' bounded between 'lowerBound' and 'upperBound' +float SpeedLimiter::boundValue(float value, const float lowerBound, const float upperBound) +{ + if(value < lowerBound) { + value = lowerBound; + } else if(value > upperBound) { + value = upperBound; + } + return value; +} + +// Returns "value/reference" as a percentage in decimal form (0.5 for 50%) +float SpeedLimiter::voltageToDecimal(const float voltage, const float reference) +{ + return voltage/reference; +}