main.cpp

00001 /******************************************************************************
00002 ***********************           Protodrive            ***********************
00003 *************           Andrew Botelho & William Price            *************
00004 ******************************************************************************/
00005 
00006 #include "mbed.h"
00007 #include "SerialRPCInterface.h"
00008 #include "RPCVariable.h"
00009 
00010 /***********************************************************/
00011 /*Constants                                                */
00012 /***********************************************************/
00013 #define MATLAB_INTERFACE //matlab used as an interface
00014 //#define SERIAL_INTERFACE //Terminal used as an interface. Mainly for debugging
00015 
00016 #define CURRENT_SENSOR_ON //turn on current sensor
00017 //#define DEBUG
00018 //#define LIGHTS_CURRENT_CONTROL //power flow light control is determined by the current sensor
00019 #define LIGHTS_LOAD_CONTROL //power flow light control is determined by the voltage demand placed on the motors
00020 
00021 /**********************Power sources options***************/
00022 /*Instruction: There are 3 power source options for the DUT
00023 and two for the load. Uncomment the option that you want to
00024 use and comment options that you do not want to use. The DUT
00025 and load should each only have one option uncommented.
00026 */
00027 
00028 //DUT
00029 //1. External supply
00030 //#define DUT_EXTERNAL_POWER_ENABLE
00031 
00032 //2. Just DUT batt enabled
00033 #define DUT_BATT_ENABLE
00034 
00035 //3. DUT batt and cap enabled
00036 //#define DUT_BATT_CAP_ENABLE
00037 
00038 //Load
00039 //1. External Supply
00040 //#define LOAD_EXTERNAL_POWER_ENABLE
00041 
00042 //2. Load batt enabled
00043 #define LOAD_BATT_ENABLE
00044 
00045 /***********************************************************/
00046 /*Global Variables                                         */
00047 /***********************************************************/
00048 float v_DUT_batt, v_load_batt, v_cap,  speed, current;
00049 int big_error,regen;
00050 int foo = 1;
00051 
00052 //motor direction and load
00053 float DUT_input = 0; //DUT duty cycle demand variable
00054 float load_input = 0; //Load duty cycle demand variable
00055 float DUT_input_direc = 1; //DUT direction variable. Default to forward
00056 float load_input_direc = 0; //Load direction variable. Default to reverse. This means it will NOT oppose the DUT
00057 
00058 /***********************************************************/
00059 /*Pin Setup                                                */
00060 /***********************************************************/
00061 //mbed LEDS
00062 DigitalOut big_error_led(LED1); //error LED
00063 
00064 //Board LEDs
00065 DigitalOut leftArrow(p29); //setting high turns on green left arrow LEDS
00066 DigitalOut greenLine(p28); //setting high turns on green centerline LEDS
00067 DigitalOut orangeLine(p27); //setting high turns on orange centerline LEDS
00068 DigitalOut rightArrow(p26); //setting high turns on orange right arrow LEDS
00069 
00070 DigitalOut load_batt_green(p25); //setting high will turn on the load batt green LED
00071 DigitalOut load_batt_orange(p24); //setting high will turn on the load batt orange LED
00072 DigitalOut DUT_batt_green(p10); //setting high will turn on the DUT batt green LED
00073 DigitalOut DUT_batt_orange(p11); //setting high will turn on the DUT batt orange LED
00074 
00075 DigitalOut cap_green(p12); //setting high will turn on the cap green LED
00076 DigitalOut cap_orange(p13); //setting high will turn on the cap orange LED
00077 
00078 #ifdef CURRENT_SENSOR_ON
00079 AnalogIn current_sense(p15); //current sensor on p15
00080 #else
00081 DigitalOut place_holder15(p15); //sets p15 as a DigitalOut that will be set low, to reduce noise on analog in pins
00082 #endif
00083 
00084 //sets p15 and p18 as a DigitalOut that will be set low, to reduce noise on analog in pins
00085 DigitalOut place_holder16(p16);
00086 DigitalOut place_holder18(p18);
00087 
00088 #ifdef LOAD_BATT_ENABLE
00089 AnalogIn v_load_batt_measure(p17); //Analog in from load battery
00090 #endif
00091 
00092 #ifdef DUT_BATT_ENABLE
00093 AnalogIn v_DUT_batt_measure(p20); //Analog in from drive battery
00094 DigitalOut place_holder19(p19); //sets p19 as a DigitalOut that will be set low, to reduce noise on analog in pins
00095 #endif
00096 
00097 #ifdef DUT_BATT_CAP_ENABLE
00098 AnalogIn v_DUT_batt_measure(p20); //Analog in from drive battery
00099 AnalogIn v_cap_measure(p19); //Analog in from super cap
00100 #endif
00101 
00102 DigitalIn kill_switch(p9); //switch to disable running program
00103 
00104 //matlab interface
00105 #ifdef MATLAB_INTERFACE
00106 //tie program variables to RPC variables used in Matlab
00107 RPCVariable<float> RPC_DUT_input(&DUT_input, "DUT_input");
00108 RPCVariable<float> RPC_load_input(&load_input, "load_input");
00109 RPCVariable<float> RPC_DUT_input_direc(&DUT_input_direc, "DUT_input_direc");
00110 RPCVariable<float> RPC_load_input_direc(&load_input_direc,"load_input_direc");
00111 RPCVariable<float> RPC_current(&current, "current");
00112 RPCVariable<float> RPC_speed(&speed, "speed");
00113 RPCVariable<float> RPC_v_DUT_batt(&v_DUT_batt, "v_DUT_batt");
00114 RPCVariable<float> RPC_v_load_batt(&v_DUT_batt, "v_load_batt");
00115 RPCVariable<float> RPC_v_cap(&v_DUT_batt, "v_cap");
00116 #endif
00117 
00118 /***********************************************************/
00119 /*Terminals                                                */
00120 /***********************************************************/
00121 
00122 #ifdef MATLAB_INTERFACE
00123 SerialRPCInterface SerialInterface(USBTX, USBRX);//establich RPC serial connection
00124 #endif
00125 
00126 #ifdef SERIAL_INTERFACE
00127 Serial pc(USBTX, USBRX); //Establish serial
00128 #endif
00129 
00130 /***********************************************************/
00131 /*Other Includes                                           */
00132 /***********************************************************/
00133 
00134 #include "power_sources.h"
00135 #include "measurement.h"
00136 #include "motor_control.h"
00137 
00138 /***********************************************************/
00139 /*Subroutines                                              */
00140 /***********************************************************/
00141 
00142 //LED control
00143 void all_LEDS_flash() { //a demo program that just switches LEDs between green and orange
00144     if (foo == 0) {
00145         leftArrow = 0;
00146         greenLine = 0;
00147         orangeLine = 1;
00148         rightArrow = 1;
00149         load_batt_green = 0;
00150         load_batt_orange = 1;
00151         DUT_batt_green = 0;
00152         DUT_batt_orange = 1;
00153         cap_green = 0;
00154         cap_orange = 1;
00155         foo = 1;
00156     } else {
00157         leftArrow = 1;
00158         greenLine = 1;
00159         orangeLine = 0;
00160         rightArrow = 0;
00161         load_batt_green = 1;
00162         load_batt_orange = 0;
00163         DUT_batt_green = 1;
00164         DUT_batt_orange = 0;
00165         cap_green = 1;
00166         cap_orange = 0;
00167         foo = 0;
00168     }
00169 }
00170 
00171 //turn all LEDs off
00172 void all_LEDS_off() {
00173     load_batt_green = 1;
00174     load_batt_orange = 1;
00175     leftArrow = 1;
00176     greenLine = 1;
00177     orangeLine = 1;
00178     rightArrow = 1;
00179     DUT_batt_green = 1;
00180     DUT_batt_orange = 1;
00181     cap_green = 1;
00182     cap_orange = 1;
00183 }
00184 
00185 //turns on the green LEDs indicating regen
00186 void regen_LEDS() {
00187     load_batt_green = 1;
00188     load_batt_orange = 0;
00189     leftArrow = 0;
00190     greenLine = 0;
00191     orangeLine = 1;
00192     rightArrow = 1;
00193     if (!relay) {
00194         DUT_batt_green = 0;
00195         DUT_batt_orange = 1;
00196         cap_green = 1;
00197         cap_orange = 1;
00198     } else {
00199         DUT_batt_green = 1;
00200         DUT_batt_orange = 1;
00201         cap_green = 0;
00202         cap_orange = 1;
00203     }
00204 }
00205 
00206 //turns on the orange LEDs indicating drive mode (power flowing out of the vehicle)
00207 void drive_LEDS() {
00208     load_batt_green = 0;
00209     load_batt_orange = 1;
00210     leftArrow = 1;
00211     greenLine = 1;
00212     orangeLine = 0;
00213     rightArrow = 0;
00214     if (!relay) {
00215         DUT_batt_green = 1;
00216         DUT_batt_orange = 0;
00217         cap_green = 1;
00218         cap_orange = 1;
00219     } else {
00220         DUT_batt_green = 1;
00221         DUT_batt_orange = 1;
00222         cap_green = 1;
00223         cap_orange = 0;
00224     }
00225 }
00226 /***********************************************************/
00227 /*Main program setup                                       */
00228 /***********************************************************/
00229 
00230 int main() {
00231 
00232     Timer timer; //create a timer for the main loop
00233     init_pwm(); //initialize PWM
00234 
00235     /***********************************************************/
00236     /*Main Loop                                                */
00237     /***********************************************************/
00238     while (1) {
00239         while (kill_switch ==0) {//run demo while waiting for kill switch to be turned on
00240             all_LEDS_flash(); //switch LEDs between green and orange
00241             wait(.5); //wait 0.5s
00242         }
00243 
00244         // System initializations
00245         DUT_direction.write(1); //set forward
00246         load_direction.write(0); //set reverse. This ensures both motors will apply a torque on the coupler in the same direction
00247 
00248         //initialize system variables to 0
00249         speed = 0;
00250         current = 0;
00251         big_error = 0;
00252         big_error_led = 0;
00253         relay = 0; //turn on battery by default
00254 
00255         //main loop timer variables
00256         int slow_timer = 0;
00257         int fast_timer = 0;
00258 
00259         timer.start(); //start loop timer
00260         all_LEDS_off(); //turn off LEDs
00261         wait(.5); //wait 0.5s before starting
00262 
00263 #ifdef DEBUG //debugging code
00264         DUT_input = 0.75;
00265         load_input = 0.25;
00266 #endif
00267 
00268 #ifdef DUT_BATT_ENABLE
00269         place_holder19 = 0; //set DigitalOut pin low to reduce noise on adjacent AnalogIn pins
00270 #endif
00271 
00272         //set unused Analog pins to ground
00273 #ifndef CURRENT_SENSOR_ON
00274         place_holder15 = 0; //set DigitalOut pin low to reduce noise on adjacent AnalogIn pins
00275 #endif
00276         place_holder16 = 0; //set DigitalOut pin low to reduce noise on adjacent AnalogIn pins
00277         place_holder18 = 0; //set DigitalOut pin low to reduce noise on adjacent AnalogIn pins
00278 
00279         while (kill_switch == 1 && big_error == 0) {//while the kill switch is on and there are no errors
00280 
00281             // --- SLOW LOOP (1) Hz ---
00282             if (timer.read_ms() - slow_timer >= 1000) {
00283                 slow_timer = timer.read_ms(); //read timer value in miliseconds
00284                 big_error = checkBattVoltages(); //check batt voltages to ensure they are in the correct voltage range
00285                 if (big_error == 1) {
00286                     big_error_led = 1;
00287                 }
00288                 //measure voltages
00289 #ifdef LOAD_BATT_ENABLE
00290                 v_load_batt = get_voltage(v_load_batt_measure); //measure load batt voltage
00291 #ifdef SERIAL_INTERFACE
00292                 pc.printf("Analog In (p17): %f ", v_load_batt); //print load voltage to terminal
00293 #endif
00294 #endif
00295 
00296 #ifdef DUT_BATT_ENABLE
00297                 v_DUT_batt =get_voltage(v_DUT_batt_measure); //measure DUT batt voltage
00298 #ifdef SERIAL_INTERFACE
00299                 pc.printf("Analog In (p20): %f ", v_DUT_batt); //print load voltage to terminal
00300 #endif
00301 #endif
00302 
00303 
00304 #ifdef DUT_BATT_CAP_ENABLE
00305                 v_cap = get_voltage(v_cap_measure); //measure cap voltage
00306                 v_DUT_batt =get_voltage(v_DUT_batt_measure); //measure DUT batt voltage
00307 #ifdef SERIAL_INTERFACE
00308                 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
00309 #endif
00310 #endif
00311             }
00312 
00313             // --- FAST LOOP (100 Hz) ---
00314             if (timer.read_ms() - fast_timer >= 10) {
00315                 fast_timer = timer.read_ms(); //read time in ms
00316                 set_duty(DUT_input,load_input); //set the duty cycle
00317                 get_speed(); //update motor speed
00318 
00319 #ifdef CURRENT_SENSOR_ON
00320                 get_current(); //update current
00321 #endif
00322 
00323 #ifdef LIGHTS_LOAD_CONTROL //controls the board LEDs based on the value of the DUT and Load motor duty cycle
00324                 //turn on green LEDs if in regen mode, and orange LEDs if in drive mode.
00325                 if (DUT_input >= load_input && (DUT_input != 0 || load_input != 0)) {//check if not in regen mode
00326                     regen = 0; //set regen flag low
00327                     drive_LEDS(); //turn on orange LEDs
00328                 } else if (DUT_input < load_input && (DUT_input != 0 || load_input != 0)) {//check if in regen mode
00329                     regen = 1; //set regen flag high
00330                     regen_LEDS(); //turn on green LEDs
00331                 } else {// DUT is niether flowing into or out of DUT
00332                     regen = 0; //set regen flag low
00333                     all_LEDS_off();// LEDs off
00334                 }
00335 #endif
00336 
00337 #ifdef LIGHTS_CURRENT_CONTROL  //controls the board LEDs based on the value of the DUT and Load motor currents
00338                 //turn on green LEDs if in regen mode, and orange LEDs if in drive mode.
00339                 if (current > 0) {
00340                     regen = 0; //set regen flag low
00341                     drive_LEDS(); //turn on orange LEDs
00342                 } else if (current < 0) {
00343                     regen = 1; //set regen flag high
00344                     regen_LEDS(); //turn on green LEDs
00345                 } else {
00346                     regen = 0; //set regen flag low
00347                     all_LEDS_off(); // LEDs off
00348                 }
00349 #endif
00350 
00351 #ifdef DUT_BATT_CAP_ENABLE
00352                 superCapControl();
00353 #endif
00354             }
00355 
00356             // reset timer if 1000 seconds have elapsed
00357             if (timer.read() >= 1000) {
00358                 timer.reset();
00359                 slow_timer = 0;
00360                 fast_timer = 0;
00361             }
00362         }
00363         //turn off motors
00364         load_motor =0;
00365         DUT_motor = 0;
00366         timer.stop();
00367         timer.reset();
00368     }
00369 
00370 }