File content as of revision 0:2dbd366be5fd:

/******************************************************************************
***********************           Protodrive            ***********************
*************           Andrew Botelho & William Price            *************
******************************************************************************/

#include "mbed.h"
#include "SerialRPCInterface.h"
#include "RPCVariable.h"

/***********************************************************/
/*Constants                                                */
/***********************************************************/
#define MATLAB_INTERFACE //matlab used as an interface
//#define SERIAL_INTERFACE //Terminal used as an interface. Mainly for debugging

#define CURRENT_SENSOR_ON //turn on current sensor
//#define DEBUG
//#define LIGHTS_CURRENT_CONTROL //power flow light control is determined by the current sensor
#define LIGHTS_LOAD_CONTROL //power flow light control is determined by the voltage demand placed on the motors

/**********************Power sources options***************/
/*Instruction: There are 3 power source options for the DUT
and two for the load. Uncomment the option that you want to
use and comment options that you do not want to use. The DUT
and load should each only have one option uncommented.
*/

//DUT
//1. External supply
//#define DUT_EXTERNAL_POWER_ENABLE

//2. Just DUT batt enabled
#define DUT_BATT_ENABLE

//3. DUT batt and cap enabled
//#define DUT_BATT_CAP_ENABLE

//Load
//1. External Supply
//#define LOAD_EXTERNAL_POWER_ENABLE

//2. Load batt enabled
#define LOAD_BATT_ENABLE

/***********************************************************/
/*Global Variables                                         */
/***********************************************************/
float v_DUT_batt, v_load_batt, v_cap,  speed, current;
int big_error,regen;
int foo = 1;

//motor direction and load
float DUT_input = 0; //DUT duty cycle demand variable
float load_input = 0; //Load duty cycle demand variable
float DUT_input_direc = 1; //DUT direction variable. Default to forward
float load_input_direc = 0; //Load direction variable. Default to reverse. This means it will NOT oppose the DUT

/***********************************************************/
/*Pin Setup                                                */
/***********************************************************/
//mbed LEDS
DigitalOut big_error_led(LED1); //error LED

//Board LEDs
DigitalOut leftArrow(p29); //setting high turns on green left arrow LEDS
DigitalOut greenLine(p28); //setting high turns on green centerline LEDS
DigitalOut orangeLine(p27); //setting high turns on orange centerline LEDS
DigitalOut rightArrow(p26); //setting high turns on orange right arrow LEDS

DigitalOut load_batt_green(p25); //setting high will turn on the load batt green LED
DigitalOut load_batt_orange(p24); //setting high will turn on the load batt orange LED
DigitalOut DUT_batt_green(p10); //setting high will turn on the DUT batt green LED
DigitalOut DUT_batt_orange(p11); //setting high will turn on the DUT batt orange LED

DigitalOut cap_green(p12); //setting high will turn on the cap green LED
DigitalOut cap_orange(p13); //setting high will turn on the cap orange LED

#ifdef CURRENT_SENSOR_ON
AnalogIn current_sense(p15); //current sensor on p15
#else
DigitalOut place_holder15(p15); //sets p15 as a DigitalOut that will be set low, to reduce noise on analog in pins
#endif

//sets p15 and p18 as a DigitalOut that will be set low, to reduce noise on analog in pins
DigitalOut place_holder16(p16);
DigitalOut place_holder18(p18);

#ifdef LOAD_BATT_ENABLE
AnalogIn v_load_batt_measure(p17); //Analog in from load battery
#endif

#ifdef DUT_BATT_ENABLE
AnalogIn v_DUT_batt_measure(p20); //Analog in from drive battery
DigitalOut place_holder19(p19); //sets p19 as a DigitalOut that will be set low, to reduce noise on analog in pins
#endif

#ifdef DUT_BATT_CAP_ENABLE
AnalogIn v_DUT_batt_measure(p20); //Analog in from drive battery
AnalogIn v_cap_measure(p19); //Analog in from super cap
#endif

DigitalIn kill_switch(p9); //switch to disable running program

