Diff: StateMachine/StateMachine.cpp

Revision:

26:7e118fc02eea

Parent:

25:249e4d56b27c

Child:

27:0a5b90cd65d6

--- a/StateMachine/StateMachine.cpp	Wed Nov 29 15:42:12 2017 +0000
+++ b/StateMachine/StateMachine.cpp	Wed Nov 29 17:56:34 2017 +0000
@@ -87,7 +87,7 @@
             timer.reset();
             isTimeoutRunning = false;
         }
-        else if (depthLoop().getPosition() < 0.2) { //if the depth is greater than 0.2 feet, go to float broadcast
+        else if (depthLoop().getPosition() < 2.0) { //if the depth is greater than 0.2 feet, go to float broadcast
             pc().printf("EC: depth: %3.1f, cmd: 0.5 [%0.1f sec]\r",depthLoop().getPosition(), timer.read());
             _state = FLOAT_BROADCAST;
             timer.reset();
@@ -129,7 +129,7 @@
         
         //what is active? (neutral finding sub-function runs until completion)        
         //check if substate returned exit state, if so stop running the sub-FSM
-        if (runNeutralStateMachine() == NEUTRAL_EXIT) { 
+        else if (runNeutralStateMachine() == NEUTRAL_EXIT) { 
             //if successful, FIND_NEUTRAL then goes to RISE
             pc().printf("*************************************** FIND_NEUTRAL sequence complete.  Rising.\n\n\r");
             _state = RISE;
@@ -188,7 +188,7 @@
             batt().unpause();
  
             // what are the commands?
-            depthLoop().setCommand(0.0);
+            depthLoop().setCommand(0.5);
             pitchLoop().setCommand(-pitchCommand);
             pc().printf("RISE: depth cmd: 0.0\r\n");
             pc().printf("RISE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
@@ -283,7 +283,7 @@
         }
         
         // what is active?
-        pc().printf("FB: 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());
+        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());
         break;
         
     case MULTI_DIVE :
@@ -810,9 +810,7 @@
         }
         
         //when you read the keyboard successfully, change the state
-        _state = _keyboard_state;   //set state at the end of this function
-        
-        wait_us(100);   
+        _state = _keyboard_state;   //set state at the end of this function 
     }
 }