Diff: StateMachine/StateMachine.cpp

Revision:

30:2964617e7676

Parent:

28:16c83a2fdefa

Child:

32:f2f8ae34aadc

diff -r 16c83a2fdefa -r 2964617e7676 StateMachine/StateMachine.cpp
--- 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)