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:
- 30:2964617e7676
- Parent:
- 28:16c83a2fdefa
- Child:
- 32:f2f8ae34aadc
--- a/StateMachine/StateMachine.cpp Fri Dec 01 22:43:14 2017 +0000 +++ b/StateMachine/StateMachine.cpp Wed Dec 06 22:08:44 2017 +0000 @@ -38,7 +38,7 @@ _state_array[0] = SIT_IDLE; //system starts in the SIT_IDLE state, record this - _substate = NEUTRAL_FIRST_PITCH; //to start sub-FSM + _substate = NEUTRAL_SINKING; //start sub-FSM in NEUTRAL_SINKING _previous_substate = -1; //to start sub-FSM _previous_state = -1; //for tracking FSM states @@ -118,11 +118,11 @@ //first iteration goes into Neutral Finding Sub-FSM //set the first state of the FSM, and start the sub-FSM - _substate = NEUTRAL_FIRST_PITCH; + _substate = NEUTRAL_SINKING; //first state in neutral sub-FSM is the pressure vessel sinking _previous_substate = -1; //save this state to the array - _substate_array[_substate_array_counter] = NEUTRAL_FIRST_PITCH; //save to state array + _substate_array[_substate_array_counter] = NEUTRAL_SINKING; //save to state array _substate_array_counter++; runNeutralStateMachine(); @@ -146,6 +146,7 @@ //if successful, FIND_NEUTRAL then goes to RISE pc().printf("*************************************** FIND_NEUTRAL sequence complete. Rising.\n\n\r"); _state = RISE; + _isTimeoutRunning = false; } break; @@ -396,8 +397,8 @@ stateMachine().getDiveSequence(); //check if this is the end of the dive sequence - //CHECK BEFORE ANYTHING ELSE that you have reached the "exit" state (Float_Level) - if (currentStateStruct.state == FLOAT_LEVEL) { + //CHECK BEFORE ANYTHING ELSE that you have reached the "exit" state (FLOAT_BROADCAST) + if (currentStateStruct.state == FLOAT_BROADCAST) { _state = FLOAT_BROADCAST; return; } @@ -635,16 +636,16 @@ break; } - // reset the sub-FSM if needed + // 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 - _substate = NEUTRAL_FIRST_PITCH; + //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 to state array + _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)