Diff: main.cpp

Revision:

88:1813f583cee9

Parent:

87:6d95f853dab3

Child:

92:52a91656458a

--- a/main.cpp	Wed May 08 13:24:04 2019 +0000
+++ b/main.cpp	Thu May 09 14:26:40 2019 +0000
@@ -44,7 +44,7 @@
     if (log_loop) {  //start logloop8
         //when the state machine is not in SIT_IDLE state (or a random keyboard press)
         
-        if(current_state != 0) {  //first if  - not in sit_idle/keyboard state
+        if((current_state != SIT_IDLE)   &&  (current_state != FLYING_IDLE)) {  //first if  - not in sit_idle/keyboard state
             if (!file_opened) {                                 //if the log file is not open, open it
                 mbedLogger().appendLogFile(current_state, 1);   //open MBED file once
                 //sdLogger().appendLogFile(current_state, 0);     //open SD file once
@@ -243,6 +243,7 @@
     have_legfile = legController().loadLeg();  // this should be 1 if the legfile reader has found 1 or more legs
     
     current_state = legController().legStructLoaded[0].state;
+    begin_state = current_state;
     sprintf(buf, "in setup(): have_legfile = %d    current_state = %d (sit_idle= 0 or 1)\n", have_legfile, current_state);
     mbedLogger().appendDiagFile(buf,3);
     sprintf(buf, "in setup(): LEG_POSITION_DIVE = %d   START_SWIM = %d \n", LEG_POSITION_DIVE, START_SWIM);
@@ -306,7 +307,7 @@
 int main() {   
     setup();    //setup electronics/hardware
     // on landing, check orientation, if upside down, fix that first
-    systemTicker.attach_us(&system_timer, 1000);         // Interrupt timer running at 0.001 seconds       (slower than original ADC time interval)
+    // systemTicker.attach_us(&system_timer, 1000);         // Interrupt timer running at 0.001 seconds       (slower than original ADC time interval)
     led2()=1; led4()=0;
     unsigned int tNow = 0;
     int vernum=0;
@@ -315,6 +316,8 @@
     xbee().printf("\n\n\r 2018-08-14 FSG PCB XBee (setup complete) \n\n\r");
     sprintf(buf, "\n\n\r 2019-may-07 FSG PCB XBee line315  in main (setup complete) \n\n\r");
     mbedLogger().appendDiagFile(buf,3);
+    int fn_timeout = 240;
+    int save_timeout = 300;
     
     
     //tNow=5; sprintf(buf, "log file config values logfile= %s     diag file= %s\n", configFileIO().logFilesStruct.logFileName,
@@ -378,6 +381,33 @@
             //NOT TRANSMITTING DATA, NORMAL OPERATIONS
             else {  // **88**
                 //FSM
+                       // preamble to running the FSM
+                if (setval == 0 && (begin_state != FLYING_IDLE) && (current_state == START_SWIM  || current_state == LEG_POSITION_DIVE) && (fabs(imu().getRoll()) < 30)) {
+                    // These tests mean: - second start,plus already upright
+                    // from setup()  current_state = legController().legStructLoaded[0].state; either start_swim or leg_pos_dive
+                    save_state = current_state;
+                    neutral_via_leg =1;
+                    save_timeout = legController().legStructLoaded[0].timeout;
+                    fn_timeout = 240;
+                    current_state = FIND_NEUTRAL;  // does runStatemachine know about this value?
+                    sprintf(buf, "in main(): setval==0, but upright, so pre-empt start_swim or leg_pos_dive\n  set state directly  to find_neutral\n");
+                    mbedLogger().appendDiagFile(buf,3);
+                    stateMachine().setstate_frommain(FIND_NEUTRAL, fn_timeout);  // this will change state inside runstatemachine, change state val inthe right structure, first
+                }
+                if ( (neutral_via_leg == 1) && (setval == 1) && current_state == RISE) {  // usual endstate of FIND_NEUTRAL new state for after successful find neutral
+                    current_state = save_state;   //setval reset at line 366  when find_neutral succeeds
+                    stateMachine().setstate_frommain(current_state, save_timeout);   //back to what you were doing
+                    sprintf(buf, "in main(): presumably after find_neutral exits to RISE, but successful in setting setval\n");
+                    mbedLogger().appendDiagFile(buf,3);
+                    neutral_via_leg = 0;
+                }
+                if ( (neutral_via_leg == 1) && (setval == 0) && current_state == RISE) {  // failed endstate of FIND_NEUTRAL new state
+                    
+                    sprintf(buf, "in main(): presumably after find_neutral exits to RISE, UNSUCCESSFULLY\n");
+                    mbedLogger().appendDiagFile(buf,3);
+                    neutral_via_leg = 0;
+                }
+                      // end preamble
                 if ( (tNow % 100) == 0 ) {   // 0.1 second intervals
                     fsm_loop = true;
                     FSM();
@@ -447,5 +477,6 @@
     sprintf(buf, "wait 5 more seconds? out of the leg loop via FB_EXIT\n\n\r");
     mbedLogger().appendDiagFile(buf,0);
     led1()=0; led2()=0; led3()= 0 ;led4()=0;
+    configFileIO().save_FinalTime();   // saves last time before closing shop
     exit(0);
 }  // end main()
\ No newline at end of file