File content as of revision 1:c6b4ccebd483:

#include "mbed.h"

/* 
##############################
uC I/O
##############################
*/ 
AnalogIn        pedal_in_hi(PA_3);
AnalogIn        pedal_in_lo(PC_0);
AnalogOut       pedal_out_hi(PA_4);
AnalogOut       pedal_out_lo(PA_5);

// test i/o
InterruptIn     b1(PC_13);
DigitalOut      led(LED1);
DigitalOut      canLed(LED2);
Serial          pc(USBTX, USBRX); // for communication / debugging
CAN             can1(PD_0, PD_1);

/* 
##############################
Constants
##############################
*/ 
const float ADC_INPUT_MAX_VALUE = 5;
const float ADC_OUTPUT_MAX_VALUE = 5;

const float PEDAL_HI_MIN_VALUE = 0.48;
const float PEDAL_HI_MAX_VALUE = 2.96;
const float PEDAL_LO_MIN_VALUE = 0.25;
const float PEDAL_LO_MAX_VALUE = 1.48;

const float SPEED_MAX = 40.0;

/* 
##############################
Static variables and functions
##############################
*/ 
static float __reference_speed = 0;
static float __measured_speed = 0;

static void setReferenceSpeed(const float speed) {
    // TODO insert mutex here
    __reference_speed = speed;
}

static void setMeasuredSpeed(const float speed) {
    // TODO insert mutex here
    __measured_speed = speed;
}

static float getReferenceSpeed() {
    // TODO insert mutex here
    return __reference_speed;
}
static float getMeasuredSpeed() {
    // TODO insert mutex here
    return __measured_speed;
}

/* 
##############################
Utility functions
##############################
*/ 

// Returns 'value' bounded between 'lowerBound' and 'upperBound'
float 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 toDecimalPercent(const float value, const float reference) {
    return value/reference;
}

// Returns voltage read on analog input port chosen for pedal input 1
float readAdcPedalHi() {
    const float decPcValue = pedal_in_hi.read();
    const float voltage = decPcValue * ADC_INPUT_MAX_VALUE;
    return voltage;
}

// Returns voltage read on analog input port chosen for pedal input 2
float readAdcPedalLo() {
    const float decPcValue = pedal_in_lo.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 writeAdcPedalHi(const float voltage) {
    const float boundedValue = boundValue(voltage, PEDAL_HI_MIN_VALUE, PEDAL_HI_MAX_VALUE);
    const float decPcValue = toDecimalPercent(boundedValue, ADC_OUTPUT_MAX_VALUE);
    pedal_out_hi.write(decPcValue);
}

// Accepts a value in volts, converts to % and sets ADC for pedal output 2
void writeAdcPedalLo(const float voltage) {
    const float boundedValue = boundValue(voltage, PEDAL_LO_MIN_VALUE, PEDAL_LO_MAX_VALUE);
    const float decPcValue = toDecimalPercent(boundedValue, ADC_OUTPUT_MAX_VALUE);
    pedal_out_lo.write(decPcValue);
}

// main transfer function
void limvitCallback() {
//    const float T_gb_r = (0.3175*2/2.0)/20.5;
//    
//    // constantes du controleur IP (Ziegler-Nichols method)
//    const float Ki = 0;
//    const float Kp = 1.0;
//    
//    // facteur de conversion
////    const float pedal_HI_range = PEDAL_HI_MAX_VALUE - PEDAL_HI_MIN_VALUE;
//    
//    const float speedVoltageConversionFactor = PEDAL_HI_MAX_VALUE / SPEED_MAX;
//    const float Kc = T_gb_r * speedVoltageConversionFactor;
//      
//    // algorithme 
//    const float Vref = getReferenceSpeed();
//    const float Vmes = getMeasuredSpeed();
//    const float err = Vref - Vmes;
//    const float errKi = err * Ki;
//    const float err2 = errKi - Vmes;
//    const float udHi = err2 * Kp * Kc;
//    const float udLo = (float)udHi / 2;
//
////    const float umHi = readAdcPedalHi();
////    const float umLo = readAdcPedalLo();
//    writeAdcPedalHi(udHi);
////    writeAdcPedalLo(udLo);
//    writeAdcPedalLo(Vref * speedVoltageConversionFactor);
//    
//    
//    // test
//    setMeasuredSpeed(udHi / speedVoltageConversionFactor);
//    

    const float tmpHi = readAdcPedalHi();
    const float tmpLo = readAdcPedalLo();
//    pc.printf("Callback\n\r");
//    pc.printf("Hi: %f\n\r", tmpHi);
//    pc.printf("Lo: %f\n\r", tmpLo);
    
    // fonction suiveur
    writeAdcPedalHi(tmpHi);
    writeAdcPedalLo(tmpLo);

}

// Main loop to be run in thread
void limvitMain() {
//    const float period = 0.001;
    const float period = 0.5;
    Ticker limvitSync;
    limvitSync.attach(&limvitCallback, period);
    
//    while(1) {
//        led = !led;
//        wait(1.0);        
//    }
}

void canCallback() {
    CANMessage msg;
    canLed = true;
    wait(1.0);
    canLed = false;
    int status = can1.read(msg);
    if(status) {
        pc.printf("CAN msg received: %x\n\r", msg.data);
    }
    else {
        pc.printf("Error: %d\n\r", status);
    }
}

void b1Callback() {
    static bool state = false;
    pc.printf("User pressed b1.\n\r");
    if (state) {
        // reset speed
        setReferenceSpeed(0.0);
        state = !state;
    }
    else {
        setReferenceSpeed(30.0);
        state = !state;
    }    
    pc.printf("%s\n\r", can1.rderror());
    canCallback();
}

int main() {
    pc.printf("\n\rAnalogIn example\n\r");
    setReferenceSpeed(0);
    setMeasuredSpeed(0);
    b1.rise(&b1Callback);
    
    // CAN test
    can1.frequency(500000);
    can1.attach(&canCallback, CAN::RxIrq);
    
    limvitMain();
    while(1) {
        wait(1.0);
    }
}