File content as of revision 3:4da392d2bae8:

/*
author: Sebastian Pelchat
date: october 2018
*/
#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;
}

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