Diff: main.cpp

Revision:

87:6d95f853dab3

Parent:

86:ba3a118b0080

Child:

88:1813f583cee9

--- a/main.cpp	Thu May 02 20:34:16 2019 +0000
+++ b/main.cpp	Wed May 08 13:24:04 2019 +0000
@@ -22,7 +22,12 @@
 void log_loop_trigger() { log_loop = true;} // log loop trigger (used in while loop)
 
 static int current_state = 0; 
-static int have_legfile = 0;  
+static int have_legfile = 0; 
+
+static int save_state = 0;   // state to hold while find_neutral runs ( if necessary)
+       int begin_state = 0;
+static int neutral_via_leg = 0;  // flag for entry of find_neutral via leg_pos and setval==0
+static int setval = -1;    // has neutral been set, found in neutral.txt   
 
 static bool file_opened = false;
 
@@ -107,6 +112,7 @@
             depthLoop().runOuterLoop();
             pitchLoop().runOuterLoop();
             headingLoop().runOuterLoop();
+            altimLoop().runOuterLoop();
         }
         
         if ( (timer_counter % 1000) == 0) {        // update at 1.0 second intervals
@@ -155,7 +161,7 @@
     print_diag=1;
     configFileIO().load_LogVers_config(print_diag);    // Cnow print info to diag file
     mbedLogger().initializeDiagFile(print_diag);
-    
+        
     configFileIO().load_BCE_config();       // load the buoyancy engine parameters from the file "bce.txt"
     configFileIO().load_BATT_config();      // load the battery mass mover parameters from the file "batt.txt"
     
@@ -186,8 +192,24 @@
     pitchLoop().init();
     //removed start, it's handled by the interrupt
     pitchLoop().setCommand(stateMachine().getPitchCommand());
+    char buf[256];
+    sprintf(buf, "INIT  of Loop operators   headingLoop().init() is coming\n");
+    mbedLogger().appendDiagFile(buf,3);
+    headingLoop().init();  
+    altimLoop().init();   
+    sprintf(buf, "AltimLOOP.init() succeeded!  even with just default values - no starting file\n");
+    mbedLogger().appendDiagFile(buf,3);  
     
-    headingLoop().init();           
+    sprintf(buf, "in setup():  before load_setneutral_status setval = %d \n\r", setval);
+    mbedLogger().appendDiagFile(buf,3);
+            
+    configFileIO().load_setneutral_status();   // "neutral.txt" has flag for whether neutral has been set, and neutral values
+    sprintf(buf, "\n in setup():  load_setneutral_status succeeded:    setval = %d \n\r", configFileIO().neutralStruct.setval);
+    mbedLogger().appendDiagFile(buf,3);
+    setval = configFileIO().neutralStruct.setval;
+    sprintf(buf, "\n in setup():  after load_setneutral_status setval = %d \n\r", setval);
+    mbedLogger().appendDiagFile(buf,3);
+    
     //removed start, it's handled by the interrupt
     //headingLoop().setCommand(stateMachine().getHeadingCommand());         // FIX LATER
     //heading flag that adjust the PID error is set in the constructor
@@ -214,12 +236,19 @@
          
     //load sequence from file
     sequenceController().loadSequence();
-    char buf[256];
+    
     //xbee().printf("\n\n\r 2018-08-14 FSG PCB XBee (setup complete) \n\n\r");
-    sprintf(buf, "\n\n\r in setup():  starting legController().loadleg() \n\n\r");
+    sprintf(buf, " in setup():  starting legController().loadleg() \n\n\r");
     mbedLogger().appendDiagFile(buf,3);
     have_legfile = legController().loadLeg();  // this should be 1 if the legfile reader has found 1 or more legs
-    sprintf(buf, "Time is amystery, here is a message before the basic call\n");
+    
+    current_state = legController().legStructLoaded[0].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);
+    mbedLogger().appendDiagFile(buf,3);
+    
+    sprintf(buf, "Time is a mystery, here is a message before the basic call\n");
     mbedLogger().appendDiagFile(buf,3);
     int jj;
     long int kk;
@@ -284,13 +313,13 @@
     int diagnum=0;
      char buf[256];
     xbee().printf("\n\n\r 2018-08-14 FSG PCB XBee (setup complete) \n\n\r");