//matlab interface
#ifdef MATLAB_INTERFACE
//tie program variables to RPC variables used in Matlab
RPCVariable<float> RPC_DUT_input(&DUT_input, "DUT_input");
RPCVariable<float> RPC_load_input(&load_input, "load_input");
RPCVariable<float> RPC_DUT_input_direc(&DUT_input_direc, "DUT_input_direc");
RPCVariable<float> RPC_load_input_direc(&load_input_direc,"load_input_direc");
RPCVariable<float> RPC_current(&current, "current");
RPCVariable<float> RPC_speed(&speed, "speed");
RPCVariable<float> RPC_v_DUT_batt(&v_DUT_batt, "v_DUT_batt");
RPCVariable<float> RPC_v_load_batt(&v_DUT_batt, "v_load_batt");
RPCVariable<float> RPC_v_cap(&v_DUT_batt, "v_cap");
#endif

/***********************************************************/
/*Terminals                                                */
/***********************************************************/

#ifdef MATLAB_INTERFACE
SerialRPCInterface SerialInterface(USBTX, USBRX);//establich RPC serial connection
#endif

#ifdef SERIAL_INTERFACE
Serial pc(USBTX, USBRX); //Establish serial
#endif

/***********************************************************/
/*Other Includes                                           */
/***********************************************************/

#include "power_sources.h"
#include "measurement.h"
#include "motor_control.h"

/***********************************************************/
/*Subroutines                                              */
/***********************************************************/

//LED control
void all_LEDS_flash() { //a demo program that just switches LEDs between green and orange
    if (foo == 0) {
        leftArrow = 0;
        greenLine = 0;
        orangeLine = 1;
        rightArrow = 1;
        load_batt_green = 0;
        load_batt_orange = 1;
        DUT_batt_green = 0;
        DUT_batt_orange = 1;
        cap_green = 0;
        cap_orange = 1;
        foo = 1;
    } else {
        leftArrow = 1;
        greenLine = 1;
        orangeLine = 0;
        rightArrow = 0;
        load_batt_green = 1;
        load_batt_orange = 0;
        DUT_batt_green = 1;
        DUT_batt_orange = 0;
        cap_green = 1;
        cap_orange = 0;
        foo = 0;
    }
}

//turn all LEDs off
void all_LEDS_off() {
    load_batt_green = 1;
    load_batt_orange = 1;
    leftArrow = 1;
    greenLine = 1;
    orangeLine = 1;
    rightArrow = 1;
    DUT_batt_green = 1;
    DUT_batt_orange = 1;
    cap_green = 1;
    cap_orange = 1;
}

//turns on the green LEDs indicating regen
void regen_LEDS() {
    load_batt_green = 1;
    load_batt_orange = 0;
    leftArrow = 0;
    greenLine = 0;
    orangeLine = 1;
    rightArrow = 1;
    if (!relay) {
        DUT_batt_green = 0;
        DUT_batt_orange = 1;
        cap_green = 1;
        cap_orange = 1;
    } else {
        DUT_batt_green = 1;
        DUT_batt_orange = 1;
        cap_green = 0;
        cap_orange = 1;
    }
}

//turns on the orange LEDs indicating drive mode (power flowing out of the vehicle)
void drive_LEDS() {
    load_batt_green = 0;
    load_batt_orange = 1;
    leftArrow = 1;
    greenLine = 1;
    orangeLine = 0;
    rightArrow = 0;
    if (!relay) {
        DUT_batt_green = 1;
        DUT_batt_orange = 0;
        cap_green = 1;
        cap_orange = 1;
    } else {
        DUT_batt_green = 1;
        DUT_batt_orange = 1;
        cap_green = 1;
        cap_orange = 0;
    }
}
/***********************************************************/
/*Main program setup                                       */
/***********************************************************/

