Diff: speedlimiter.cpp

Revision:

3:4da392d2bae8

Parent:

2:06f128641b62

Child:

4:a8c9f6a13633

--- 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;
+}