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: StateMachine/StateMachine.cpp
- Revision:
- 32:f2f8ae34aadc
- Parent:
- 30:2964617e7676
- Child:
- 33:29a4268fbc74
- Child:
- 34:9b66c5188051
--- a/StateMachine/StateMachine.cpp Wed Dec 06 22:45:23 2017 +0000 +++ b/StateMachine/StateMachine.cpp Wed Dec 20 13:52:50 2017 +0000 @@ -2,15 +2,15 @@ #include "StaticDefs.hpp" StateMachine::StateMachine() { - _timeout = 480; // generic timeout for every state, seconds + _timeout = 20; // generic timeout for every state, seconds _pitchTolerance = 5.0; // pitch angle tolerance for FLOAT_LEVEL state _bceFloatPosition = bce().getTravelLimit(); // bce position for "float" states (max travel limit for BCE is 320 mm) _battFloatPosition = batt().getTravelLimit(); // batt position tail high for "broadcast" state (max travel limit for battery is 75 mm) - _depthCommand = 2.0; // user keyboard depth (default) - _pitchCommand = -20.0; // user keyboard pitch (default) + _depth_command = 2.0; // user keyboard depth (default) + _pitch_command = -20.0; // user keyboard pitch (default) _neutral_timer = 0; //timer used in FIND_NEUTRAL sub-FSM @@ -44,6 +44,14 @@ _max_recorded_depth_neutral = -99; //float to record max depth _max_recorded_depth_dive = -99; //float to record max depth + + _neutral_sink_command_mm = -2.5; //defaults for neutral finding sub-FSM + _neutral_rise_command_mm = 2.0; + _neutral_pitch_command_mm = 0.5; + + _max_recorded_auto_neutral_depth = -99; + + _file_closed = true; } //Finite State Machine (FSM) @@ -61,9 +69,12 @@ // what is active? bce().pause(); batt().pause(); - + //reset sub FSM _isSubStateTimerRunning = false; + + //close the MBED file + _file_closed = true; } // how exit? @@ -85,6 +96,16 @@ // what are the commands? bce().setPosition_mm(bce().getTravelLimit()); batt().setPosition_mm(0.0); + + //create the log file (works only if the file is closed) + createNewFile(); + + //show that this is the start of a new EMERGENCY_CLIMB sequence + mbedLogger().saveSequenceStringToFile("EMERGENCY_CLIMB"); + recordState(_state); + + //this was missing + _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again } // how exit? @@ -100,6 +121,10 @@ timer.reset(); _isTimeoutRunning = false; } + + //record data every 5 seconds + recordData(); + break; case FIND_NEUTRAL : @@ -125,7 +150,17 @@ _substate_array[_substate_array_counter] = NEUTRAL_SINKING; //save to state array _substate_array_counter++; - runNeutralStateMachine(); + runNeutralStateMachine(); + + //create the log file (works only if the file is closed) + createNewFile(); + + //show that this is the start of a new FIND_NEUTRAL sequence + mbedLogger().saveSequenceStringToFile("FIND_NEUTRAL"); + recordState(_state); + + //this was missing + _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again } // how exit? (exit with the timer, if timer still running continue processing sub FSM) @@ -148,10 +183,15 @@ _state = RISE; _isTimeoutRunning = false; } + + //record data every 5 seconds + recordData(); + break; case DIVE : // start local state timer and init any other one-shot actions + if (!_isTimeoutRunning) { pc().printf("\r\n\nstate: DIVE\r\n"); timer.reset(); // timer goes back to zero @@ -163,24 +203,39 @@ batt().unpause(); // what are the commands? - depthLoop().setCommand(_depthCommand); - pitchLoop().setCommand(_pitchCommand); - pc().printf("DIVE: depth cmd: %3.1f\r\n",depthLoop().getCommand()); - pc().printf("DIVE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand()); + depthLoop().setCommand(_depth_command); + pitchLoop().setCommand(_pitch_command); + + //variables to record every 1-4 seconds + _depth_command = depthLoop().getCommand(); + _pitch_command = pitchLoop().getCommand(); + + pc().printf("DIVE: depth cmd: %3.1f\r\n",_depth_command); + pc().printf("DIVE: pitch cmd: %3.1f\r\n",_pitch_command); //reset max dive depth _max_recorded_depth_dive = -99; //float to record max depth + + //create the log file (works only if the file is closed) + createNewFile(); + + //show that this is the start of new dive sequence + mbedLogger().saveSequenceStringToFile("DIVE"); + recordState(_state); + + //this was missing + _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again } // how exit? - if (timer > _timeout) { + if (timer.read() > _timeout) { pc().printf("DIVE: timed out\n\n\r"); _state = RISE; //new behavior 11/17/2017 timer.reset(); _isTimeoutRunning = false; } - else if (depthLoop().getPosition() > depthLoop().getCommand()) { - pc().printf("DIVE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand()); + else if (depthLoop().getPosition() > depthLoop().getCommand() - 0.5) { // including offset for low momentum approaches + pc().printf("DIVE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand()); _state = RISE; timer.reset(); _isTimeoutRunning = false; @@ -188,16 +243,21 @@ // what is active? pc().printf("DIVE: bce pos: %3.1f mm, batt pos: %3.1f mm (depth: %3.1f ft) (pitch: %3.1f deg)[%0.2f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read()); - bce().setPosition_mm(depthLoop().getOutput()); + bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors batt().setPosition_mm(pitchLoop().getOutput()); if (depthLoop().getPosition() > _max_recorded_depth_dive) { //debug _max_recorded_depth_dive = depthLoop().getPosition(); //new max depth recorded } + + //record data every 5 seconds + recordData(); + break; case RISE : // start local state timer and init any other one-shot actions + if (!_isTimeoutRunning) { pc().printf("\r\n\nstate: RISE\r\n"); timer.reset(); // timer goes back to zero @@ -210,20 +270,38 @@ // what are the commands? depthLoop().setCommand(-1.0); //make sure to get towards the surface (saw issues at LASR pool) - pitchLoop().setCommand(-_pitchCommand); - pc().printf("RISE: depth cmd: 0.0\r\n"); - pc().printf("RISE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand()); + pitchLoop().setCommand(-_pitch_command); + + //variables to record every 1-4 seconds + _depth_command = depthLoop().getCommand(); + _pitch_command = pitchLoop().getCommand(); + + pc().printf("RISE: depth cmd: %0.1f\r\n", _depth_command); + pc().printf("RISE: pitch cmd: %0.1f\r\n",_pitch_command); + + //create the log file (works only if the file is closed) + createNewFile(); + + //show that this is the start of new rise sequence + mbedLogger().saveSequenceStringToFile("RISE"); + recordState(_state); + + //this was missing + _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again } // how exit? - if (timer > _timeout) { + if (timer.read() > _timeout) { pc().printf("RISE: timed out\r\n"); _state = EMERGENCY_CLIMB; timer.reset(); _isTimeoutRunning = false; } - else if (depthLoop().getPosition() < 0.5) { //removed depthLoop().getCommand() - pc().printf("RISE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand()); + + //modified from (depthLoop().getPosition() < depthLoop().getCommand() + 0.5) + //did not work correctly in bench test (stuck in rise state) + else if (depthLoop().getPosition() < 0.5) { + pc().printf("RISE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand()); _state = FLOAT_BROADCAST; timer.reset(); _isTimeoutRunning = false; @@ -232,7 +310,11 @@ // what is active? pc().printf("RISE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read()); bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors - batt().setPosition_mm(pitchLoop().getOutput()); + batt().setPosition_mm(pitchLoop().getOutput()); + + //record data every 5 seconds + recordData(); + break; case FLOAT_LEVEL : @@ -250,6 +332,16 @@ // what are the commands? bce().setPosition_mm(_bceFloatPosition); pitchLoop().setCommand(0.0); + + //create the log file (works only if the file is closed) + createNewFile(); + + //show that this is the start of a new float level sequence + mbedLogger().saveSequenceStringToFile("FLOAT_LEVEL"); + recordState(_state); + + //this was missing + _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again } // how exit? @@ -269,6 +361,10 @@ // what is active? pc().printf("FL: pitchLoop output: %3.1f, batt pos: %3.1f, piston pos: %3.1f [%0.1f sec]\r", pitchLoop().getOutput(), batt().getPosition_mm(), bce().getPosition_mm(), timer.read()); batt().setPosition_mm(pitchLoop().getOutput()); + + //record data every 5 seconds + recordData(); + break; case FLOAT_BROADCAST : @@ -286,6 +382,16 @@ // what are the commands? bce().setPosition_mm(_bceFloatPosition); batt().setPosition_mm(_battFloatPosition); + + //create the log file (works only if the file is closed) + createNewFile(); + + //show that this is the start of a new float broadcast sequence + mbedLogger().saveSequenceStringToFile("BROADCAST"); + recordState(_state); + + //this was missing + _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again } // how exit? @@ -293,6 +399,10 @@ pc().printf("FB: timed out\r\n"); _state = SIT_IDLE; timer.reset(); + + //stop recording data + mbedLogger().closeFile(); + _isTimeoutRunning = false; } else if ( (fabs(bce().getPosition_mm() - bce().getSetPosition_mm()) < bce().getDeadband()) and @@ -300,11 +410,19 @@ pc().printf("FB: position: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",bce().getPosition_mm(), bce().getSetPosition_mm(), bce().getDeadband()); _state = SIT_IDLE; timer.reset(); + + //stop recording data + mbedLogger().closeFile(); + _isTimeoutRunning = false; } // what is active? - pc().printf("FB: bce pos: %0.1f mm, batt pos: %0.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec] (setPos batt: %0.1f bce: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read(), bce().getSetPosition_mm(),batt().getSetPosition_mm()); + pc().printf("FB: bce pos: %0.1f mm, batt pos: %0.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec] (CMD batt: %0.1f bce: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read(), bce().getSetPosition_mm(),batt().getSetPosition_mm()); + + //record data every 5 seconds + recordData(); + break; case MULTI_DIVE : @@ -320,13 +438,25 @@ batt().unpause(); //retrieve commands from structs (loaded from sequence.cfg file) - float sequence_depthCommand = currentStateStruct.depth; - float sequence_pitchCommand = currentStateStruct.pitch; + float sequence_depth_command = currentStateStruct.depth; + float sequence_pitch_command = currentStateStruct.pitch; // what are the commands? - depthLoop().setCommand(sequence_depthCommand); - pitchLoop().setCommand(sequence_pitchCommand); + depthLoop().setCommand(sequence_depth_command); + pitchLoop().setCommand(sequence_pitch_command); pc().printf("MULTI-DIVE: depth cmd: %3.1f ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand()); + + //create the log file (works only if the file is closed) + createNewFile(); + + //show that this is the start of a new MULTI_DIVE sequence + mbedLogger().saveSequenceStringToFile("MULTI_DIVE"); + recordState(_state); + + //no max depth recording right now + + //this was missing + _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again } // how exit? @@ -347,6 +477,10 @@ pc().printf("MULTI-DIVE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read()); bce().setPosition_mm(depthLoop().getOutput()); batt().setPosition_mm(pitchLoop().getOutput()); + + //record data every 5 seconds + recordData(); + break; case MULTI_RISE : @@ -366,12 +500,22 @@ stateMachine().getDiveSequence(); //retrieve just pitch command from struct - float sequence_pitchCommand = currentStateStruct.pitch; + float sequence_pitch_command = currentStateStruct.pitch; // what are the commands? (send back to 0.5 feet, not surface) // 11/21/2017 depthLoop().setCommand(0.5); - pitchLoop().setCommand(-sequence_pitchCommand); + pitchLoop().setCommand(-sequence_pitch_command); pc().printf("MULTI-RISE: depth cmd: 0.0 ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand()); + + //create the log file (works only if the file is closed) + createNewFile(); + + //show that this is the start of a new MULTI_DIVE sequence + mbedLogger().saveSequenceStringToFile("MULTI_RISE"); + recordState(_state); + + //this was missing + _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again } // how exit? @@ -413,7 +557,126 @@ pc().printf("MULTI-RISE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read()); bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors batt().setPosition_mm(pitchLoop().getOutput()); + + //record data every 5 seconds + recordData(); + break; + + case FIND_AUTO_NEUTRAL_DEPTH : + //RUN TO DEPTH, if this doesn't work, surface; if it does, continue to FIND_AUTO_NEUTRAL_PITCH + + // start local state timer and init any other one-shot actions + if (!_isTimeoutRunning) { + pc().printf("\r\n\nstate: FIND_AUTO_NEUTRAL_DEPTH\r\n"); + timer.reset(); // timer goes back to zero + timer.start(); // background timer starts running + _isTimeoutRunning = true; + + // what needs to be started? + bce().unpause(); + //BATTERY NOT MOVING + + // what are the commands? + depthLoop().setCommand(_depth_command); + pc().printf("FIND_AUTO_NEUTRAL_DEPTH: depth cmd: %3.1f\r\n",depthLoop().getCommand()); + + //reset max dive depth + _max_recorded_auto_neutral_depth = -99; //float to record max depth + + //this was missing + _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again + } + + // how exit? + if (timer > _timeout) { + pc().printf("FIND_AUTO_NEUTRAL_DEPTH: timed out (next state: EMERGENCY_CLIMB)\n\n\r"); + _state = EMERGENCY_CLIMB; + _isTimeoutRunning = false; + } + + //transition only when the PV settles and is at depth (then save) + else if (depthLoop().getPosition() > depthLoop().getCommand()) { + if (depthLoop().getVelocity() < 1.0) { //less than 1 foot/s + //if successful, save the data + _neutral_bce_pos_mm = bce().getPosition_mm(); //get the position of the battery motor + + pc().printf("FIND_AUTO_NEUTRAL_DEPTH: Saving neutral (depth) Buoyancy Engine position: %0.1f mm\n\n\r", _neutral_bce_pos_mm); + configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm); + + //transition to the next state + _state = FIND_AUTO_NEUTRAL_PITCH; + _isTimeoutRunning = false; + } + } + + // what is active? + pc().printf("FIND_AUTO_NEUTRAL_DEPTH: bce pos: %0.1f mm, batt pos: %0.1f mm (depth: %0.1f ft) (pitch: %0.1f deg)[%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read()); + bce().setPosition_mm(depthLoop().getOutput()); + //BATTERY NOT ACTIVE + + //record max depth for testing + if (depthLoop().getPosition() > _max_recorded_auto_neutral_depth) { //debug + _max_recorded_auto_neutral_depth = depthLoop().getPosition(); //new max depth recorded + } + + //record data every 5 seconds + recordData(); + + break; + + case FIND_AUTO_NEUTRAL_PITCH : + //RUN TO DEPTH, if this doesn't work, surface; if it does, continue to FIND_AUTO_NEUTRAL_PITCH + + // start local state timer and init any other one-shot actions + if (!_isTimeoutRunning) { + pc().printf("\r\n\nstate: FIND_AUTO_NEUTRAL_PITCH\r\n"); + timer.reset(); // timer goes back to zero + timer.start(); // background timer starts running + _isTimeoutRunning = true; + + // what needs to be started? + batt().unpause(); + bce().pause(); //BUOYANCY ENGINE NOT MOVING + + + // what are the commands? + pitchLoop().setCommand(0.0); + pc().printf("FIND_AUTO_NEUTRAL_PITCH: pitch cmd: %3.1f\r\n",pitchLoop().getCommand()); + + //this was missing + _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again + } + + // how exit? + if (timer > _timeout) { + pc().printf("FIND_AUTO_NEUTRAL_PITCH: timed out (next state: EMERGENCY_CLIMB)\n\n\r"); + _state = EMERGENCY_CLIMB; + _isTimeoutRunning = false; + } + + //transition only when the PV settles and is at depth (within 2 degrees and rate is less than 2 deg/s) (then save) + else if ((fabs(pitchLoop().getPosition()) < 2.0) && (pitchLoop().getVelocity() < 2.0)) { + //if successful, save the data + _neutral_batt_pos_mm = batt().getPosition_mm(); //get the position of the battery motor + + pc().printf("FIND_AUTO_NEUTRAL_PITCH: Saving neutral (pitch) battery position: %0.1f mm\n\n\r", _neutral_batt_pos_mm); + configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm); + + //transition to the next state + _state = RISE; + _isTimeoutRunning = false; + } + + // what is active? + pc().printf("FIND_AUTO_NEUTRAL_PITCH: bce pos: %0.1f mm, batt pos: %0.1f mm (depth: %0.1f ft) (pitch: %0.1f deg)[%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read()); + batt().setPosition_mm(pitchLoop().getOutput()); //trying to get to zero pitch + //BUOYANCY ENGINE NOT ACTIVE + + //record data every 5 seconds + recordData(); + + break; default : pc().printf("DEBUG: SIT_IDLE\n\r"); @@ -432,6 +695,8 @@ // output the keyboard menu for user's reference void StateMachine::showMenu() { pc().printf("\r\r\n\nKEYBOARD MENU:\r\r\n"); + pc().printf(" G to initiate auto-neutral sequence\r\n"); + pc().printf(" F to jump to auto-neutral FIND PITCH state\r\n"); pc().printf(" N to find neutral\r\n"); pc().printf(" M to initiate multi-dive cycle\r\n"); pc().printf(" D to initiate dive cycle\r\n"); @@ -442,10 +707,12 @@ //pc().printf(" H to run homing sequence on both BCE and Batt\r\n"); pc().printf(" T to tare the depth sensor\r\n"); pc().printf(" Z to show FSM and sub-FSM states.\r\n"); - pc().printf("[/] to change bce neutral position\r\n"); - pc().printf("</> to change batt neutral position\r\n"); - pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",_pitchCommand); - pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",_depthCommand); + pc().printf(" P to print the current log file.\r\n"); + pc().printf(" X to print the list of log files.\r\n"); + pc().printf("[/] to change bce neutral position: %0.1f\r\n", _neutral_bce_pos_mm); + pc().printf("</> to change batt neutral position: %0.1f\r\n", _neutral_batt_pos_mm); + pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",_pitch_command); + pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",_depth_command); pc().printf("+/- to decrease/increase timeout: %d s\r\n",_timeout); pc().printf(" 1 BCE PID sub-menu\r\n"); pc().printf(" 2 BATT PID sub-menu\r\n"); @@ -467,14 +734,11 @@ pc().printf("\r\n\nNEUTRAL_SINKING: Next retraction at %0.1f sec [current time: %0.1f] (pitch: %0.1f)\n\r", _neutral_timer, timer.read(), pitchLoop().getPosition()); - // what are the commands? - //move piston at start of sequence (retract 2.5 mm) - bce().setPosition_mm(bce().getSetPosition_mm() - 2.5); //no depth command + // what are the commands? (BCE linear actuator active, no pitch movement) + //move piston at start of sequence (default: retract 2.5 mm) + bce().setPosition_mm(bce().getSetPosition_mm() - _neutral_sink_command_mm); //no depth command - // it's okay to run the pitch outer loop now since we've already found pitch level in the previous state - pitchLoop().setCommand(0.0); - - pc().printf("NEUTRAL_SINKING: Retracting piston 5 mm [BCE CMD : %0.1f] [pitch cmd: %0.1f] (pitch: %0.1f)\n\r", bce().getSetPosition_mm(), pitchLoop().getCommand(), pitchLoop().getPosition()); + pc().printf("NEUTRAL_SINKING: Retracting piston %0.1f mm [BCE CMD : %0.1f] [pitch cmd: %0.1f] (pitch: %0.1f)\n\r", _neutral_sink_command_mm, bce().getSetPosition_mm(), pitchLoop().getCommand(), pitchLoop().getPosition()); _isSubStateTimerRunning = true; //disable this block after one iteration } @@ -487,7 +751,7 @@ _isSubStateTimerRunning = false; // reset the sub state timer } //once deeper than the commanded setpoint... - else if (depthLoop().getPosition() > _depthCommand) { + else if (depthLoop().getPosition() > _depth_command) { _substate = NEUTRAL_SLOWLY_RISE; // next state _isSubStateTimerRunning = false; //reset the sub state timer } @@ -495,13 +759,13 @@ // what is active? //once the 10 second timer is complete, reset the timeout so the state one-shot entry will move the setpoint if (timer.read() >= _neutral_timer) { - pc().printf("\r\n\n NEUTRAL_SINKING TIMER COMPLETE! Retracting BCE piston 5 mm [current time: %0.1f]\r\n", timer.read()); + pc().printf("\r\n\n NEUTRAL_SINKING TIMER COMPLETE! [current time: %0.1f]\r\n", timer.read()); _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again } - - // what is active? - batt().setPosition_mm(pitchLoop().getOutput()); // pitch outer loop is running + // what is active? (only the buoyancy engine moved every 5 seconds) + pc().printf("depthLoop getOutput (position): %0.1f (current depth: %0.1f ft)\r", depthLoop().getOutput(), depthLoop().getPosition()); //debug + bce().setPosition_mm(depthLoop().getOutput()); // (DID NOT WORK ON BENCH) break; case NEUTRAL_SLOWLY_RISE: @@ -511,13 +775,13 @@ pc().printf("\r\n\nNEUTRAL_SLOWLY_RISE: Next extension at %0.1f sec) [current time: %0.1f]\r\n",_neutral_timer,timer.read()); // what are the commands? - //move piston at start of sequence (extend) - bce().setPosition_mm(bce().getSetPosition_mm() + 2); //no depth command + //move piston at start of sequence (default: extend 2.0 mm) + bce().setPosition_mm(bce().getSetPosition_mm() + _neutral_rise_command_mm); //no depth command // it's okay to run the pitch outer loop now since we've already found pitch level in the previous state pitchLoop().setCommand(0.0); - pc().printf("NEUTRAL_SLOWLY_RISE: Extending BCE piston 5 mm [BCE CMD : %0.1f] [pitch cmd: %0.1f]\n\r", bce().getSetPosition_mm(), pitchLoop().getCommand()); + pc().printf("NEUTRAL_SLOWLY_RISE: Extending BCE piston %0.1f mm [BCE CMD : %0.1f] [pitch cmd: %0.1f]\n\r", _neutral_rise_command_mm, bce().getSetPosition_mm(), pitchLoop().getCommand()); _isSubStateTimerRunning = true; //disable this block after one iteration } @@ -538,31 +802,31 @@ // what is active? //once 5 second timer complete, reset the timeout so the state one-shot entry will move the setpoint if (timer.read() >= _neutral_timer) { - pc().printf("\r\n\n NEUTRAL_SLOWLY_RISE TIMER COMPLETE! Extending 1 mm [timer: %0.1f]\r\n", timer.read()); + pc().printf("\r\n\n NEUTRAL_SLOWLY_RISE TIMER COMPLETE! [timer: %0.1f]\r\n", timer.read()); _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again } - // what is active? - batt().setPosition_mm(pitchLoop().getOutput()); // pitch outer loop is running + // what is active? (only the buoyancy engine moved every 5 seconds) + pc().printf("depthLoop getOutput: %0.1f\r", depthLoop().getOutput()); //debug + bce().setPosition_mm(depthLoop().getOutput()); // (DID NOT WORK ON BENCH) break; case NEUTRAL_CHECK_PITCH : // fall thru to next state is desired - case NEUTRAL_FIRST_PITCH : // start local state timer and init any other one-shot actions if (!_isSubStateTimerRunning) { _neutral_timer = timer.read() + 10; // record time when this block is entered and add several seconds pc().printf("\r\nNEUTRAL_CHECK_PITCH: Next move in %0.1f sec \r\n",_neutral_timer - timer.read()); - // what are the commands? + // what are the commands? (default: retract or extend 0.5 mm) if (pitchLoop().getPosition() > 2) { // nose is high - batt().setPosition_mm(batt().getSetPosition_mm() + 0.5); // move battery forward (using setpoint from linear actuator) - pc().printf("\n\rNeutral Check Pitch: moving battery FWD in 1mm increments\n\n\r"); + batt().setPosition_mm(batt().getSetPosition_mm() + _neutral_pitch_command_mm); // move battery forward (using setpoint from linear actuator) + pc().printf("\n\rNeutral Check Pitch: moving battery FWD in %0.1f mm increments\n\n\r", _neutral_pitch_command_mm); } else if (pitchLoop().getPosition() < -2) { // nose is low - batt().setPosition_mm(batt().getSetPosition_mm() - 0.5); // move battery aft (using setpoint from linear actuator) - pc().printf("\n\rNeutral Check Pitch: moving battery AFT in 1mm increments\n\n\r"); + batt().setPosition_mm(batt().getSetPosition_mm() - _neutral_pitch_command_mm); // move battery aft (using setpoint from linear actuator) + pc().printf("\n\rNeutral Check Pitch: moving battery AFT in %0.1f mm increments\n\n\r", _neutral_pitch_command_mm); } _isSubStateTimerRunning = true; //disable this block after one iteration @@ -579,18 +843,8 @@ _max_recorded_depth_neutral = depthLoop().getPosition(); //new max depth recorded } - if (_substate == NEUTRAL_FIRST_PITCH) { - _substate = NEUTRAL_SINKING; // next state starts the sinking - _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again - - // save this neutral (not in file) for pitch - pitchLoop().setOutputOffset(batt().getPosition_mm()); - - pc().printf("substate: NEUTRAL_FIRST_PITCH (next substate: NEUTRAL_SINKING)\n\r"); - } - // found level and at depth too, so save it all now - else if (_substate == NEUTRAL_CHECK_PITCH) { + if (_substate == NEUTRAL_CHECK_PITCH) { //save positions locally _neutral_batt_pos_mm = batt().getPosition_mm(); _neutral_bce_pos_mm = bce().getPosition_mm(); @@ -603,7 +857,7 @@ configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm); //P,I,D,batt zeroOffset configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm); //P,I,D, bce zeroOffset - pc().printf("\n\rSaving Positions: BCE: %0.1f mm, BATT: %0.1f\n\n\r",_neutral_bce_pos_mm,_neutral_batt_pos_mm); + pc().printf("\n\n\r>>> Saving Positions: BCE: %0.1f mm, BATT: %0.1f <<<\n\n\r",_neutral_bce_pos_mm,_neutral_batt_pos_mm); _substate = NEUTRAL_EXIT; _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again @@ -667,6 +921,160 @@ return _substate; // message to calling function of what sub-state it's in } } + +////Find Neutral sub finite state machine +//// Note: the sub-fsm only moves the pistons once at the start of each timer loop +//// (timer completes, move piston, timer completes, move piston, etc) +//int StateMachine::runActiveNeutralStateMachine() { +// switch (_substate) { +// case NEUTRAL_AUTO_DEPTH : +// //start the 10 second timer +// if (!_isSubStateTimerRunning) { +// _neutral_timer = timer.read() + 5; //record the time when this block is first entered and add 5 seconds +// +// pc().printf("\r\n\nNEUTRAL_SINKING: Next retraction at %0.1f sec [current time: %0.1f] (pitch: %0.1f)\n\r", _neutral_timer, timer.read(), pitchLoop().getPosition()); +// +// // what are the commands? (BCE linear actuator active, no pitch movement) +// //move piston at start of sequence (default: retract 2.5 mm) +// bce().setPosition_mm(bce().getSetPosition_mm() - _neutral_sink_command_mm); //no depth command +// +// pc().printf("NEUTRAL_SINKING: Retracting piston %0.1f mm [BCE CMD : %0.1f] [pitch cmd: %0.1f] (pitch: %0.1f)\n\r", _neutral_sink_command_mm, bce().getSetPosition_mm(), pitchLoop().getCommand(), pitchLoop().getPosition()); +// +// _isSubStateTimerRunning = true; //disable this block after one iteration +// } +// +// // how exit? +// //once reached the travel limit, no need to keep trying, so exit +// if (bce().getPosition_mm() <= 0) { +// pc().printf("\n\rDEBUG: BCE current position is %0.1f mm (NEXT SUBSTATE NEUTRAL EXIT)\n\r", bce().getPosition_mm()); +// _substate = NEUTRAL_EXIT; +// _isSubStateTimerRunning = false; // reset the sub state timer +// } +// //once deeper than the commanded setpoint... +// else if (depthLoop().getPosition() > _depth_command) { +// _substate = NEUTRAL_SLOWLY_RISE; // next state +// _isSubStateTimerRunning = false; //reset the sub state timer +// } +// +// // what is active? +// //once the 10 second timer is complete, reset the timeout so the state one-shot entry will move the setpoint +// if (timer.read() >= _neutral_timer) { +// pc().printf("\r\n\n NEUTRAL_SINKING TIMER COMPLETE! [current time: %0.1f]\r\n", timer.read()); +// +// _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again +// } +// // what is active? (only the buoyancy engine moved every 5 seconds) +// break; +// +// case NEUTRAL_CHECK_PITCH : // fall thru to next state is desired +// // start local state timer and init any other one-shot actions +// +// if (!_isSubStateTimerRunning) { +// _neutral_timer = timer.read() + 10; // record time when this block is entered and add several seconds +// pc().printf("\r\nNEUTRAL_CHECK_PITCH: Next move in %0.1f sec \r\n",_neutral_timer - timer.read()); +// +// // what are the commands? (default: retract or extend 0.5 mm) +// if (pitchLoop().getPosition() > 2) { // nose is high +// batt().setPosition_mm(batt().getSetPosition_mm() + _neutral_pitch_command_mm); // move battery forward (using setpoint from linear actuator) +// pc().printf("\n\rNeutral Check Pitch: moving battery FWD in %0.1f mm increments\n\n\r", _neutral_pitch_command_mm); +// } +// else if (pitchLoop().getPosition() < -2) { // nose is low +// batt().setPosition_mm(batt().getSetPosition_mm() - _neutral_pitch_command_mm); // move battery aft (using setpoint from linear actuator) +// pc().printf("\n\rNeutral Check Pitch: moving battery AFT in %0.1f mm increments\n\n\r", _neutral_pitch_command_mm); +// } +// +// _isSubStateTimerRunning = true; //disable this block after one iteration +// } +// +// // how exit? +// //pitch angle and pitch rate within small tolerance +// //benchtop tests confirm angle needs to be around 2 degrees +// if ((fabs(pitchLoop().getPosition()) < 2.0) and (fabs(pitchLoop().getVelocity()) < 5.0)) { +// pc().printf("Debug: Found Level (NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH)\n\r"); //debug +// // found level, but don't need to save anything this time +// +// if (depthLoop().getPosition() > _max_recorded_depth_neutral) { //debug +// _max_recorded_depth_neutral = depthLoop().getPosition(); //new max depth recorded +// } +// +// // found level and at depth too, so save it all now +// if (_substate == NEUTRAL_CHECK_PITCH) { +// //save positions locally +// _neutral_batt_pos_mm = batt().getPosition_mm(); +// _neutral_bce_pos_mm = bce().getPosition_mm(); +// +// //set the neutral positions in each outer loop +// depthLoop().setOutputOffset(_neutral_bce_pos_mm); +// pitchLoop().setOutputOffset(_neutral_batt_pos_mm); +// +// // save into the depth.txt and pitch.txt files +// configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm); //P,I,D,batt zeroOffset +// configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm); //P,I,D, bce zeroOffset +// +// pc().printf("\n\rSaving Positions: BCE: %0.1f mm, BATT: %0.1f\n\n\r",_neutral_bce_pos_mm,_neutral_batt_pos_mm); +// +// _substate = NEUTRAL_EXIT; +// _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again +// } +// +// else { +// pc().printf("\n\rDid not find NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH, how did I get here?!\n\r"); +// _substate = NEUTRAL_EXIT; +// } +// } +// +// // what is active? +// //once timer complete, reset the timeout so the state one-shot entry will move the setpoint +// if (timer.read() >= _neutral_timer) { +// pc().printf("\r\n\nlevel timer COMPLETE!"); +// pc().printf("\r\n\n (BATT POS: %0.1f) moving 1 mm [timer: %0.1f]\r\n", batt().getPosition_mm(), timer.read()); +// _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again +// } +// break; +// +// //this state could be removed, it is only used as a transition but is needed to stop entering this function +// case NEUTRAL_EXIT : +// pc().printf("substate: NEUTRAL_EXIT\n\r"); +// break; +// +// default : +// pc().printf("how did we get to substate: default?\n\r"); //debug +// //a default within the sub-state machine +// _substate = NEUTRAL_EXIT; +// break; +// } +// +// // reset the sub-FSM if needed (useful if you need to redo the neutral-finding sequence) +// if (_substate == NEUTRAL_EXIT) { +// pc().printf("******************************** EXITING sub-FSM! *******************************\n\n\r"); +// +// //reset internal sub-state back to first entry conditions (first state is immediately sinking) +// _substate = NEUTRAL_SINKING; +// _isSubStateTimerRunning = false; // reset the sub state timer +// +// //record sub-states to view after sequence +// _substate_array[_substate_array_counter] = NEUTRAL_EXIT; //save exit to state array +// _substate_array_counter++; +// +// //reset _previous_substate on exit (has to be done in FIND_NEUTRAL if emergency exit) +// _previous_substate = -1; +// +// //NEUTRAL_EXIT state is used to tell the greater FSM that this sub-FSM has completed +// return NEUTRAL_EXIT; // message to calling function we just exited +// } +// else { +// //record sub-states to view after sequence (when changed) +// if (_previous_substate != _substate) { +// _substate_array[_substate_array_counter] = _substate; //save current state to state array +// _substate_array_counter++; +// +// //record the current substate for comparison +// _previous_substate = _substate; +// } +// +// return _substate; // message to calling function of what sub-state it's in +// } +//} // keyboard runs independently of the state machine, handling one key at a time //keyboard updates the desired _keyboard_state that is used in the state machine @@ -694,6 +1102,12 @@ if (userInput == 'D' or userInput == 'd') { _keyboard_state = DIVE; } + else if (userInput == 'G') { + _keyboard_state = FIND_AUTO_NEUTRAL_DEPTH; //new test 12/7/2017 + } + else if (userInput == 'F') { + _keyboard_state = FIND_AUTO_NEUTRAL_PITCH; //new test 12/7/2017 + } else if (userInput == 'N' or userInput == 'n') { _keyboard_state = FIND_NEUTRAL; } @@ -720,6 +1134,19 @@ else if (userInput == 'E' or userInput == 'e') { _keyboard_state = EMERGENCY_CLIMB; } + else if (userInput == 'P') { + //Print current SD card log file + printCurrentSdLog(); + mbedLogger().printFromLogFile(); //print the current log file to the screen + } + else if (userInput == 'X') { + printDirectory(); + //mbedLogger().printDirectory(); //print all log files to the screen + } + else if (userInput == 'C') { + //Transmit data (work in progress) + transmitData(); + } // else if (userInput == 'H' or userInput == 'h') { // pc().printf("running homing procedure\r\n"); // bce().unpause(); bce().homePiston(); bce().pause(); @@ -757,9 +1184,7 @@ string string_substate; for (int i = 0; i < _substate_array_counter; i++) { - if (_substate_array[i] == NEUTRAL_FIRST_PITCH) - string_substate = "NEUTRAL_FIRST_PITCH"; - else if (_substate_array[i] == NEUTRAL_SINKING) + if (_substate_array[i] == NEUTRAL_SINKING) string_substate = "NEUTRAL_SINKING"; else if (_substate_array[i] == NEUTRAL_SLOWLY_RISE) string_substate = "NEUTRAL_SLOWLY_RISE"; @@ -782,20 +1207,32 @@ } else if (userInput == '[' or userInput == '{') { - depthLoop().setOutputOffset(depthLoop().getOutputOffset() - 1); // decrease the bce neutral setpoint - pc().printf("Adjusting bce neutral position. new: %3.1f\r\n",depthLoop().getOutputOffset()); + _neutral_bce_pos_mm = depthLoop().getOutputOffset() - 1; + depthLoop().setOutputOffset(_neutral_bce_pos_mm); // decrease the bce neutral setpoint + pc().printf("Adjusting bce neutral position. new offset: %0.1f\r\n",depthLoop().getOutputOffset()); + // save neutral depth value to config file + configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm); } else if (userInput == ']' or userInput == '}') { - depthLoop().setOutputOffset(depthLoop().getOutputOffset() + 1); // increase the bce neutral setpoint - pc().printf("Adjusting bce neutral position. new: %3.1f\r\n",depthLoop().getOutputOffset()); + _neutral_bce_pos_mm = depthLoop().getOutputOffset() + 1; + depthLoop().setOutputOffset(_neutral_bce_pos_mm); // increase the bce neutral setpoint + pc().printf("Adjusting bce neutral position. new offset: %0.1f\r\n",depthLoop().getOutputOffset()); + // save neutral depth value to config file + configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm); } else if (userInput == '<' or userInput == ',') { - pitchLoop().setOutputOffset(pitchLoop().getOutputOffset() - 1); // decrease the batt neutral setpoint - pc().printf("Adjusting batt neutral position. new: %3.1f\r\n",pitchLoop().getOutputOffset()); + _neutral_batt_pos_mm = pitchLoop().getOutputOffset() - 1; + pitchLoop().setOutputOffset(_neutral_batt_pos_mm); // decrease the batt neutral setpoint + pc().printf("Adjusting batt neutral position. new offset: %0.1f\r\n",pitchLoop().getOutputOffset()); + // save neutral pitch value to config file + configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm); } else if (userInput == '>' or userInput == '.') { - pitchLoop().setOutputOffset(pitchLoop().getOutputOffset() + 1); // increase the batt neutral setpoint - pc().printf("Adjusting batt neutral position. new: %3.1f\r\n",pitchLoop().getOutputOffset()); + _neutral_batt_pos_mm = pitchLoop().getOutputOffset() + 1; + pitchLoop().setOutputOffset(_neutral_batt_pos_mm); // increase the batt neutral setpoint + pc().printf("Adjusting batt neutral position. new offset: %0.1f\r\n",pitchLoop().getOutputOffset()); + // save neutral pitch value to config file + configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm); } else if (userInput == '?') { @@ -806,23 +1243,23 @@ // change settings else if (userInput == 'Q' or userInput == 'q') { - _pitchCommand -= 0.5; //decrement the pitch setpoint - pitchLoop().setCommand(_pitchCommand); + _pitch_command -= 0.5; //decrement the pitch setpoint + pitchLoop().setCommand(_pitch_command); pc().printf(">>> new pitch angle setpoint: %0.3f deg (decreased)\r\n", pitchLoop().getCommand()); } else if (userInput == 'W' or userInput == 'w') { - _pitchCommand += 0.5; //increment the pitch setpoint - pitchLoop().setCommand(_pitchCommand); + _pitch_command += 0.5; //increment the pitch setpoint + pitchLoop().setCommand(_pitch_command); pc().printf(">>> new pitch angle setpoint: %0.3f deg (increased)\r\n", pitchLoop().getCommand()); } else if (userInput == 'A' or userInput == 'a') { - _depthCommand -= 0.5; //decrement the depth setpoint - depthLoop().setCommand(_depthCommand); + _depth_command -= 0.5; //decrement the depth setpoint + depthLoop().setCommand(_depth_command); pc().printf(">>> new depth (ft) setpoint: %0.3f ft (sink)\r\n", depthLoop().getCommand()); } else if (userInput == 'S' or userInput == 's') { - _depthCommand += 0.5; //increment the depth setpoint - depthLoop().setCommand(_depthCommand); + _depth_command += 0.5; //increment the depth setpoint + depthLoop().setCommand(_depth_command); pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand()); } else if (userInput == '-') { @@ -849,18 +1286,21 @@ } else if (userInput == 'C' or userInput == 'c') { - pc().printf("depth: %3.1f\r\n",depthLoop().getPosition()); - pc().printf("pitch: %3.1f\r\n",imu().getPitch()); + + pc().printf("\n\n\rCURRENT STATUS AND PARAMETERS:\n\r"); + pc().printf("depth: %3.1f ft\r\n",depthLoop().getPosition()); + pc().printf("pitch: %3.1f deg\r\n",imu().getPitch()); pc().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm()); pc().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm()); pc().printf("batt().getPosition_mm(): %3.1f\r\n",batt().getPosition_mm()); pc().printf("batt().getSetPosition_mm(): %3.1f\r\n",batt().getSetPosition_mm()); pc().printf("depthLoop().getCommand(): %3.1f\r\n",depthLoop().getCommand()); pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand()); - pc().printf("Latest Neutral Buoyancy Positions: batt: %0.1f, bce: %0.1f\r\n",_neutral_batt_pos_mm,_neutral_bce_pos_mm); + + pc().printf("\n\rNeutral Buoyancy Positions: bce: %0.1f, batt: %0.1f\r\n",_neutral_bce_pos_mm,_neutral_batt_pos_mm); pc().printf("depthLoop().getOutputOffset(): %0.1f\r\n",depthLoop().getOutputOffset()); pc().printf("pitchLoop().getOutputOffset(): %0.1f\r\n",pitchLoop().getOutputOffset()); - pc().printf("Max recorded depth: neutral: %0.1f, dive: %0.1f\n\n\r",_max_recorded_depth_neutral, _max_recorded_depth_dive); + pc().printf("Max recorded depth: neutral: %0.1f, dive: %0.1f, auto_neutral_depth: %0.1f\n\n\r",_max_recorded_depth_neutral, _max_recorded_depth_dive, _max_recorded_auto_neutral_depth); } //when you read the keyboard successfully, change the state @@ -1118,7 +1558,7 @@ pitchLoop().setControllerI(_pitch_KI); pitchLoop().setControllerD(_pitch_KD); - // save pitch PID values for outer loop + // save pitch PID values for outer loop (must save neutral position also) configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm); break; //exit the while loop } @@ -1132,11 +1572,23 @@ } float StateMachine::getDepthCommand() { - return _depthCommand; + return _depth_command; } float StateMachine::getPitchCommand() { - return _pitchCommand; + return _pitch_command; +} + +float StateMachine::getDepthReading() { + return _depth_reading; +} + +float StateMachine::getPitchReading() { + return _pitch_reading; +} + +float StateMachine::getTimerReading() { + return _timer_reading; } void StateMachine::setState(int input_state) { @@ -1152,11 +1604,11 @@ } void StateMachine::setDepthCommand(float input_depth_command) { - _depthCommand = input_depth_command; + _depth_command = input_depth_command; } void StateMachine::setPitchCommand(float input_pitch_command) { - _pitchCommand = input_pitch_command; + _pitch_command = input_pitch_command; } void StateMachine::setNeutralPositions(float batt_pos_mm, float bce_pos_mm) { @@ -1179,4 +1631,119 @@ currentStateStruct.pitch = sequenceController().sequenceStructLoaded[_multi_dive_counter].pitch; _timeout = currentStateStruct.timeout; //set timeout before exiting this function +} + +void StateMachine::recordState(int input_state) { + string string_state; + + for (int i = 0; i < _state_array_counter; i++) { + if (input_state == SIT_IDLE) + string_state = "SIT_IDLE"; + else if (input_state == FIND_NEUTRAL) + string_state = "FIND_NEUTRAL"; + else if (input_state == DIVE) + string_state = "DIVE"; + else if (input_state == RISE) + string_state = "RISE"; + else if (input_state == FLOAT_LEVEL) + string_state = "FLOAT_LEVEL"; + else if (input_state == FLOAT_BROADCAST) + string_state = "FLOAT_BROADCAST"; + else if (input_state == EMERGENCY_CLIMB) + string_state = "EMERGENCY_CLIMB"; + else if (input_state == MULTI_DIVE) + string_state = "MULTI_DIVE"; + else if (input_state == MULTI_RISE) + string_state = "MULTI_RISE"; + else if (input_state == KEYBOARD) + string_state = "KEYBOARD"; + } + datalogger().printf("%s\n", string_state.c_str()); +} + +void StateMachine::recordData() { + if (!_is_log_timer_running) { + //pc().printf("\n\n\rlog timer running...\n\n\r"); //debug + + _log_timer = timer.read() + 1; //record the time when this block is first entered and add 5 seconds + _is_log_timer_running = true; //disable this block after one iteration + + _data_log[0] = timer.read(); //timer reading (shouldn't this be constant throughout the state?) + _data_log[1] = depthLoop().getCommand(); //depth command + _data_log[2] = depthLoop().getPosition(); //depth reading + _data_log[3] = pitchLoop().getCommand(); //pitch command + _data_log[4] = pitchLoop().getPosition(); //pitch reading + _data_log[5] = bce().getSetPosition_mm(); + _data_log[6] = bce().getPosition_mm(); + _data_log[7] = batt().getSetPosition_mm(); + _data_log[8] = batt().getPosition_mm(); + + //record data to the MBED every 5 seconds + mbedLogger().saveArrayToFile(_data_log); + + //record data to OpenLog every 5 seconds + datalogger().printf("%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f,%0.1f\n",_data_log[0],_data_log[1], + _data_log[2],_data_log[3],_data_log[4],_data_log[5],_data_log[6],_data_log[7],_data_log[8]); + } + if (timer.read() >= _log_timer) { + _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again + //pc().printf("\n\n\r TIME READ >= _log_timer \n\n\r"); //debug + } +} + +void StateMachine::printDirectory() { + //create a DirectoryList object that points to the local directory + DirectoryList mbed_dir( "/local" ); + + if ( mbed_dir.error_check() ) { + //error( "MBED directory could not be opened\r\n" ); + pc().printf("MBED directory could not be opened\r\n"); + } + + pc().printf("\n\rFiles in MBED directory:\n\r"); + for ( int i = 0; i < mbed_dir.size(); i++ ) + pc().printf( "%s\r\n", mbed_dir[ i ].c_str() ); + + //SD CARD DIRECTORY + DirectoryList sd_dir( "/sd" ); + + if ( sd_dir.error_check() ) { + //error( "MBED directory could not be opened\r\n" ); + pc().printf("MBED directory could not be opened\r\n"); + } + + pc().printf("\n\rFiles in SD card directory:\n\r"); + for ( int i = 0; i < sd_dir.size(); i++ ) + pc().printf( "%s\r\n", sd_dir[ i ].c_str() ); +} + +void StateMachine::printCurrentSdLog() { + pc().printf("SD card log work in progress\n\r"); + //might be worth saving the last few logs to the MBED... +} + +//check if the file is still opened +void StateMachine::createNewFile() { + if (_file_closed) { + mbedLogger().createFile(); //create a new MBED file + + _file_closed = false; //file is still open until you get to SIT_IDLE + } +} + +void StateMachine::transmitData() { + static float transmit_timer = 0; + static bool is_transmit_timer_running = false; + + if (!is_transmit_timer_running) { + //pc().printf("\n\n\rTRANSMIT timer running...\n\n\r"); //debug + + transmit_timer = timer.read() + 1; //record the time when this block is first entered and add 5 seconds + is_transmit_timer_running = true; //disable this block after one iteration + + pc().printf("TESTING to see if this transmits once a second. (timer: %0.1f)\n\r", timer.read()); + } + if (timer.read() >= transmit_timer) { + is_transmit_timer_running = false; // reset the sub state timer to do one-shot actions again + } } \ No newline at end of file