int main() {

    Timer timer; //create a timer for the main loop
    init_pwm(); //initialize PWM

    /***********************************************************/
    /*Main Loop                                                */
    /***********************************************************/
    while (1) {
        while (kill_switch ==0) {//run demo while waiting for kill switch to be turned on
            all_LEDS_flash(); //switch LEDs between green and orange
            wait(.5); //wait 0.5s
        }

        // System initializations
        DUT_direction.write(1); //set forward
        load_direction.write(0); //set reverse. This ensures both motors will apply a torque on the coupler in the same direction

        //initialize system variables to 0
        speed = 0;
        current = 0;
        big_error = 0;
        big_error_led = 0;
        relay = 0; //turn on battery by default

        //main loop timer variables
        int slow_timer = 0;
        int fast_timer = 0;

        timer.start(); //start loop timer
        all_LEDS_off(); //turn off LEDs
        wait(.5); //wait 0.5s before starting

#ifdef DEBUG //debugging code
        DUT_input = 0.75;
        load_input = 0.25;
#endif

#ifdef DUT_BATT_ENABLE
        place_holder19 = 0; //set DigitalOut pin low to reduce noise on adjacent AnalogIn pins
#endif

        //set unused Analog pins to ground
#ifndef CURRENT_SENSOR_ON
        place_holder15 = 0; //set DigitalOut pin low to reduce noise on adjacent AnalogIn pins
#endif
        place_holder16 = 0; //set DigitalOut pin low to reduce noise on adjacent AnalogIn pins
        place_holder18 = 0; //set DigitalOut pin low to reduce noise on adjacent AnalogIn pins

        while (kill_switch == 1 && big_error == 0) {//while the kill switch is on and there are no errors

            // --- SLOW LOOP (1) Hz ---
            if (timer.read_ms() - slow_timer >= 1000) {
                slow_timer = timer.read_ms(); //read timer value in miliseconds
                big_error = checkBattVoltages(); //check batt voltages to ensure they are in the correct voltage range
                if (big_error == 1) {
                    big_error_led = 1;
                }
                //measure voltages
#ifdef LOAD_BATT_ENABLE
                v_load_batt = get_voltage(v_load_batt_measure); //measure load batt voltage
#ifdef SERIAL_INTERFACE
                pc.printf("Analog In (p17): %f ", v_load_batt); //print load voltage to terminal
#endif
#endif

#ifdef DUT_BATT_ENABLE
                v_DUT_batt =get_voltage(v_DUT_batt_measure); //measure DUT batt voltage
#ifdef SERIAL_INTERFACE
                pc.printf("Analog In (p20): %f ", v_DUT_batt); //print load voltage to terminal
#endif
#endif


#ifdef DUT_BATT_CAP_ENABLE
                v_cap = get_voltage(v_cap_measure); //measure cap voltage
                v_DUT_batt =get_voltage(v_DUT_batt_measure); //measure DUT batt voltage
#ifdef SERIAL_INTERFACE
                pc.printf("Analog In (p19): %f Analog In (p20): %f Speed: %f Current: %f\r\n", v_cap, v_DUT_batt, speed, current); //print cap voltage, DUT batt voltage, speed, current to terminal
#endif
#endif
            }

            // --- FAST LOOP (100 Hz) ---
            if (timer.read_ms() - fast_timer >= 10) {
                fast_timer = timer.read_ms(); //read time in ms
                set_duty(DUT_input,load_input); //set the duty cycle
                get_speed(); //update motor speed

#ifdef CURRENT_SENSOR_ON
                get_current(); //update current
#endif

#ifdef LIGHTS_LOAD_CONTROL //controls the board LEDs based on the value of the DUT and Load motor duty cycle
                //turn on green LEDs if in regen mode, and orange LEDs if in drive mode.
                if (DUT_input >= load_input && (DUT_input != 0 || load_input != 0)) {//check if not in regen mode
                    regen = 0; //set regen flag low
                    drive_LEDS(); //turn on orange LEDs
                } else if (DUT_input < load_input && (DUT_input != 0 || load_input != 0)) {//check if in regen mode
                    regen = 1; //set regen flag high
                    regen_LEDS(); //turn on green LEDs
                } else {// DUT is niether flowing into or out of DUT
                    regen = 0; //set regen flag low
                    all_LEDS_off();// LEDs off
                }
#endif

#ifdef LIGHTS_CURRENT_CONTROL  //controls the board LEDs based on the value of the DUT and Load motor currents
                //turn on green LEDs if in regen mode, and orange LEDs if in drive mode.
                if (current > 0) {
                    regen = 0; //set regen flag low
                    drive_LEDS(); //turn on orange LEDs
                } else if (current < 0) {
                    regen = 1; //set regen flag high
                    regen_LEDS(); //turn on green LEDs
                } else {
                    regen = 0; //set regen flag low
                    all_LEDS_off(); // LEDs off
                }
#endif

#ifdef DUT_BATT_CAP_ENABLE
                superCapControl();
#endif
            }

            // reset timer if 1000 seconds have elapsed
            if (timer.read() >= 1000) {
                timer.reset();
                slow_timer = 0;
                fast_timer = 0;
            }
        }
        //turn off motors
        load_motor =0;
        DUT_motor = 0;
        timer.stop();
        timer.reset();
    }

}