File content as of revision 14:70fc660d4386:

/*
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)
{
}

SpeedLimiter::~SpeedLimiter()
{
}

void SpeedLimiter::controllerCallbackFunction()
{    
    float nextReferenceSpeed = getReferenceSpeed(true);
    setReferenceSpeed(nextReferenceSpeed);

    
    // write voltages at beginning of function to prevent jitter
    // voltage will be delayed by 1 call which is okay.
    const float voutHi = getOutputPedalVoltageHi();
    const float voutLo = getOutputPedalVoltageLo();

    writeAdcPedalHi(voutHi);
    writeAdcPedalLo(voutLo);

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

//    pc->printf("Reference speed: %f\n\r", nextReferenceSpeed);
//    pc->printf("tmpHi: %f\t tmpLo: %f\n\r", outputAdcVoltageHi, outputAdcVoltageLo);
//    pc->printf("Hi: %f\t Lo: %f\n\r", voutHi, voutLo);

    setOutputPedalVoltageHi(outputAdcVoltageHi);
    setOutputPedalVoltageLo(outputAdcVoltageLo);
}

float SpeedLimiter::ecoDisabledAlgorithm()
{
    const float value = readAdcPedalHi();
    return value;
}

float SpeedLimiter::ecoEnabledAlgorithm()
{
    static bool first_acquisition = true;
    static time_t referenceTime = time(NULL); // seconds since January 1, 1970, a.k.a. since reset (time not set)

    // calculs
    const double Vm = getMeasuredSpeed();
    const double Vd = getReferenceSpeed();

    double out = 0.0;
    if (readAdcPedalHi() > 1) {
        
        out = piControlFunction(Vd, Vm);
//        out = ipControlFunction(Vd, Vm);

        // force to voltage conversion
        out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;

        if(first_acquisition) {
            pc->printf("Acquisition start:\n\r");
            referenceTime = time(NULL);
            first_acquisition = false;
        }
    } else {
        const bool reset = true;
        piControlFunction(0,reset);
//        ipControlFunction(0,reset);

        first_acquisition = true;
    }
    
    time_t elapsedTime = time(NULL) - referenceTime;
    pc->printf("%d: Vd: %.2f\tVm: %.2f\t\n\r", elapsedTime, Vd, Vm);

//    char timestamp[32];
//    strftime(timestamp, 32, "%I:%M %p\n", localtime(&seconds));
//    pc->printf("%s: Vd: %.2f\tVm: %.2f\t\n\r", timestamp, Vd, Vm);

    return (float)out;
}

double SpeedLimiter::piControlFunction(const double reference, const double measured, bool reset) {
    // valeurs anterieures
    static double ie = 0.0;
    if(reset) {
        ie = 0.000001;
    }
    
    const double var = readAdcTest();
    
    // constants calibrated in vehicle
    const double Kp =  20389 * var ;
    const double Ki = 60188.0;
        
    pc->printf("Current Kp value: %f\n\r\n\r", Kp);
        
    // calculations
    double error = reference - measured;
    const double eMax = 4;
    if(error > eMax) error = eMax; // upper bound the error
    const double dt = TRANSFER_FUNCTION_PERIOD;
    ie = ie + error*dt;
    
    double out = Kp*error + Ki*ie;
    return out;
}

double SpeedLimiter::ipControlFunction(const double reference, const double measured, bool reset) {
    // valeurs anterieures
    static double ie = 0.0;
    if(reset) {
        ie = 0.000001;
    }
    
    // constants calibrated in vehicle
    const double Kp =  18000 ;
    const double Ki = 4300.0;
        
    // calculations
    double errorI = reference - measured;

    const double dt = TRANSFER_FUNCTION_PERIOD;
    ie = ie + errorI*dt;
    
    double out = Ki*ie;

    double errorP = out - measured;
    out = Kp*errorP; 

    return out;
}

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

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