Proto Drive
Dependencies: RPCInterface mbed
main.cpp@0:2dbd366be5fd, 2012-05-07 (annotated)
- Committer:
- protodriveev
- Date:
- Mon May 07 16:14:45 2012 +0000
- Revision:
- 0:2dbd366be5fd
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| protodriveev | 0:2dbd366be5fd | 1 | /****************************************************************************** |
| protodriveev | 0:2dbd366be5fd | 2 | *********************** Protodrive *********************** |
| protodriveev | 0:2dbd366be5fd | 3 | ************* Andrew Botelho & William Price ************* |
| protodriveev | 0:2dbd366be5fd | 4 | ******************************************************************************/ |
| protodriveev | 0:2dbd366be5fd | 5 | |
| protodriveev | 0:2dbd366be5fd | 6 | #include "mbed.h" |
| protodriveev | 0:2dbd366be5fd | 7 | #include "SerialRPCInterface.h" |
| protodriveev | 0:2dbd366be5fd | 8 | #include "RPCVariable.h" |
| protodriveev | 0:2dbd366be5fd | 9 | |
| protodriveev | 0:2dbd366be5fd | 10 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 11 | /*Constants */ |
| protodriveev | 0:2dbd366be5fd | 12 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 13 | #define MATLAB_INTERFACE //matlab used as an interface |
| protodriveev | 0:2dbd366be5fd | 14 | //#define SERIAL_INTERFACE //Terminal used as an interface. Mainly for debugging |
| protodriveev | 0:2dbd366be5fd | 15 | |
| protodriveev | 0:2dbd366be5fd | 16 | #define CURRENT_SENSOR_ON //turn on current sensor |
| protodriveev | 0:2dbd366be5fd | 17 | //#define DEBUG |
| protodriveev | 0:2dbd366be5fd | 18 | //#define LIGHTS_CURRENT_CONTROL //power flow light control is determined by the current sensor |
| protodriveev | 0:2dbd366be5fd | 19 | #define LIGHTS_LOAD_CONTROL //power flow light control is determined by the voltage demand placed on the motors |
| protodriveev | 0:2dbd366be5fd | 20 | |
| protodriveev | 0:2dbd366be5fd | 21 | /**********************Power sources options***************/ |
| protodriveev | 0:2dbd366be5fd | 22 | /*Instruction: There are 3 power source options for the DUT |
| protodriveev | 0:2dbd366be5fd | 23 | and two for the load. Uncomment the option that you want to |
| protodriveev | 0:2dbd366be5fd | 24 | use and comment options that you do not want to use. The DUT |
| protodriveev | 0:2dbd366be5fd | 25 | and load should each only have one option uncommented. |
| protodriveev | 0:2dbd366be5fd | 26 | */ |
| protodriveev | 0:2dbd366be5fd | 27 | |
| protodriveev | 0:2dbd366be5fd | 28 | //DUT |
| protodriveev | 0:2dbd366be5fd | 29 | //1. External supply |
| protodriveev | 0:2dbd366be5fd | 30 | //#define DUT_EXTERNAL_POWER_ENABLE |
| protodriveev | 0:2dbd366be5fd | 31 | |
| protodriveev | 0:2dbd366be5fd | 32 | //2. Just DUT batt enabled |
| protodriveev | 0:2dbd366be5fd | 33 | #define DUT_BATT_ENABLE |
| protodriveev | 0:2dbd366be5fd | 34 | |
| protodriveev | 0:2dbd366be5fd | 35 | //3. DUT batt and cap enabled |
| protodriveev | 0:2dbd366be5fd | 36 | //#define DUT_BATT_CAP_ENABLE |
| protodriveev | 0:2dbd366be5fd | 37 | |
| protodriveev | 0:2dbd366be5fd | 38 | //Load |
| protodriveev | 0:2dbd366be5fd | 39 | //1. External Supply |
| protodriveev | 0:2dbd366be5fd | 40 | //#define LOAD_EXTERNAL_POWER_ENABLE |
| protodriveev | 0:2dbd366be5fd | 41 | |
| protodriveev | 0:2dbd366be5fd | 42 | //2. Load batt enabled |
| protodriveev | 0:2dbd366be5fd | 43 | #define LOAD_BATT_ENABLE |
| protodriveev | 0:2dbd366be5fd | 44 | |
| protodriveev | 0:2dbd366be5fd | 45 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 46 | /*Global Variables */ |
| protodriveev | 0:2dbd366be5fd | 47 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 48 | float v_DUT_batt, v_load_batt, v_cap, speed, current; |
| protodriveev | 0:2dbd366be5fd | 49 | int big_error,regen; |
| protodriveev | 0:2dbd366be5fd | 50 | int foo = 1; |
| protodriveev | 0:2dbd366be5fd | 51 | |
| protodriveev | 0:2dbd366be5fd | 52 | //motor direction and load |
| protodriveev | 0:2dbd366be5fd | 53 | float DUT_input = 0; //DUT duty cycle demand variable |
| protodriveev | 0:2dbd366be5fd | 54 | float load_input = 0; //Load duty cycle demand variable |
| protodriveev | 0:2dbd366be5fd | 55 | float DUT_input_direc = 1; //DUT direction variable. Default to forward |
| protodriveev | 0:2dbd366be5fd | 56 | float load_input_direc = 0; //Load direction variable. Default to reverse. This means it will NOT oppose the DUT |
| protodriveev | 0:2dbd366be5fd | 57 | |
| protodriveev | 0:2dbd366be5fd | 58 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 59 | /*Pin Setup */ |
| protodriveev | 0:2dbd366be5fd | 60 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 61 | //mbed LEDS |
| protodriveev | 0:2dbd366be5fd | 62 | DigitalOut big_error_led(LED1); //error LED |
| protodriveev | 0:2dbd366be5fd | 63 | |
| protodriveev | 0:2dbd366be5fd | 64 | //Board LEDs |
| protodriveev | 0:2dbd366be5fd | 65 | DigitalOut leftArrow(p29); //setting high turns on green left arrow LEDS |
| protodriveev | 0:2dbd366be5fd | 66 | DigitalOut greenLine(p28); //setting high turns on green centerline LEDS |
| protodriveev | 0:2dbd366be5fd | 67 | DigitalOut orangeLine(p27); //setting high turns on orange centerline LEDS |
| protodriveev | 0:2dbd366be5fd | 68 | DigitalOut rightArrow(p26); //setting high turns on orange right arrow LEDS |
| protodriveev | 0:2dbd366be5fd | 69 | |
| protodriveev | 0:2dbd366be5fd | 70 | DigitalOut load_batt_green(p25); //setting high will turn on the load batt green LED |
| protodriveev | 0:2dbd366be5fd | 71 | DigitalOut load_batt_orange(p24); //setting high will turn on the load batt orange LED |
| protodriveev | 0:2dbd366be5fd | 72 | DigitalOut DUT_batt_green(p10); //setting high will turn on the DUT batt green LED |
| protodriveev | 0:2dbd366be5fd | 73 | DigitalOut DUT_batt_orange(p11); //setting high will turn on the DUT batt orange LED |
| protodriveev | 0:2dbd366be5fd | 74 | |
| protodriveev | 0:2dbd366be5fd | 75 | DigitalOut cap_green(p12); //setting high will turn on the cap green LED |
| protodriveev | 0:2dbd366be5fd | 76 | DigitalOut cap_orange(p13); //setting high will turn on the cap orange LED |
| protodriveev | 0:2dbd366be5fd | 77 | |
| protodriveev | 0:2dbd366be5fd | 78 | #ifdef CURRENT_SENSOR_ON |
| protodriveev | 0:2dbd366be5fd | 79 | AnalogIn current_sense(p15); //current sensor on p15 |
| protodriveev | 0:2dbd366be5fd | 80 | #else |
| protodriveev | 0:2dbd366be5fd | 81 | DigitalOut place_holder15(p15); //sets p15 as a DigitalOut that will be set low, to reduce noise on analog in pins |
| protodriveev | 0:2dbd366be5fd | 82 | #endif |
| protodriveev | 0:2dbd366be5fd | 83 | |
| protodriveev | 0:2dbd366be5fd | 84 | //sets p15 and p18 as a DigitalOut that will be set low, to reduce noise on analog in pins |
| protodriveev | 0:2dbd366be5fd | 85 | DigitalOut place_holder16(p16); |
| protodriveev | 0:2dbd366be5fd | 86 | DigitalOut place_holder18(p18); |
| protodriveev | 0:2dbd366be5fd | 87 | |
| protodriveev | 0:2dbd366be5fd | 88 | #ifdef LOAD_BATT_ENABLE |
| protodriveev | 0:2dbd366be5fd | 89 | AnalogIn v_load_batt_measure(p17); //Analog in from load battery |
| protodriveev | 0:2dbd366be5fd | 90 | #endif |
| protodriveev | 0:2dbd366be5fd | 91 | |
| protodriveev | 0:2dbd366be5fd | 92 | #ifdef DUT_BATT_ENABLE |
| protodriveev | 0:2dbd366be5fd | 93 | AnalogIn v_DUT_batt_measure(p20); //Analog in from drive battery |
| protodriveev | 0:2dbd366be5fd | 94 | DigitalOut place_holder19(p19); //sets p19 as a DigitalOut that will be set low, to reduce noise on analog in pins |
| protodriveev | 0:2dbd366be5fd | 95 | #endif |
| protodriveev | 0:2dbd366be5fd | 96 | |
| protodriveev | 0:2dbd366be5fd | 97 | #ifdef DUT_BATT_CAP_ENABLE |
| protodriveev | 0:2dbd366be5fd | 98 | AnalogIn v_DUT_batt_measure(p20); //Analog in from drive battery |
| protodriveev | 0:2dbd366be5fd | 99 | AnalogIn v_cap_measure(p19); //Analog in from super cap |
| protodriveev | 0:2dbd366be5fd | 100 | #endif |
| protodriveev | 0:2dbd366be5fd | 101 | |
| protodriveev | 0:2dbd366be5fd | 102 | DigitalIn kill_switch(p9); //switch to disable running program |
| protodriveev | 0:2dbd366be5fd | 103 | |
| protodriveev | 0:2dbd366be5fd | 104 | //matlab interface |
| protodriveev | 0:2dbd366be5fd | 105 | #ifdef MATLAB_INTERFACE |
| protodriveev | 0:2dbd366be5fd | 106 | //tie program variables to RPC variables used in Matlab |
| protodriveev | 0:2dbd366be5fd | 107 | RPCVariable<float> RPC_DUT_input(&DUT_input, "DUT_input"); |
| protodriveev | 0:2dbd366be5fd | 108 | RPCVariable<float> RPC_load_input(&load_input, "load_input"); |
| protodriveev | 0:2dbd366be5fd | 109 | RPCVariable<float> RPC_DUT_input_direc(&DUT_input_direc, "DUT_input_direc"); |
| protodriveev | 0:2dbd366be5fd | 110 | RPCVariable<float> RPC_load_input_direc(&load_input_direc,"load_input_direc"); |
| protodriveev | 0:2dbd366be5fd | 111 | RPCVariable<float> RPC_current(¤t, "current"); |
| protodriveev | 0:2dbd366be5fd | 112 | RPCVariable<float> RPC_speed(&speed, "speed"); |
| protodriveev | 0:2dbd366be5fd | 113 | RPCVariable<float> RPC_v_DUT_batt(&v_DUT_batt, "v_DUT_batt"); |
| protodriveev | 0:2dbd366be5fd | 114 | RPCVariable<float> RPC_v_load_batt(&v_DUT_batt, "v_load_batt"); |
| protodriveev | 0:2dbd366be5fd | 115 | RPCVariable<float> RPC_v_cap(&v_DUT_batt, "v_cap"); |
| protodriveev | 0:2dbd366be5fd | 116 | #endif |
| protodriveev | 0:2dbd366be5fd | 117 | |
| protodriveev | 0:2dbd366be5fd | 118 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 119 | /*Terminals */ |
| protodriveev | 0:2dbd366be5fd | 120 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 121 | |
| protodriveev | 0:2dbd366be5fd | 122 | #ifdef MATLAB_INTERFACE |
| protodriveev | 0:2dbd366be5fd | 123 | SerialRPCInterface SerialInterface(USBTX, USBRX);//establich RPC serial connection |
| protodriveev | 0:2dbd366be5fd | 124 | #endif |
| protodriveev | 0:2dbd366be5fd | 125 | |
| protodriveev | 0:2dbd366be5fd | 126 | #ifdef SERIAL_INTERFACE |
| protodriveev | 0:2dbd366be5fd | 127 | Serial pc(USBTX, USBRX); //Establish serial |
| protodriveev | 0:2dbd366be5fd | 128 | #endif |
| protodriveev | 0:2dbd366be5fd | 129 | |
| protodriveev | 0:2dbd366be5fd | 130 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 131 | /*Other Includes */ |
| protodriveev | 0:2dbd366be5fd | 132 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 133 | |
| protodriveev | 0:2dbd366be5fd | 134 | #include "power_sources.h" |
| protodriveev | 0:2dbd366be5fd | 135 | #include "measurement.h" |
| protodriveev | 0:2dbd366be5fd | 136 | #include "motor_control.h" |
| protodriveev | 0:2dbd366be5fd | 137 | |
| protodriveev | 0:2dbd366be5fd | 138 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 139 | /*Subroutines */ |
| protodriveev | 0:2dbd366be5fd | 140 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 141 | |
| protodriveev | 0:2dbd366be5fd | 142 | //LED control |
| protodriveev | 0:2dbd366be5fd | 143 | void all_LEDS_flash() { //a demo program that just switches LEDs between green and orange |
| protodriveev | 0:2dbd366be5fd | 144 | if (foo == 0) { |
| protodriveev | 0:2dbd366be5fd | 145 | leftArrow = 0; |
| protodriveev | 0:2dbd366be5fd | 146 | greenLine = 0; |
| protodriveev | 0:2dbd366be5fd | 147 | orangeLine = 1; |
| protodriveev | 0:2dbd366be5fd | 148 | rightArrow = 1; |
| protodriveev | 0:2dbd366be5fd | 149 | load_batt_green = 0; |
| protodriveev | 0:2dbd366be5fd | 150 | load_batt_orange = 1; |
| protodriveev | 0:2dbd366be5fd | 151 | DUT_batt_green = 0; |
| protodriveev | 0:2dbd366be5fd | 152 | DUT_batt_orange = 1; |
| protodriveev | 0:2dbd366be5fd | 153 | cap_green = 0; |
| protodriveev | 0:2dbd366be5fd | 154 | cap_orange = 1; |
| protodriveev | 0:2dbd366be5fd | 155 | foo = 1; |
| protodriveev | 0:2dbd366be5fd | 156 | } else { |
| protodriveev | 0:2dbd366be5fd | 157 | leftArrow = 1; |
| protodriveev | 0:2dbd366be5fd | 158 | greenLine = 1; |
| protodriveev | 0:2dbd366be5fd | 159 | orangeLine = 0; |
| protodriveev | 0:2dbd366be5fd | 160 | rightArrow = 0; |
| protodriveev | 0:2dbd366be5fd | 161 | load_batt_green = 1; |
| protodriveev | 0:2dbd366be5fd | 162 | load_batt_orange = 0; |
| protodriveev | 0:2dbd366be5fd | 163 | DUT_batt_green = 1; |
| protodriveev | 0:2dbd366be5fd | 164 | DUT_batt_orange = 0; |
| protodriveev | 0:2dbd366be5fd | 165 | cap_green = 1; |
| protodriveev | 0:2dbd366be5fd | 166 | cap_orange = 0; |
| protodriveev | 0:2dbd366be5fd | 167 | foo = 0; |
| protodriveev | 0:2dbd366be5fd | 168 | } |
| protodriveev | 0:2dbd366be5fd | 169 | } |
| protodriveev | 0:2dbd366be5fd | 170 | |
| protodriveev | 0:2dbd366be5fd | 171 | //turn all LEDs off |
| protodriveev | 0:2dbd366be5fd | 172 | void all_LEDS_off() { |
| protodriveev | 0:2dbd366be5fd | 173 | load_batt_green = 1; |
| protodriveev | 0:2dbd366be5fd | 174 | load_batt_orange = 1; |
| protodriveev | 0:2dbd366be5fd | 175 | leftArrow = 1; |
| protodriveev | 0:2dbd366be5fd | 176 | greenLine = 1; |
| protodriveev | 0:2dbd366be5fd | 177 | orangeLine = 1; |
| protodriveev | 0:2dbd366be5fd | 178 | rightArrow = 1; |
| protodriveev | 0:2dbd366be5fd | 179 | DUT_batt_green = 1; |
| protodriveev | 0:2dbd366be5fd | 180 | DUT_batt_orange = 1; |
| protodriveev | 0:2dbd366be5fd | 181 | cap_green = 1; |
| protodriveev | 0:2dbd366be5fd | 182 | cap_orange = 1; |
| protodriveev | 0:2dbd366be5fd | 183 | } |
| protodriveev | 0:2dbd366be5fd | 184 | |
| protodriveev | 0:2dbd366be5fd | 185 | //turns on the green LEDs indicating regen |
| protodriveev | 0:2dbd366be5fd | 186 | void regen_LEDS() { |
| protodriveev | 0:2dbd366be5fd | 187 | load_batt_green = 1; |
| protodriveev | 0:2dbd366be5fd | 188 | load_batt_orange = 0; |
| protodriveev | 0:2dbd366be5fd | 189 | leftArrow = 0; |
| protodriveev | 0:2dbd366be5fd | 190 | greenLine = 0; |
| protodriveev | 0:2dbd366be5fd | 191 | orangeLine = 1; |
| protodriveev | 0:2dbd366be5fd | 192 | rightArrow = 1; |
| protodriveev | 0:2dbd366be5fd | 193 | if (!relay) { |
| protodriveev | 0:2dbd366be5fd | 194 | DUT_batt_green = 0; |
| protodriveev | 0:2dbd366be5fd | 195 | DUT_batt_orange = 1; |
| protodriveev | 0:2dbd366be5fd | 196 | cap_green = 1; |
| protodriveev | 0:2dbd366be5fd | 197 | cap_orange = 1; |
| protodriveev | 0:2dbd366be5fd | 198 | } else { |
| protodriveev | 0:2dbd366be5fd | 199 | DUT_batt_green = 1; |
| protodriveev | 0:2dbd366be5fd | 200 | DUT_batt_orange = 1; |
| protodriveev | 0:2dbd366be5fd | 201 | cap_green = 0; |
| protodriveev | 0:2dbd366be5fd | 202 | cap_orange = 1; |
| protodriveev | 0:2dbd366be5fd | 203 | } |
| protodriveev | 0:2dbd366be5fd | 204 | } |
| protodriveev | 0:2dbd366be5fd | 205 | |
| protodriveev | 0:2dbd366be5fd | 206 | //turns on the orange LEDs indicating drive mode (power flowing out of the vehicle) |
| protodriveev | 0:2dbd366be5fd | 207 | void drive_LEDS() { |
| protodriveev | 0:2dbd366be5fd | 208 | load_batt_green = 0; |
| protodriveev | 0:2dbd366be5fd | 209 | load_batt_orange = 1; |
| protodriveev | 0:2dbd366be5fd | 210 | leftArrow = 1; |
| protodriveev | 0:2dbd366be5fd | 211 | greenLine = 1; |
| protodriveev | 0:2dbd366be5fd | 212 | orangeLine = 0; |
| protodriveev | 0:2dbd366be5fd | 213 | rightArrow = 0; |
| protodriveev | 0:2dbd366be5fd | 214 | if (!relay) { |
| protodriveev | 0:2dbd366be5fd | 215 | DUT_batt_green = 1; |
| protodriveev | 0:2dbd366be5fd | 216 | DUT_batt_orange = 0; |
| protodriveev | 0:2dbd366be5fd | 217 | cap_green = 1; |
| protodriveev | 0:2dbd366be5fd | 218 | cap_orange = 1; |
| protodriveev | 0:2dbd366be5fd | 219 | } else { |
| protodriveev | 0:2dbd366be5fd | 220 | DUT_batt_green = 1; |
| protodriveev | 0:2dbd366be5fd | 221 | DUT_batt_orange = 1; |
| protodriveev | 0:2dbd366be5fd | 222 | cap_green = 1; |
| protodriveev | 0:2dbd366be5fd | 223 | cap_orange = 0; |
| protodriveev | 0:2dbd366be5fd | 224 | } |
| protodriveev | 0:2dbd366be5fd | 225 | } |
| protodriveev | 0:2dbd366be5fd | 226 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 227 | /*Main program setup */ |
| protodriveev | 0:2dbd366be5fd | 228 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 229 | |
| protodriveev | 0:2dbd366be5fd | 230 | int main() { |
| protodriveev | 0:2dbd366be5fd | 231 | |
| protodriveev | 0:2dbd366be5fd | 232 | Timer timer; //create a timer for the main loop |
| protodriveev | 0:2dbd366be5fd | 233 | init_pwm(); //initialize PWM |
| protodriveev | 0:2dbd366be5fd | 234 | |
| protodriveev | 0:2dbd366be5fd | 235 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 236 | /*Main Loop */ |
| protodriveev | 0:2dbd366be5fd | 237 | /***********************************************************/ |
| protodriveev | 0:2dbd366be5fd | 238 | while (1) { |
| protodriveev | 0:2dbd366be5fd | 239 | while (kill_switch ==0) {//run demo while waiting for kill switch to be turned on |
| protodriveev | 0:2dbd366be5fd | 240 | all_LEDS_flash(); //switch LEDs between green and orange |
| protodriveev | 0:2dbd366be5fd | 241 | wait(.5); //wait 0.5s |
| protodriveev | 0:2dbd366be5fd | 242 | } |
| protodriveev | 0:2dbd366be5fd | 243 | |
| protodriveev | 0:2dbd366be5fd | 244 | // System initializations |
| protodriveev | 0:2dbd366be5fd | 245 | DUT_direction.write(1); //set forward |
| protodriveev | 0:2dbd366be5fd | 246 | load_direction.write(0); //set reverse. This ensures both motors will apply a torque on the coupler in the same direction |
| protodriveev | 0:2dbd366be5fd | 247 | |
| protodriveev | 0:2dbd366be5fd | 248 | //initialize system variables to 0 |
| protodriveev | 0:2dbd366be5fd | 249 | speed = 0; |
| protodriveev | 0:2dbd366be5fd | 250 | current = 0; |
| protodriveev | 0:2dbd366be5fd | 251 | big_error = 0; |
| protodriveev | 0:2dbd366be5fd | 252 | big_error_led = 0; |
| protodriveev | 0:2dbd366be5fd | 253 | relay = 0; //turn on battery by default |
| protodriveev | 0:2dbd366be5fd | 254 | |
| protodriveev | 0:2dbd366be5fd | 255 | //main loop timer variables |
| protodriveev | 0:2dbd366be5fd | 256 | int slow_timer = 0; |
| protodriveev | 0:2dbd366be5fd | 257 | int fast_timer = 0; |
| protodriveev | 0:2dbd366be5fd | 258 | |
| protodriveev | 0:2dbd366be5fd | 259 | timer.start(); //start loop timer |
| protodriveev | 0:2dbd366be5fd | 260 | all_LEDS_off(); //turn off LEDs |
| protodriveev | 0:2dbd366be5fd | 261 | wait(.5); //wait 0.5s before starting |
| protodriveev | 0:2dbd366be5fd | 262 | |
| protodriveev | 0:2dbd366be5fd | 263 | #ifdef DEBUG //debugging code |
| protodriveev | 0:2dbd366be5fd | 264 | DUT_input = 0.75; |
| protodriveev | 0:2dbd366be5fd | 265 | load_input = 0.25; |
| protodriveev | 0:2dbd366be5fd | 266 | #endif |
| protodriveev | 0:2dbd366be5fd | 267 | |
| protodriveev | 0:2dbd366be5fd | 268 | #ifdef DUT_BATT_ENABLE |
| protodriveev | 0:2dbd366be5fd | 269 | place_holder19 = 0; //set DigitalOut pin low to reduce noise on adjacent AnalogIn pins |
| protodriveev | 0:2dbd366be5fd | 270 | #endif |
| protodriveev | 0:2dbd366be5fd | 271 | |
| protodriveev | 0:2dbd366be5fd | 272 | //set unused Analog pins to ground |
| protodriveev | 0:2dbd366be5fd | 273 | #ifndef CURRENT_SENSOR_ON |
| protodriveev | 0:2dbd366be5fd | 274 | place_holder15 = 0; //set DigitalOut pin low to reduce noise on adjacent AnalogIn pins |
| protodriveev | 0:2dbd366be5fd | 275 | #endif |
| protodriveev | 0:2dbd366be5fd | 276 | place_holder16 = 0; //set DigitalOut pin low to reduce noise on adjacent AnalogIn pins |
| protodriveev | 0:2dbd366be5fd | 277 | place_holder18 = 0; //set DigitalOut pin low to reduce noise on adjacent AnalogIn pins |
| protodriveev | 0:2dbd366be5fd | 278 | |
| protodriveev | 0:2dbd366be5fd | 279 | while (kill_switch == 1 && big_error == 0) {//while the kill switch is on and there are no errors |
| protodriveev | 0:2dbd366be5fd | 280 | |
| protodriveev | 0:2dbd366be5fd | 281 | // --- SLOW LOOP (1) Hz --- |
| protodriveev | 0:2dbd366be5fd | 282 | if (timer.read_ms() - slow_timer >= 1000) { |
| protodriveev | 0:2dbd366be5fd | 283 | slow_timer = timer.read_ms(); //read timer value in miliseconds |
| protodriveev | 0:2dbd366be5fd | 284 | big_error = checkBattVoltages(); //check batt voltages to ensure they are in the correct voltage range |
| protodriveev | 0:2dbd366be5fd | 285 | if (big_error == 1) { |
| protodriveev | 0:2dbd366be5fd | 286 | big_error_led = 1; |
| protodriveev | 0:2dbd366be5fd | 287 | } |
| protodriveev | 0:2dbd366be5fd | 288 | //measure voltages |
| protodriveev | 0:2dbd366be5fd | 289 | #ifdef LOAD_BATT_ENABLE |
| protodriveev | 0:2dbd366be5fd | 290 | v_load_batt = get_voltage(v_load_batt_measure); //measure load batt voltage |
| protodriveev | 0:2dbd366be5fd | 291 | #ifdef SERIAL_INTERFACE |
| protodriveev | 0:2dbd366be5fd | 292 | pc.printf("Analog In (p17): %f ", v_load_batt); //print load voltage to terminal |
| protodriveev | 0:2dbd366be5fd | 293 | #endif |
| protodriveev | 0:2dbd366be5fd | 294 | #endif |
| protodriveev | 0:2dbd366be5fd | 295 | |
| protodriveev | 0:2dbd366be5fd | 296 | #ifdef DUT_BATT_ENABLE |
| protodriveev | 0:2dbd366be5fd | 297 | v_DUT_batt =get_voltage(v_DUT_batt_measure); //measure DUT batt voltage |
| protodriveev | 0:2dbd366be5fd | 298 | #ifdef SERIAL_INTERFACE |
| protodriveev | 0:2dbd366be5fd | 299 | pc.printf("Analog In (p20): %f ", v_DUT_batt); //print load voltage to terminal |
| protodriveev | 0:2dbd366be5fd | 300 | #endif |
| protodriveev | 0:2dbd366be5fd | 301 | #endif |
| protodriveev | 0:2dbd366be5fd | 302 | |
| protodriveev | 0:2dbd366be5fd | 303 | |
| protodriveev | 0:2dbd366be5fd | 304 | #ifdef DUT_BATT_CAP_ENABLE |
| protodriveev | 0:2dbd366be5fd | 305 | v_cap = get_voltage(v_cap_measure); //measure cap voltage |
| protodriveev | 0:2dbd366be5fd | 306 | v_DUT_batt =get_voltage(v_DUT_batt_measure); //measure DUT batt voltage |
| protodriveev | 0:2dbd366be5fd | 307 | #ifdef SERIAL_INTERFACE |
| protodriveev | 0:2dbd366be5fd | 308 | 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 |
| protodriveev | 0:2dbd366be5fd | 309 | #endif |
| protodriveev | 0:2dbd366be5fd | 310 | #endif |
| protodriveev | 0:2dbd366be5fd | 311 | } |
| protodriveev | 0:2dbd366be5fd | 312 | |
| protodriveev | 0:2dbd366be5fd | 313 | // --- FAST LOOP (100 Hz) --- |
| protodriveev | 0:2dbd366be5fd | 314 | if (timer.read_ms() - fast_timer >= 10) { |
| protodriveev | 0:2dbd366be5fd | 315 | fast_timer = timer.read_ms(); //read time in ms |
| protodriveev | 0:2dbd366be5fd | 316 | set_duty(DUT_input,load_input); //set the duty cycle |
| protodriveev | 0:2dbd366be5fd | 317 | get_speed(); //update motor speed |
| protodriveev | 0:2dbd366be5fd | 318 | |
| protodriveev | 0:2dbd366be5fd | 319 | #ifdef CURRENT_SENSOR_ON |
| protodriveev | 0:2dbd366be5fd | 320 | get_current(); //update current |
| protodriveev | 0:2dbd366be5fd | 321 | #endif |
| protodriveev | 0:2dbd366be5fd | 322 | |
| protodriveev | 0:2dbd366be5fd | 323 | #ifdef LIGHTS_LOAD_CONTROL //controls the board LEDs based on the value of the DUT and Load motor duty cycle |
| protodriveev | 0:2dbd366be5fd | 324 | //turn on green LEDs if in regen mode, and orange LEDs if in drive mode. |
| protodriveev | 0:2dbd366be5fd | 325 | if (DUT_input >= load_input && (DUT_input != 0 || load_input != 0)) {//check if not in regen mode |
| protodriveev | 0:2dbd366be5fd | 326 | regen = 0; //set regen flag low |
| protodriveev | 0:2dbd366be5fd | 327 | drive_LEDS(); //turn on orange LEDs |
| protodriveev | 0:2dbd366be5fd | 328 | } else if (DUT_input < load_input && (DUT_input != 0 || load_input != 0)) {//check if in regen mode |
| protodriveev | 0:2dbd366be5fd | 329 | regen = 1; //set regen flag high |
| protodriveev | 0:2dbd366be5fd | 330 | regen_LEDS(); //turn on green LEDs |
| protodriveev | 0:2dbd366be5fd | 331 | } else {// DUT is niether flowing into or out of DUT |
| protodriveev | 0:2dbd366be5fd | 332 | regen = 0; //set regen flag low |
| protodriveev | 0:2dbd366be5fd | 333 | all_LEDS_off();// LEDs off |
| protodriveev | 0:2dbd366be5fd | 334 | } |
| protodriveev | 0:2dbd366be5fd | 335 | #endif |
| protodriveev | 0:2dbd366be5fd | 336 | |
| protodriveev | 0:2dbd366be5fd | 337 | #ifdef LIGHTS_CURRENT_CONTROL //controls the board LEDs based on the value of the DUT and Load motor currents |
| protodriveev | 0:2dbd366be5fd | 338 | //turn on green LEDs if in regen mode, and orange LEDs if in drive mode. |
| protodriveev | 0:2dbd366be5fd | 339 | if (current > 0) { |
| protodriveev | 0:2dbd366be5fd | 340 | regen = 0; //set regen flag low |
| protodriveev | 0:2dbd366be5fd | 341 | drive_LEDS(); //turn on orange LEDs |
| protodriveev | 0:2dbd366be5fd | 342 | } else if (current < 0) { |
| protodriveev | 0:2dbd366be5fd | 343 | regen = 1; //set regen flag high |
| protodriveev | 0:2dbd366be5fd | 344 | regen_LEDS(); //turn on green LEDs |
| protodriveev | 0:2dbd366be5fd | 345 | } else { |
| protodriveev | 0:2dbd366be5fd | 346 | regen = 0; //set regen flag low |
| protodriveev | 0:2dbd366be5fd | 347 | all_LEDS_off(); // LEDs off |
| protodriveev | 0:2dbd366be5fd | 348 | } |
| protodriveev | 0:2dbd366be5fd | 349 | #endif |
| protodriveev | 0:2dbd366be5fd | 350 | |
| protodriveev | 0:2dbd366be5fd | 351 | #ifdef DUT_BATT_CAP_ENABLE |
| protodriveev | 0:2dbd366be5fd | 352 | superCapControl(); |
| protodriveev | 0:2dbd366be5fd | 353 | #endif |
| protodriveev | 0:2dbd366be5fd | 354 | } |
| protodriveev | 0:2dbd366be5fd | 355 | |
| protodriveev | 0:2dbd366be5fd | 356 | // reset timer if 1000 seconds have elapsed |
| protodriveev | 0:2dbd366be5fd | 357 | if (timer.read() >= 1000) { |
| protodriveev | 0:2dbd366be5fd | 358 | timer.reset(); |
| protodriveev | 0:2dbd366be5fd | 359 | slow_timer = 0; |
| protodriveev | 0:2dbd366be5fd | 360 | fast_timer = 0; |
| protodriveev | 0:2dbd366be5fd | 361 | } |
| protodriveev | 0:2dbd366be5fd | 362 | } |
| protodriveev | 0:2dbd366be5fd | 363 | //turn off motors |
| protodriveev | 0:2dbd366be5fd | 364 | load_motor =0; |
| protodriveev | 0:2dbd366be5fd | 365 | DUT_motor = 0; |
| protodriveev | 0:2dbd366be5fd | 366 | timer.stop(); |
| protodriveev | 0:2dbd366be5fd | 367 | timer.reset(); |
| protodriveev | 0:2dbd366be5fd | 368 | } |
| protodriveev | 0:2dbd366be5fd | 369 | |
| protodriveev | 0:2dbd366be5fd | 370 | } |