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
Diff: main.cpp
- Revision:
- 87:6d95f853dab3
- Parent:
- 86:ba3a118b0080
- Child:
- 88:1813f583cee9
diff -r ba3a118b0080 -r 6d95f853dab3 main.cpp --- 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);