File content as of revision 6:1a4ed88c9a4b:

/*
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::ipControllerTransferFunction()
{
    // write voltages at beginning of function to prevent jitter
    // voltage will be delayed by 1 call which is okay.
//    pc->printf("H\n\r");
    const float voutHi = getOutputPedalVoltageHi();
    const float voutLo = getOutputPedalVoltageLo();

//    pc->printf("Hi: %f\t Lo: %f\n\r", voutHi, voutLo);

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

//    outputAdcVoltageHi = ADC_OUTPUT_MAX_VALUE / 2;
    outputAdcVoltageLo = outputAdcVoltageHi / 2;

//    pc->printf("tmpHi: %f\t tmpLo: %f\n\r", outputAdcVoltageHi, outputAdcVoltageLo);

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

//// Proportionnel 3s
//float SpeedLimiter::ecoEnabledAlgorithm()
//{
//    static bool first_acquisition = true;
//
//    // constantes calcules dans MATLAB
//    double Kp =  1.6783e+03; //3.1848e+03
//
//    // valeurs anterieures
//    static double ie = 0.0;
//
//    // calculs
//    double Vm = getMeasuredSpeed();
//    double Vd = getReferenceSpeed();
//    double out = 0.0;
//    if (readAdcPedalHi() > 1) {
//        double e = Vd - Vm;
//        const double dt = TRANSFER_FUNCTION_PERIOD;
//        ie = ie + e*dt;
//        
//        out = Kp*e;
//
//        // out est maintenant en 'force'
//        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");
//            first_acquisition = false;
//        }
//    } else {
//        ie = 0.00000000001;
//        first_acquisition = true;
//    }
//
//    pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
//
//    return (float)out;
//}

// PI 3s (meilleur a date)
float SpeedLimiter::ecoEnabledAlgorithm()
{
    static bool first_acquisition = true;
    
    // constantes calcules dans MATLAB
    double var = readAdcTest();
    double Kp =  18000 ; //3.1848e+03
    double Ki = 4300.0; // 2.5181e+03

    // valeurs anterieures
    static double ie = 0.0;

    // calculs
    double Vm = getMeasuredSpeed();
    double Vd = 40 * var;
    double out = 0.0;
    if (readAdcPedalHi() > 1) {
        double e = Vd - Vm;
        const double dt = TRANSFER_FUNCTION_PERIOD;
        ie = ie + e*dt;
        
        out = Kp*e + Ki*ie;

        // out est maintenant en 'force'
        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");
            first_acquisition = false;
        }
    } else {
        ie = 0.00000000001;
        first_acquisition = true;
    }

    pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t Kp: %.2f\tVar: %.2f\n\r", Vm, Vd, out, Ki, var);

    return (float)out;
}

//// essai routier: lim = 10 kmh, overshoot de 50%, un seul ronflement
//// max 15 kmh, min 7 kmh.
//// temps de stabilisation 3s
//float SpeedLimiter::ecoEnabledAlgorithm()
//{
//    static bool first_acquisition = true;
//
//    // constantes calcules dans MATLAB
//    const double b0 = 3312.0;
//    const double b1 = -3061.0;
//    const double a1 = -1.0;
//
//    // valeurs anterieures
//    static double lastE = 0.0;
//    static double lastOut = 0.0;
//
//    // calculs
//    double Vm = getMeasuredSpeed();
//    double Vd = getReferenceSpeed();
//    double out = 0.0;
//    if (readAdcPedalHi() > 1) {
//        const double e = Vd - Vm;
//
//        // y(n) = b0 * x(n) + b1 * x(n-1) - a1 * y(n-1)
//        out = (b0 * e) + (b1 * lastE) - (a1 * lastOut);
//
//        lastE = e;
//        lastOut = out;
//
//        // out est maintenant en 'force'
//        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");
//            first_acquisition = false;
//        }
//    } else {
//        lastE = 0.00000000001;
//        lastOut = 0.00000000001;
//        first_acquisition = true;
//    }
//
//    pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
//
//    return (float)out;
//}

//// temps de stabilisation 1s
//// oscillation, grand overshoot (20 kmh), consigne 10 kmh
//float SpeedLimiter::ecoEnabledAlgorithm()
//{
//    static bool first_acquisition = true;
//
//    // constantes calcules dans MATLAB
//    const double b0 = 10790.0;
//    const double b1 = -8514.0;
//    const double a1 = -1.0;
//
//    // valeurs anterieures
//    static double lastE = 0.0;
//    static double lastOut = 0.0;
//
//    // calculs
//    double Vm = getMeasuredSpeed();
//    double Vd = getReferenceSpeed();
//    double out = 0.0;
//    if (readAdcPedalHi() > 1) {
//        const double e = Vd - Vm;
//
//        // y(n) = b0 * x(n) + b1 * x(n-1) - a1 * y(n-1)
//        out = (b0 * e) + (b1 * lastE) - (a1 * lastOut);
//
//        lastE = e;
//        lastOut = out;
//
//        // out est maintenant en 'force'
//        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");
//            first_acquisition = false;
//        }
//    } else {
//        lastE = 0.00000000001;
//        lastOut = 0.00000000001;
//        first_acquisition = true;
//    }
//
//    pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
//
//    return (float)out;
//}

//// implementation du PID 2s de stab
//// overshoot significatif, ronflement constant
//float SpeedLimiter::ecoEnabledAlgorithm()
//{
//    static bool first_acquisition = true;
//
//    // constantes calcules dans MATLAB
//    const double b0 = 4.246e04;
//    const double b1 = -7.958e04;
//    const double b2 =  3.768e04;
//    const double a1 = -1.0;
//
//    // valeurs anterieures
//    static double lastE = 0.0;
//    static double lastlastE = 0.0;
//    static double lastOut = 0.0;
//    static double lastlastOut = 0.0;
//
//    // calculs
//    double Vm = getMeasuredSpeed();
//    double Vd = getReferenceSpeed();
//    double out = 0.0;
//    if (readAdcPedalHi() > 1) {
//        const double e = Vd - Vm;
//
//        // y(n) = b0 * x(n) + b1 * x(n-1) - a1 * y(n-1)
//        out = (b0 * e) + (b1 * lastE) + (b2 * lastlastE) - (a1 * lastOut);
//
//        lastlastE = lastE;
//        lastE = e;
//        lastlastOut = lastOut;
//        lastOut = out;
//
//        // out est maintenant en 'force'
//        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");
//            first_acquisition = false;
//        }
//    } else {
//        lastE = 0.00000000001;
//        lastOut = 0.00000000001;
//        first_acquisition = true;
//    }
//
//    pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
//
//    return (float)out;
//}

//// overshoot ~17, oscillation / ronflement
//float SpeedLimiter::ecoEnabledAlgorithm()
//{
//    static bool first_acquisition = true;
//
//    // constantes calcules dans MATLAB
//    double Kp =  4.7772e+03;
//    double Ki = 5.6658e+03;
//    double Kd = 4000.0;
//
//    // valeurs anterieures
//    static double ie = 0.0;
//    static double lastE = 0.0;
//
//    // calculs
//    double Vm = getMeasuredSpeed();
//    double Vd = getReferenceSpeed();
//    double out = 0.0;
//    if (readAdcPedalHi() > 1) {
//        double e = Vd - Vm;
//        const double dt = TRANSFER_FUNCTION_PERIOD;
//        ie = ie + e*dt;
//        double de = (e - lastE)/dt;
//        
//        out = Kp*e + Ki*ie + Kd*de;
//        
//        lastE = e;
//
//        // out est maintenant en 'force'
//        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");
//            first_acquisition = false;
//        }
//    } else {
//        lastE = 0.00000000001;
//        ie = 0.00000000001;
//        first_acquisition = true;
//    }
//
//    pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
//
//    return (float)out;
//}


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