most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown

Dependencies:   mbed MODSERIAL FATFileSystem

Committer:
tnhnrl
Date:
Mon Jun 25 15:44:00 2018 +0000
Revision:
67:c86a4b464682
Parent:
66:0f20870117b7
Child:
68:8f549749b8ce
fix timing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
danstrider 10:085ab7328054 1 /*
tnhnrl 58:94b7fd55185e 2 Modified FSG PCB V_1_1
tnhnrl 52:f207567d3ea4 3 - Freezes when doing a dive or any timed sequence (commented out SD card references)
tnhnrl 52:f207567d3ea4 4 - commented out sdLogger().appendLogFile(current_state, 0); //open SD file once
tnhnrl 52:f207567d3ea4 5 - commented out sdLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 52:f207567d3ea4 6 - commented out sdLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 52:f207567d3ea4 7 - reduced timer to 20 seconds for bench testing
tnhnrl 52:f207567d3ea4 8 - modified ConfigFileIO for rudder()
tnhnrl 52:f207567d3ea4 9 - added in getFloatUserInput function from newer code
tnhnrl 53:c0586fe62b01 10 - changed LinearActuator & batt() in StaticDefs to match new pinouts from Bob/Matt/Troy fixes
tnhnrl 53:c0586fe62b01 11 - slowed down battery motor because it's silly fast at 30 volts (bench test)
tnhnrl 53:c0586fe62b01 12 * BCE gain is proportional 0.1 now 0.01
tnhnrl 53:c0586fe62b01 13 - BATT was moving in the wrong direction, gain was P: 0.10, I: 0.0, D: 0.0
tnhnrl 57:ec69651c8c21 14 * change gain to P: -0.10 (gain was flipped, I think the old circuit board had the voltages flipped ? ?)
tnhnrl 53:c0586fe62b01 15 - StateMachine changes for testing
tnhnrl 53:c0586fe62b01 16 * added keyboard_menu_STREAM_STATUS();
tnhnrl 53:c0586fe62b01 17 * added keyboard_menu_RUDDER_SERVO_settings();
tnhnrl 54:d4990fb68404 18 - modified the zero on the battery position from 610 to 836
tnhnrl 54:d4990fb68404 19 - BMM (batt) slope may be incorrect, seems low, currently 0.12176
tnhnrl 54:d4990fb68404 20 - modified the zero on BCE from 253 to 460
tnhnrl 54:d4990fb68404 21 - Pressure readings are wrong
tnhnrl 54:d4990fb68404 22 * added readADCCounts() to omegaPX209 class to see channel readings
tnhnrl 54:d4990fb68404 23 * modified omegaPX209 class to use filtered ADC readings from SpiADC.readCh4()
tnhnrl 54:d4990fb68404 24 - fixed rudderLoop to headingLoop from newer code
tnhnrl 58:94b7fd55185e 25 Modified FSG PCB V_1_2
tnhnrl 55:f4ec445c42fe 26 - added init headingLoop to main
tnhnrl 55:f4ec445c42fe 27 - added pitch and heading outputs to STREAM_STATUS
tnhnrl 54:d4990fb68404 28
tnhnrl 54:d4990fb68404 29 NOTE: Flipped motor controller output on connector side with battery mass mover (BMM)
tnhnrl 54:d4990fb68404 30 - Motor direction was opposite the BCE motor (because of gearing)
tnhnrl 54:d4990fb68404 31 - BMM P gain is now positive 0.02 (from -0.10)
tnhnrl 54:d4990fb68404 32
tnhnrl 58:94b7fd55185e 33 Modified FSG PCB V_1_3
tnhnrl 56:48a8a5a65b82 34 - added timing code for interrupt that drives the rudder (testing with o-scope)
tnhnrl 58:94b7fd55185e 35 - PID controller replaced with newer version from 5/29 code branch
tnhnrl 58:94b7fd55185e 36 - StateMachine hanged style of variables to match convention in code
tnhnrl 63:6cb0405fc6e6 37 Modified FSG PCB V_1_4
tnhnrl 67:c86a4b464682 38 - adc tests
tnhnrl 65:2ac186553959 39 Modified FSG PCB V_1_5
tnhnrl 65:2ac186553959 40 - IMU update
tnhnrl 67:c86a4b464682 41 - Testing print outs
tnhnrl 67:c86a4b464682 42 Modified FSG PCB V_1_6
tnhnrl 67:c86a4b464682 43 - new BMM zero count of 240 (confirmed manually)
tnhnrl 67:c86a4b464682 44 - new BCE zero count of 400 (confirmed manually)
tnhnrl 67:c86a4b464682 45 - Modified emergency climb to go to position 10 on the BMM, not zero (almost relying on the limit switch)
tnhnrl 67:c86a4b464682 46 - fixed keyboard input; including typing in the timeout (can enter exact times)
tnhnrl 67:c86a4b464682 47 Modified FSG PCB V_1_7
tnhnrl 67:c86a4b464682 48 - removed data logger references
tnhnrl 67:c86a4b464682 49
danstrider 10:085ab7328054 50 */
tnhnrl 65:2ac186553959 51
tnhnrl 65:2ac186553959 52 /* removed unused variables */
tzyoung 0:ea293bbf9717 53 #include "mbed.h"
tzyoung 0:ea293bbf9717 54 #include "StaticDefs.hpp"
tnhnrl 65:2ac186553959 55
tnhnrl 65:2ac186553959 56 #ifndef lround
tnhnrl 65:2ac186553959 57 #define lround(var) (long)(var+0.5f)
tnhnrl 65:2ac186553959 58 #endif
tnhnrl 65:2ac186553959 59
tnhnrl 65:2ac186553959 60 ////////////////////////////////////////////////////////////////// NEW TICKER
tnhnrl 65:2ac186553959 61 Ticker systemTicker;
tnhnrl 65:2ac186553959 62 bool setup_complete = false;
tnhnrl 65:2ac186553959 63 volatile unsigned int bTick = 0;
tnhnrl 65:2ac186553959 64 volatile unsigned int timer_counter = 0;
tnhnrl 65:2ac186553959 65
tnhnrl 65:2ac186553959 66 char hex[9];
tnhnrl 65:2ac186553959 67
tnhnrl 65:2ac186553959 68 char * conversion(float input_float) {
tnhnrl 65:2ac186553959 69 int integer_number = lround(100.0 * input_float); //convert floating point input to integer to broadcast over serial (reduce precision)
tnhnrl 65:2ac186553959 70
tnhnrl 65:2ac186553959 71 memset(hex, 0, sizeof(hex) ); // void* memset( void* dest, int ch, size_t count ); // CLEAR IT (need null at end of string)
tnhnrl 65:2ac186553959 72 sprintf(hex, "%8x", integer_number); //generates spaces 0x20
tnhnrl 65:2ac186553959 73 return hex;
tnhnrl 65:2ac186553959 74 }
tnhnrl 65:2ac186553959 75
tnhnrl 65:2ac186553959 76 static unsigned int read_ticker(void) { //Basically this makes sure you're reading the data at one instance (not while it's changing)
tnhnrl 65:2ac186553959 77 unsigned int val = bTick;
tnhnrl 65:2ac186553959 78 if( val )
tnhnrl 65:2ac186553959 79 bTick = 0;
tnhnrl 65:2ac186553959 80 return( val );
tnhnrl 65:2ac186553959 81 }
tnhnrl 65:2ac186553959 82 ////////////////////////////////////////////////////////////////// NEW TICKER
tnhnrl 20:8987a9ae2bc7 83
tnhnrl 32:f2f8ae34aadc 84 // loop rate used to determine how fast events trigger in the while loop
tnhnrl 65:2ac186553959 85 //Ticker main_loop_rate_ticker;
tnhnrl 65:2ac186553959 86 //Ticker log_loop_rate_ticker;
tnhnrl 32:f2f8ae34aadc 87
tnhnrl 65:2ac186553959 88 volatile bool fsm_loop = false; //used so the compiler does not optimize this variable (load from memory, do not assume state of variable)
tnhnrl 34:9b66c5188051 89 volatile bool log_loop = false; //used so the compiler does not optimize this variable (load from memory, do not assume state of variable)
tnhnrl 32:f2f8ae34aadc 90
tnhnrl 65:2ac186553959 91 void loop_trigger() { fsm_loop = true;} // loop trigger (used in while loop)
tnhnrl 34:9b66c5188051 92 void log_loop_trigger() { log_loop = true;} // log loop trigger (used in while loop)
tnhnrl 32:f2f8ae34aadc 93
tnhnrl 65:2ac186553959 94 static int current_state = 0;
tnhnrl 65:2ac186553959 95 static bool file_opened = false;
tnhnrl 65:2ac186553959 96
tnhnrl 65:2ac186553959 97 void FSM() { // FSM loop runs at 100 hz
tnhnrl 65:2ac186553959 98 if(fsm_loop) {
tnhnrl 65:2ac186553959 99 fsm_loop = false; // wait until the loop rate timer fires again
tnhnrl 65:2ac186553959 100 current_state = stateMachine().runStateMachine(); //running State Machine. Returns 0 if sitting idle or keyboard press (SIT_IDLE state).
tnhnrl 65:2ac186553959 101 }
tnhnrl 65:2ac186553959 102 }
tnhnrl 65:2ac186553959 103
tnhnrl 65:2ac186553959 104 void log_function() {
tnhnrl 65:2ac186553959 105 // log loop runs at 1 hz
tnhnrl 65:2ac186553959 106 if (log_loop) {
tnhnrl 65:2ac186553959 107 //when the state machine is not in SIT_IDLE state (or a random keyboard press)
tnhnrl 65:2ac186553959 108
tnhnrl 65:2ac186553959 109 if (current_state == TRANSMIT_MBED_LOG or current_state == RECEIVE_SEQUENCE) {
tnhnrl 65:2ac186553959 110 ; //pass
tnhnrl 65:2ac186553959 111 }
tnhnrl 65:2ac186553959 112
tnhnrl 65:2ac186553959 113 else if(current_state != 0) {
tnhnrl 65:2ac186553959 114 if (!file_opened) { //if the log file is not open, open it
tnhnrl 65:2ac186553959 115 mbedLogger().appendLogFile(current_state, 0); //open MBED file once
tnhnrl 65:2ac186553959 116 //sdLogger().appendLogFile(current_state, 0); //open SD file once
tnhnrl 65:2ac186553959 117
tnhnrl 65:2ac186553959 118 file_opened = true; //stops it from continuing to open it
tnhnrl 56:48a8a5a65b82 119
tnhnrl 65:2ac186553959 120 pc().printf(">>>>>>>> Recording. Log file opened. <<<<<<<<\n\r");
tnhnrl 65:2ac186553959 121 }
tnhnrl 65:2ac186553959 122
tnhnrl 65:2ac186553959 123 //record to Mbed file system
tnhnrl 65:2ac186553959 124 led4() = !led4();
tnhnrl 65:2ac186553959 125
tnhnrl 65:2ac186553959 126 mbedLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 65:2ac186553959 127 //sdLogger().appendLogFile(current_state, 1); //writing data
tnhnrl 65:2ac186553959 128 }
tnhnrl 65:2ac186553959 129
tnhnrl 65:2ac186553959 130 //when the current FSM state is zero (SIT_IDLE), close the file
tnhnrl 65:2ac186553959 131 else {
tnhnrl 65:2ac186553959 132 //this can only happen once
tnhnrl 65:2ac186553959 133 if (file_opened) {
tnhnrl 67:c86a4b464682 134 //WRITE ONCE
tnhnrl 67:c86a4b464682 135 mbedLogger().appendLogFile(current_state, 1); //write the idle state, then close
tnhnrl 67:c86a4b464682 136
tnhnrl 65:2ac186553959 137 mbedLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 65:2ac186553959 138 //sdLogger().appendLogFile(current_state, 0); //close log file
tnhnrl 65:2ac186553959 139
tnhnrl 65:2ac186553959 140 file_opened = false;
tnhnrl 65:2ac186553959 141
tnhnrl 65:2ac186553959 142 pc().printf(">>>>>>>> Stopped recording. Log file closed. <<<<<<<<\n\r");
tnhnrl 65:2ac186553959 143 }
tnhnrl 65:2ac186553959 144 }
tnhnrl 65:2ac186553959 145 } //END OF LOG LOOP
tnhnrl 65:2ac186553959 146
tnhnrl 65:2ac186553959 147 log_loop = false; // wait until the loop rate timer fires again
tnhnrl 65:2ac186553959 148 }
tnhnrl 56:48a8a5a65b82 149
tnhnrl 65:2ac186553959 150 static void system_timer(void) {
tnhnrl 65:2ac186553959 151 bTick = 1;
tnhnrl 65:2ac186553959 152
tnhnrl 56:48a8a5a65b82 153 timer_counter++;
tnhnrl 56:48a8a5a65b82 154
tnhnrl 56:48a8a5a65b82 155 //only start these updates when everything is properly setup (through setup function)
tnhnrl 56:48a8a5a65b82 156 if (setup_complete) {
tnhnrl 65:2ac186553959 157 if ( (timer_counter % 1) == 0) { //this runs at 0.001 second intervals (1000 Hz)
tnhnrl 65:2ac186553959 158 adc().update(); //every iteration of this the A/D converter runs //now this runs at 0.01 second intervals 03/12/2018
tnhnrl 65:2ac186553959 159 }
tnhnrl 65:2ac186553959 160
tnhnrl 65:2ac186553959 161 if ( (timer_counter % 10) == 0) {
tnhnrl 65:2ac186553959 162 bce().update(); //update() inside LinearActuator class (running at 0.01 second intervals)
tnhnrl 65:2ac186553959 163 batt().update();
tnhnrl 65:2ac186553959 164 led2() = !led2();
tnhnrl 65:2ac186553959 165 }
tnhnrl 65:2ac186553959 166
tnhnrl 65:2ac186553959 167 if ( (timer_counter % 20) == 0 ) { // 0.02 second intervals
tnhnrl 65:2ac186553959 168 rudder().runServo();
tnhnrl 65:2ac186553959 169 }
tnhnrl 65:2ac186553959 170
tnhnrl 65:2ac186553959 171 if ( (timer_counter % 50) == 0 ) { // 0.05 second intervals
tnhnrl 67:c86a4b464682 172 imu().runIMU();
tnhnrl 65:2ac186553959 173 }
tnhnrl 65:2ac186553959 174
tnhnrl 65:2ac186553959 175 if ( (timer_counter % 100) == 0) { // 100,000 microseconds = 0.1 second intervals
tnhnrl 65:2ac186553959 176 depthLoop().runOuterLoop();
tnhnrl 65:2ac186553959 177 pitchLoop().runOuterLoop();
tnhnrl 65:2ac186553959 178 headingLoop().runOuterLoop();
tnhnrl 65:2ac186553959 179 }
tnhnrl 65:2ac186553959 180
tnhnrl 65:2ac186553959 181 if ( (timer_counter % 500) == 0) { // 500,000 microseconds = 0.5 second intervals
tnhnrl 65:2ac186553959 182 //serialComms().getDepthPitchHeading();
tnhnrl 65:2ac186553959 183 log_loop = true;
tnhnrl 65:2ac186553959 184 log_function();
tnhnrl 65:2ac186553959 185 }
tnhnrl 56:48a8a5a65b82 186 }
tnhnrl 56:48a8a5a65b82 187 }
tnhnrl 56:48a8a5a65b82 188
danstrider 10:085ab7328054 189 void setup() {
danstrider 11:3b241ecb75ed 190 pc().baud(57600);
tnhnrl 66:0f20870117b7 191 pc().printf("\n\n\r FSG PCB Bench Test V1.5)\n\n\r");
tnhnrl 65:2ac186553959 192
danstrider 10:085ab7328054 193 // start up the system timer
tnhnrl 65:2ac186553959 194 //systemTimer().start();
tnhnrl 20:8987a9ae2bc7 195
danstrider 10:085ab7328054 196 // set up and start the adc. This runs on a fixed interval and is interrupt driven
tnhnrl 65:2ac186553959 197 adc().initialize();
tnhnrl 67:c86a4b464682 198 //one central interrupt is updating the ADC (not using the start function)
tnhnrl 65:2ac186553959 199
tnhnrl 65:2ac186553959 200 // setup and run the rudder(servo) pwm signal (start the ticker)
tnhnrl 65:2ac186553959 201 //rudder().init();
tnhnrl 65:2ac186553959 202 pc().printf("Rudder servo initialized!\n\r");
danstrider 10:085ab7328054 203
danstrider 10:085ab7328054 204 // set up and start the imu. This polls in the background
danstrider 10:085ab7328054 205 imu().initialize();
tnhnrl 65:2ac186553959 206 //imu().start();
danstrider 10:085ab7328054 207
tnhnrl 48:20e681885161 208 // construct the MBED local file system
danstrider 10:085ab7328054 209 local();
tnhnrl 48:20e681885161 210
tnhnrl 48:20e681885161 211 // construct the SD card file system
tnhnrl 65:2ac186553959 212 //sd_card();
tnhnrl 20:8987a9ae2bc7 213
danstrider 10:085ab7328054 214 // load config data from files
tnhnrl 65:2ac186553959 215 configFileIO().load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt"
tnhnrl 65:2ac186553959 216 configFileIO().load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt"
tnhnrl 65:2ac186553959 217
tnhnrl 65:2ac186553959 218 configFileIO().load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt" (contains neutral position)
tnhnrl 65:2ac186553959 219 configFileIO().load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt" (contains neutral position)
tnhnrl 65:2ac186553959 220
tnhnrl 65:2ac186553959 221 configFileIO().load_RUDDER_config(); // load the rudder servo inner loop parameters from the file "SERVO.txt"
tnhnrl 65:2ac186553959 222 configFileIO().load_HEADING_config(); // load the rudder servo outer loop HEADING control parameters from the file "HEADING.txt" (contains neutral position)
tnhnrl 43:891baf306e0a 223
danstrider 10:085ab7328054 224 // set up the linear actuators. adc has to be running first.
tnhnrl 65:2ac186553959 225 bce().setPIDHighLimit(bce().getTravelLimit()); //travel limit of this linear actuator
danstrider 10:085ab7328054 226 bce().init();
tnhnrl 65:2ac186553959 227 //bce().start(); //removed start, it's handled by the interrupt
tnhnrl 67:c86a4b464682 228 bce().runLinearActuator();
mkelly10 12:a0519d11d2b6 229 bce().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 230
tnhnrl 65:2ac186553959 231 batt().setPIDHighLimit(batt().getTravelLimit()); //travel limit of this linear actuator
danstrider 10:085ab7328054 232 batt().init();
tnhnrl 65:2ac186553959 233 batt().runLinearActuator(); // _init = true;
tnhnrl 65:2ac186553959 234 //batt().start();//removed start, it's handled by the interrupt
mkelly10 12:a0519d11d2b6 235 batt().pause(); // start by not moving
tnhnrl 20:8987a9ae2bc7 236
tnhnrl 65:2ac186553959 237 // set up the depth, pitch, and rudder outer loop controllers
danstrider 10:085ab7328054 238 depthLoop().init();
tnhnrl 65:2ac186553959 239 //removed start, it's handled by the interrupt
tnhnrl 16:3363b9f14913 240 depthLoop().setCommand(stateMachine().getDepthCommand());
tnhnrl 20:8987a9ae2bc7 241
danstrider 10:085ab7328054 242 pitchLoop().init();
tnhnrl 65:2ac186553959 243 //removed start, it's handled by the interrupt
tnhnrl 16:3363b9f14913 244 pitchLoop().setCommand(stateMachine().getPitchCommand());
tnhnrl 55:f4ec445c42fe 245
tnhnrl 55:f4ec445c42fe 246 headingLoop().init();
tnhnrl 65:2ac186553959 247 //removed start, it's handled by the interrupt
tnhnrl 65:2ac186553959 248 //headingLoop().setCommand(stateMachine().getHeadingCommand()); // FIX LATER
tnhnrl 65:2ac186553959 249 //heading flag that adjust the PID error is set in the constructor
tnhnrl 65:2ac186553959 250
tnhnrl 65:2ac186553959 251 //systemTicker.attach_us(&system_timer, 10000); // Interrupt timer running at 0.01 seconds (slower than original ADC time interval)
tnhnrl 65:2ac186553959 252
tnhnrl 65:2ac186553959 253
tnhnrl 20:8987a9ae2bc7 254
tnhnrl 20:8987a9ae2bc7 255 // show that the PID gains are loading from the file
tnhnrl 38:83d06c294807 256 pc().printf("bce P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
tnhnrl 38:83d06c294807 257 pc().printf("batt P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
tnhnrl 65:2ac186553959 258 pc().printf("rudder min pwm: %6.2f, max pwm: %6.2f, center pwm: %6.2f, min deg: %6.2f, max deg: %6.2f\r\n", rudder().getMinPWM(), rudder().getMaxPWM(), rudder().getCenterPWM(), rudder().getMinDeg(), rudder().getMaxDeg());
tnhnrl 65:2ac186553959 259
tnhnrl 65:2ac186553959 260 pc().printf("depth P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
tnhnrl 65:2ac186553959 261 pc().printf("pitch P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
tnhnrl 65:2ac186553959 262 pc().printf("heading P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f deg (deadband: %0.1f)\r\n", headingLoop().getControllerP(), headingLoop().getControllerI(), headingLoop().getControllerD(), headingLoop().getOutputOffset(), headingLoop().getDeadband());
tnhnrl 65:2ac186553959 263
danstrider 10:085ab7328054 264 pc().printf("\n\r");
tnhnrl 21:38c8544db6f4 265
tnhnrl 17:7c16b5671d0e 266 //load sequence from file
tnhnrl 17:7c16b5671d0e 267 sequenceController().loadSequence();
tnhnrl 32:f2f8ae34aadc 268
tnhnrl 47:fb3c7929d3f3 269 //set time of logger (to current or close-to-current time)
tnhnrl 65:2ac186553959 270 mbedLogger().setLogTime();
tnhnrl 65:2ac186553959 271 //sdLogger().setLogTime();
tnhnrl 47:fb3c7929d3f3 272
tnhnrl 47:fb3c7929d3f3 273 //create log files if not present on file system
tnhnrl 47:fb3c7929d3f3 274 mbedLogger().initializeLogFile();
tnhnrl 65:2ac186553959 275 //sdLogger().initializeLogFile();
tnhnrl 65:2ac186553959 276
tnhnrl 65:2ac186553959 277 setup_complete = true;
tnhnrl 67:c86a4b464682 278
tnhnrl 67:c86a4b464682 279 // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298
tnhnrl 67:c86a4b464682 280 wait(0.1); //giving this time to work
tnhnrl 67:c86a4b464682 281 depth().init();
tnhnrl 67:c86a4b464682 282 wait(0.1); //giving this time to work
tnhnrl 67:c86a4b464682 283 depth().tare(); //this did not work correctly before the ADC starts
tnhnrl 65:2ac186553959 284 }
tnhnrl 58:94b7fd55185e 285
danstrider 10:085ab7328054 286 int main() {
danstrider 10:085ab7328054 287 setup();
tnhnrl 56:48a8a5a65b82 288
tnhnrl 65:2ac186553959 289 unsigned int tNow = 0;
tnhnrl 65:2ac186553959 290
tnhnrl 65:2ac186553959 291 pc().baud(57600);
tnhnrl 67:c86a4b464682 292 pc().printf("\n\n\r FSG PCB v1.6 (XBee) 6/22/2018 \n\n\r");
tnhnrl 56:48a8a5a65b82 293
tnhnrl 56:48a8a5a65b82 294 systemTicker.attach_us(&system_timer, 1000); // Interrupt timer running at 0.001 seconds (slower than original ADC time interval)
tnhnrl 65:2ac186553959 295
tnhnrl 65:2ac186553959 296 while (1) {
tnhnrl 66:0f20870117b7 297 if( read_ticker() ) // read_ticker runs at the speed of 10 kHz (adc timing)
tnhnrl 65:2ac186553959 298 {
tnhnrl 65:2ac186553959 299 ++tNow;
mkelly10 51:c5c40272ecc3 300
tnhnrl 66:0f20870117b7 301 //run finite state machine fast when transmitting data
tnhnrl 66:0f20870117b7 302 if (current_state == TRANSMIT_MBED_LOG or current_state == RECEIVE_SEQUENCE) {
tnhnrl 66:0f20870117b7 303 if ( (tNow % 10) == 0 ) { // 0.001 second intervals (1000 Hz)
tnhnrl 66:0f20870117b7 304 fsm_loop = true;
tnhnrl 66:0f20870117b7 305 FSM();
tnhnrl 66:0f20870117b7 306 }
tnhnrl 45:16b8162188ca 307 }
tnhnrl 34:9b66c5188051 308
tnhnrl 67:c86a4b464682 309 //NOT TRANSMITTING DATA, NORMAL OPERATIONS
tnhnrl 66:0f20870117b7 310 else {
tnhnrl 66:0f20870117b7 311 if ( (tNow % 100) == 0 ) { // 0.1 second intervals (10 Hz)
tnhnrl 66:0f20870117b7 312 fsm_loop = true;
tnhnrl 66:0f20870117b7 313 FSM();
tnhnrl 66:0f20870117b7 314 }
tnhnrl 66:0f20870117b7 315
tnhnrl 66:0f20870117b7 316 if ( (tNow % 1000) == 0 ) { // 1.0 second intervals
tnhnrl 66:0f20870117b7 317 log_loop = true;
tnhnrl 66:0f20870117b7 318 log_function();
tnhnrl 67:c86a4b464682 319
tnhnrl 67:c86a4b464682 320 led1() = !led1();
tnhnrl 66:0f20870117b7 321 }
tnhnrl 66:0f20870117b7 322 }
tnhnrl 65:2ac186553959 323 }
danstrider 10:085ab7328054 324 }
danstrider 10:085ab7328054 325 }