-    sprintf(buf, "\n\n\r 2019-feb-25 FSG PCB XBee line271main (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);
-    sprintf(buf, "finished setting up and will exit\n\n\r");
-    mbedLogger().appendDiagFile(buf,3);
+    
+    
     //tNow=5; sprintf(buf, "log file config values logfile= %s     diag file= %s\n", configFileIO().logFilesStruct.logFileName,
      //        configFileIO().logFilesStruct.diagFileName); tNow=0;
-    mbedLogger().appendDiagFile(buf,3);
+    //         mbedLogger().appendDiagFile(buf,3);
     vernum = configFileIO().logFilesStruct.logversion;
     diagnum = configFileIO().logFilesStruct.diagversion;
     sprintf(buf, "translated values LOG FILE VERSION number (vernum)=%d           diag file version number(diagnum) = %d\n", vernum, diagnum);
@@ -315,14 +344,23 @@
         if(have_legfile) {
             //install the leg variables in a structure, and set the state there.
             stateMachine().getLegParams(); //should set up everything with proper LEG_POSITION_DIVE state
-            sprintf(buf, "have_legfile succeeded \n\r");
+            sprintf(buf, "have_legfile succeeded  main: line 318 \n\r");
              mbedLogger().appendDiagFile(buf,3);
             }
+        
         if(!have_legfile) {
             sprintf(buf, "have_legfile failed!   .... will exit\n\n\r");
             mbedLogger().appendDiagFile(buf,3);
             keeprunning = 0;
             }
+        if(motorDisconnect().read() == 1) {
+            sprintf(buf, "motorDisconnect.read() = 1  surprising!\n\n\r");
+            mbedLogger().appendDiagFile(buf,3); led1()=1; led4()=1;
+            }
+        if(motorDisconnect().read() == 0) {
+            sprintf(buf, "motorDisconnect.read() = 0  I expected that\n\n\r");
+            mbedLogger().appendDiagFile(buf,3); led1()=1; led3()=1;
+            }
   
     while (keeprunning) {
         if( read_ticker() ) {                       // read_ticker runs at the speed of 10 kHz (adc timing)
@@ -351,16 +389,21 @@
                 if ( (tNow % 1000) == 0 ) {   // 1.0 second intervals
                     log_loop = true;
                     log_function();
-                    sprintf(buf, "hit 1 second  log interval in main loop tNow =%d  imu.roll = %f not-unsampled \n\n\r", tNow, imu().getRoll());  
-                    mbedLogger().appendDiagFile(buf,3);
-                    led3()=0; led2()=0;
+                    //sprintf(buf, "hit 1 second  log interval in main loop tNow =%d  imu.roll = %f not-unsampled \n\n\r", tNow, imu().getRoll());  
+                    //mbedLogger().appendDiagFile(buf,3);
+                    led3()=0; led2()=0; led1()=0; led4()=0;
                 }
-                if ( (tNow % 5000) == 0 ) {   // 5 second intervals
+                if ( (tNow % 2000) == 0 ) {   // 2 second intervals
                     
-                    led3()=1; led2()=1;
+                    led3()=1; 
+                    sprintf(buf, "hit 2 second  log interval in main loop current_state=%d tNow =%d  imu.roll = %f not-unsampled \n\n\r", current_state, tNow, imu().getRoll());  
+                    mbedLogger().appendDiagFile(buf,3);
                 }
+                if ((tNow % 3000) == 0) {
+                    led2()=1;
+                    }
                 //update the log and diagnostics files
-                if ( (tNow % 31000) == 0 ) {   // 1.0 hour intervals= 3600*1000  check for testing via 4 second intervals
+                if ( (tNow % 31000) == 0 ) {   // 1.0 hour intervals= 3600*1000  check for testing via 31 second intervals
                 sprintf(buf, "hit cycle seconds  replace_logfiles interval in main loop tNow =%d  \n\n\r", tNow);
                     mbedLogger().appendDiagFile(buf,3);
                     vernum = configFileIO().logFilesStruct.logversion;
@@ -376,21 +419,25 @@
                     // initialize new log and diagnostics files
                     log_loop = true;
                     log_function();
-                    led3()=1;
+                    led3()=1; led1()=1;
                 }
             } // end else { at  **88**
             if(current_state == FB_EXIT) {
                 log_loop=true;
-                log_function(); led2()=1;
+                log_function(); led2()=1; led4()=1;
                 keeprunning=0;  // and this will force exit to the main while() loop.
                 //  switch state to EOL_WAIT, spit out xbee messages,  wait 60 seconds or so. if keyboard comes back, do not exit
                 sprintf(buf, "INSIDE  main loop: BUT now called to exit out of it via FB_EXIT\n\n\r");
                 mbedLogger().appendDiagFile(buf,3);
             }
+                          if(tNow ==  90000) {  // don't wait forever  -remove this for real operations!!
+                              keeprunning=0;
+                              }
         }  // end if(read_ticker) {
     }  // end while(keeprunning)
+    
     mbedLogger().appendLogFile(current_state, 1);
-    sprintf(buf, "outside of main loop:  exiting out of the leg loop via FB_EXIT\n\n\r");
+    sprintf(buf, "outside of main loop:  exiting out of the leg loop via FB_EXIT  tNow= %d \n\n\r", tNow);
     mbedLogger().appendDiagFile(buf,3);
     led1()=1; led2() =1; led3()=1 ;led4() = 1;
     wait(5);