Diff: StateMachine/StateMachine.cpp

Revision:

20:8987a9ae2bc7

Parent:

19:aaa0f4f29d50

Child:

21:38c8544db6f4

--- a/StateMachine/StateMachine.cpp	Wed Nov 22 00:08:03 2017 +0000
+++ b/StateMachine/StateMachine.cpp	Wed Nov 22 14:32:06 2017 +0000
@@ -1,24 +1,27 @@
 #include "StateMachine.hpp"
 #include "StaticDefs.hpp"
-
+ 
 StateMachine::StateMachine() {
-    _timeout = 20;             // generic timeout for every state, seconds
+    _timeout = 20;            // generic timeout for every state, seconds
+    
     depthTolerance = 0.25;    // depth tolerance for neutral finding exit critera
     pitchTolerance = 1.0;     // pitch angle tolerance for neutral finding exit criteria
+ 
     bceFloatPosition = 300;   // bce position for "float" states
     battFloatPosition = 50;   // batt position for "broadcast" state
-    
+ 
     depthCommand = 3.5;       // user keyboard depth
     pitchCommand = -20.0;     // user keyboard depth
     
-    _neutral_sink_timer = 10;
-    _neutral_rise_timer = 5;
+    _neutral_sink_timer = 0;
+    _neutral_rise_timer = 0;
     
-    previousPosition_mm = 220;      //centered, overwritten by the state machine (LOAD THIS FROM CONFIG?)
-    _state = SIT_IDLE;               // select starting state here
-    
+    previousPosition_mm = 220; // Troy: centered, overwritten by the state machine (LOAD THIS FROM CONFIG?)
+                               // Dan: no, think - where do you Want the bce to start from?  Full extend limt probably.
+ 
+    _state = SIT_IDLE;              // select starting state here
     isTimeoutRunning = false;       // default timer to not running
-    isSubStateTimeoutRunning = false;
+    isSubStateTimerRunning = false; // default timer to not running
     
     _neutral_buoyancy_bce_pos_mm = 0;
     _neutral_buoyancy_batt_pos_mm = 0;
@@ -27,11 +30,10 @@
     _next_state = -1;    //next state is used to prevent states from changing as the FSM executes
     _state_counter = 0;
 }
-
+ 
 //Finite State Machine (FSM)
 void StateMachine::runStateMachine() { 
-    //use the _next_state when the state machine is run again (so that it cannot change states while the FSM executes)
-
+ 
     static bool runFirstNeutral = false;
             
     //use the _next_state when the state machine is run again (so that it cannot change states while the FSM executes)   
@@ -46,18 +48,19 @@
             showMenu();
             pc().printf("\r\n\nstate: SIT_IDLE\r\n");
             isTimeoutRunning = true; 
-
+ 
             // what is active?
             bce().pause();
             batt().pause();
             
             //reset sub FSM
-            isSubStateTimeoutRunning = false;
+            isSubStateTimerRunning = false;
         }
+        
         // how exit?
-        // separate keyboard function will change the states
+        keyboard(); // keyboard function will change the state if needed
         break;
-    
+ 
     case EMERGENCY_CLIMB :
         // start local state timer and init any other one-shot actions
         if (!isTimeoutRunning) {
@@ -69,11 +72,12 @@
             // what needs to be started?
             bce().unpause();
             batt().unpause();
-
-            // what is active?
+ 
+            // what are the commands?
             bce().setPosition_mm(bce().getTravelLimit());
             batt().setPosition_mm(0.0);
         }
+        
         // how exit?
         if (timer > _timeout) {
             pc().printf("EC: timed out\r\n");
@@ -88,41 +92,39 @@
             isTimeoutRunning = false;
         }
         break;
-
+ 
     case FIND_NEUTRAL :
-        /* start local state timer and init any other one-shot actions */
+        // start local state timer and init any other one-shot actions
         if (!isTimeoutRunning) {
             pc().printf("\r\n\nstate: FIND_NEUTRAL\r\n");
             timer.reset(); // timer goes back to zero
             timer.start(); // background timer starts running
-            isTimeoutRunning = true; 
+            isTimeoutRunning = true;
             
             // what needs to be started?
             bce().unpause();
             batt().unpause();
             
             //first iteration sends SINKING sub-state
-            runFirstNeutral = true;            
+            runFirstNeutral = true;
         }
-        
-        /* how exit? (exit with the timer, if timer still running continue processing sub FSM) */
+ 
+        // how exit? (exit with the timer, if timer still running continue processing sub FSM)
         if (timer > _timeout) {
             pc().printf("FN: timed out [time: %0.1f sec]\r\n", timer.read());
             _next_state = EMERGENCY_CLIMB;         //new behavior (if this times out it emergency surfaces)
             timer.reset();
             isTimeoutRunning = false;
         }
-        
+ 
         // what is active? (call neutral finding sub-function every iteration)
         if (runFirstNeutral) {
             //pc().printf("First iteration of find neutral\n\r"); // debug (confirmed working)
             _sub_state = findNeutralSubState(NEUTRAL_SINKING);
-            _previous_sub_state = _sub_state;       //save previous sub-state
+            _previous_sub_state = _sub_state; //save previous sub-state
             runFirstNeutral = false;
         }
-        
-        //second and further iterations this is running
-        else {
+        else { //second and further iterations, this is running
             _sub_state = findNeutralSubState(_previous_sub_state);
             _previous_sub_state = _sub_state;       //save previous sub-state
             
@@ -145,13 +147,14 @@
             // what needs to be started?
             bce().unpause();
             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());
         }
+ 
         // how exit?
         if (timer > _timeout) {
             pc().printf("DIVE: timed out\n\n\r");
@@ -165,6 +168,7 @@
             timer.reset();
             isTimeoutRunning = false;
         }
+ 
         // what is active?
         pc().printf("DIVE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) [%0.2f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read());
         bce().setPosition_mm(depthLoop().getOutput());
@@ -189,6 +193,7 @@
             pc().printf("RISE: depth cmd: 0.0\r\n");
             pc().printf("RISE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
         }
+ 
         // how exit?
         if (timer > _timeout) {
             pc().printf("RISE: timed out\r\n");
@@ -202,8 +207,9 @@
             timer.reset();
             isTimeoutRunning = false;
         }
+ 
+        // 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());
-        // what is active?
         bce().setPosition_mm(depthLoop().getOutput());  //constantly checking the Outer Loop output to move the motors
         batt().setPosition_mm(pitchLoop().getOutput()); 
         break;  
@@ -220,10 +226,11 @@
             bce().unpause();
             batt().unpause();
  
-            // what are the commands
+            // what are the commands?
             bce().setPosition_mm(bceFloatPosition);
             pitchLoop().setCommand(0.0);
         }
+        
         // how exit?
         if (timer > _timeout) {
             pc().printf("FL: timed out\r\n");
@@ -237,6 +244,7 @@
             timer.reset();
             isTimeoutRunning = false;
         }
+        
         // 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());
@@ -253,11 +261,12 @@
             // what needs to be started?
             bce().unpause();
             batt().unpause();
-
+ 
             // what are the commands?
             bce().setPosition_mm(bceFloatPosition);
             batt().setPosition_mm(battFloatPosition);
         }
+        
         // how exit?
         if (timer > _timeout) {
             pc().printf("FB: timed out\r\n");
@@ -265,13 +274,15 @@
             timer.reset();
             isTimeoutRunning = false;
         }
-        if ( (fabs(bce().getPosition_mm() - bce().getSetPosition_mm()) < bce().getDeadband()) and
-             (fabs(batt().getPosition_mm() - batt().getSetPosition_mm()) < batt().getDeadband()) ) {
+        else if ( (fabs(bce().getPosition_mm() - bce().getSetPosition_mm()) < bce().getDeadband()) and
+                  (fabs(batt().getPosition_mm() - batt().getSetPosition_mm()) < batt().getDeadband()) ) {
             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());
             _next_state = SIT_IDLE;
             timer.reset();
             isTimeoutRunning = false;
         }
+        
+        // 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());
         break;
         
@@ -286,7 +297,7 @@
             // what needs to be started?
             bce().unpause();
             batt().unpause();
-
+ 
             //NEW: retrieve depth and pitch commands from config file struct
             // concept is to load this each time the multi-dive restarts
             
@@ -303,6 +314,7 @@
             pc().printf("MULTI-DIVE: depth cmd: %3.1f\r\n",depthLoop().getCommand());
             pc().printf("MULTI-DIVE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
         }
+        
         // how exit?
         if (timer > _timeout) {
             pc().printf("MULTI-DIVE: timed out\n\n\r");
@@ -316,6 +328,7 @@
             timer.reset();
             isTimeoutRunning = false;
         }
+        
         // what is active?
         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());
@@ -347,6 +360,7 @@
             pc().printf("MULTI-RISE: depth cmd: 0.0\r\n");
             pc().printf("MULTI-RISE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
         }
+        
         // how exit?
         if (timer > _timeout) {
             pc().printf("MULTI-RISE: timed out\r\n");
@@ -357,9 +371,7 @@
             //reset multi-dive sequence to start
             _state_counter = 0;
         }
-        
-        //depth is less than 0.5 (zero is surface level)
-        else if (depthLoop().getPosition() < 0.5) {
+        else if (depthLoop().getPosition() < 0.5) { // depth is less than 0.5 (zero is surface level)
             pc().printf("MULTI-RISE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
             
             //going to next state            
@@ -383,8 +395,9 @@
             
             //have to stop this with the _state_counter variable!
         }
+        
+        // what is active?
         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());
-        // what is active?
         bce().setPosition_mm(depthLoop().getOutput());  //constantly checking the Outer Loop output to move the motors
         batt().setPosition_mm(pitchLoop().getOutput()); 
         break; 
@@ -394,7 +407,7 @@
         _next_state = SIT_IDLE;
     } 
 }
-
+ 
 // output the keyboard menu for user's reference
 void StateMachine::showMenu() {
     pc().printf("\r\r\n\nKEYBOARD MENU:\r\r\n");
@@ -419,162 +432,147 @@
     pc().printf(" C See sensor readings\r\n");
     pc().printf(" ? to reset mbed\r\n");
 }
-
+ 
 //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::findNeutralSubState(int input_substate) {  
-    float pitch_angle = pitchLoop().getPosition();    
-    float pitch_rate = pitchLoop().getVelocity();
-     
-    float abs_pitch_angle = fabs(pitch_angle);
-    float abs_pitch_rate = fabs(pitch_rate);
-    
+ 
     //output substate is the input substate unless it is changed in the sub FSM
     static int output_substate = NEUTRAL_SINKING;   //to start
-    //int output_substate = input_substate;    
     
     switch (input_substate) {
-        case NEUTRAL_SINKING:            
+        case NEUTRAL_SINKING :
             //start the 10 second timer
-            if (!isSubStateTimeoutRunning) { 
+            if (!isSubStateTimerRunning) {
                 pc().printf(" input_substate: %d\n\r", input_substate);   //debug              
-                _neutral_sink_timer = timer.read()+ 10; //record the time when this block is first entered and add 10 seconds
-                pc().printf("\r\n\n(8) START NEUTRAL_SINKING (_neutral_sink_timer will go off at %0.1f sec)  %3.1f\r\n", _neutral_sink_timer,timer.read());
+                _neutral_sink_timer = timer.read() + 10; //record the time when this block is first entered and add 10 seconds
                 
-                // what is active?
+                pc().printf("\r\n\n START NEUTRAL_SINKING (_neutral_sink_timer will go off at %0.1f sec)  %3.1f\r\n", _neutral_sink_timer, timer.read());
+                
+                // what are the commands?
                 //move piston at start of sequence (retract 10 mm)
                 pc().printf("Neutral Sinking: Retracting piston 10 mm\n\r");
                 previousPosition_mm -= 10;
                 bce().setPosition_mm(previousPosition_mm);  //no depth command
                 
-                isSubStateTimeoutRunning = true;    //disable this block after one iteration
+                isSubStateTimerRunning = true;    //disable this block after one iteration
             }
-                        
-            //once the 10 second timer is complete move the piston with code above             
-            if (timer.read() >= _neutral_sink_timer) {
-                pc().printf("\r\n\n(8) NEUTRAL_SINKING TIMER COMPLETE! %3.1f\r\n", timer.read());
-                pc().printf("\r\n\n(8) (BATT POS: %0.1f) retract 10 mm (time: %3.1f)\r\n", previousPosition_mm, timer.read());
-                
-                //when you finish executing the motor controls, reset the neutral sinking timer in the next state
-                isSubStateTimeoutRunning = false;
+ 
+            // how exit?
+            //once reached the travel limit, no need to keep trying, so exit
+            if (bce().getPosition_mm() <= 0) {
+                output_substate = NEUTRAL_EXIT;
             }
-            
-            
-
-            //depth >= depth command, go to the next substate the next iteration
-            if (depthLoop().getPosition() > depthCommand) {
-                pc().printf("Next substate: NEUTRAL_SLOWLY_RISE\n\r");
-                output_substate = NEUTRAL_SLOWLY_RISE;
+            //once deeper than the commanded setpoint...
+            else if (depthLoop().getPosition() > depthCommand) {
+                output_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_sink_timer) {
+                pc().printf("\r\n\n NEUTRAL_SINKING TIMER COMPLETE! %3.1f\r\n", timer.read());
+                pc().printf("\r\n\n (BATT POS: %0.1f) retract 10 mm (time: %3.1f)\r\n", previousPosition_mm, timer.read());
                 
-                //when you finish executing the motor controls, reset the neutral sinking timer in the next state
-                isSubStateTimeoutRunning = false;
-            }
-            
-            //before the sub-fsm runs again, make sure the piston is in a safe position (retracted)
-            //NEW: need to double-check this behavior
-            if (bce().getPosition_mm() <= 0) {
-                output_substate = NEUTRAL_EXIT;     
-                //the "case" NEUTRAL_EXIT is used to tell the greater FSM that this sub-FSM has completed
+                isSubStateTimerRunning = false; //reset the sub state timer
             }
             break;
             
         case NEUTRAL_SLOWLY_RISE:
-            if (!isSubStateTimeoutRunning) {                
+            if (!isSubStateTimerRunning) {                
                 pc().printf("\r\n\n(9) START NEUTRAL_SLOWLY_RISE (will go off at %0.1f sec)\r\n", timer.read());
-                _neutral_sink_timer = timer.read()+ 5; //record the time when this block is first entered and add 10 seconds
+                _neutral_rise_timer = timer.read()+ 5; //record the time when this block is first entered and add 5 seconds
                 
-                isSubStateTimeoutRunning = true;    //disable this block after one iteration
+                isSubStateTimerRunning = true;    //disable this block after one iteration
                 
-                // what is active?
-                //move piston at start of sequence (retract 10 mm)
+                // what are the commands?
+                //move piston at start of sequence (extend)
                 pc().printf("Neutral Slowly Rise: Extending piston 1 mm\n\r");
                 previousPosition_mm += 1;
                 bce().setPosition_mm(previousPosition_mm);  //no depth command
             }
             
-            //once 5 second timer complete move the piston with code above  
-            if (timer.read() >= _neutral_sink_timer) {
-                pc().printf("\r\n\n(9)NEUTRAL_SLOWLY_RISE TIMER COMPLETE! (time: %3.1f)\r\n", timer.read());
-                pc().printf("\r\n\n(9) (BATT POS: %0.1f) extend 1 mm %3.1f\r\n", previousPosition_mm, timer.read());
-                
-                //when you finish executing the motor controls, reset the neutral sinking timer in the next state
-                isSubStateTimeoutRunning = false;
+            // how exit?
+            //once at full travel limit and haven't yet risen, time to give up and exit
+            if (bce().getPosition_mm() >= bce().getTravelLimit()) {
+                output_substate = NEUTRAL_EXIT;     
             }
-            
             //depth rate or sink rate < 0 ft/s, go to the next substate the next iteration
-            if (depthLoop().getVelocity() < 0) { //less than zero ft/s
+            else if (depthLoop().getVelocity() < 0) { //less than zero ft/s
                 pc().printf("\r\n\n(9) NEUTRAL_SLOWLY_RISE: Sink Rate below 0 ft/s [time: %0.1f]\r\n", timer.read());
-                
-                pc().printf("Next substate: NEUTRAL_CHECK_PITCH\n\r");
                 output_substate = NEUTRAL_CHECK_PITCH;
             }
             
-            //before the sub-fsm runs again, make sure the piston is in a safe position (extended)
-            //NEW: need to double-check this behavior
-            //need to set a max travel position ? or just hardcode? or use bceFloatPosition?
-            if (bce().getPosition_mm() >= 320) {
-                output_substate = NEUTRAL_EXIT;     
-                //the "case" NEUTRAL_EXIT is used to tell the greater FSM that this sub-FSM has completed
+            // 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_sink_timer) {
+                pc().printf("\r\n\n(9)NEUTRAL_SLOWLY_RISE TIMER COMPLETE! (time: %3.1f)\r\n", timer.read());
+                pc().printf("\r\n\n (BATT POS: %0.1f) extend 1 mm %3.1f\r\n", previousPosition_mm, timer.read());
+   
+                isSubStateTimerRunning = false; // reset the sub state timer
             }
             break;   
                 
-        case NEUTRAL_CHECK_PITCH:
-            //no timeout in this sub FSM state (find_neutral timer still running)        
-            
-            //what is active?
+        case NEUTRAL_CHECK_PITCH :
+            pc().printf("NEUTRAL_CHECK_PITCH (time: %3.1f) [%0.1f deg, %0.1f deg/s] \r", timer.read(), pitchLoop().getPosition(), pitchLoop().getVelocity()); //debug  
+            //no timeout in this sub FSM state (find_neutral timer still running)
+ 
+            //what are commands?
             //the BCE piston is maintaining its position 
             //the battery is maintaining the pitch at zero degrees
             
-            //check sink rate < 0.5 ft/s, go to the next substate the next iteration (pitch velocity)
-            //check pitch angle is less than 1.0 degree
-            
-            pc().printf("(10) NEUTRAL_CHECK_PITCH (time: %3.1f) [%0.1f deg, %0.1f deg/s] \r", timer.read(), pitch_angle, pitch_rate); //debug  
-            output_substate = NEUTRAL_CHECK_PITCH;
-            
-            //benchtop tests confirm it needs to be around 2 degrees
-            if ((abs_pitch_rate < 0.5) && (abs_pitch_angle < 2.0)) { //less than zero ft/s and 1 degree
-                //SAVE POSITIONS
-                _neutral_buoyancy_bce_pos_mm = bce().getPosition_mm();
-                _neutral_buoyancy_batt_pos_mm = batt().getPosition_mm();
+            // how exit?
+            //pitch angle and pitch rate within small tolerance
+            //benchtop tests confirm angle needs to be around 2 degrees
+            if ((fabs(pitchLoop().getPosition()) < 0.5) and (fabs(pitchLoop().getVelocity()) < 2.0)) {
+                pc().printf("Saving Positions: BCE: %0.1f mm, BATT: %0.1f\n\r",bce().getPosition_mm(),batt().getPosition_mm());
+ 
+                //save positions locally
+                depthLoop().setOutputOffset(bce().getPosition_mm());
+                pitchLoop().setOutputOffset(batt().getPosition_mm());
                 
                 //save to neutral.cfg
-                ConfigFileIO().saveNeutralPositions(_neutral_buoyancy_bce_pos_mm,_neutral_buoyancy_batt_pos_mm); //BCE, BATT
+                // Troy: save into the depth.txt and pitch.txt files, not a separate file.
+                ConfigFileIO().saveNeutralPositions(bce().getPosition_mm(),batt().getPosition_mm()); //BCE, BATT
                 
-                pc().printf("Saving Positions: BCE: %0.1f mm, BATT: %0.1f\n\r",_neutral_buoyancy_bce_pos_mm,_neutral_buoyancy_batt_pos_mm);
-                pc().printf("Next substate: NEUTRAL_EXIT\n\r");
                 output_substate = NEUTRAL_EXIT;     //the "case" NEUTRAL_EXIT is used to tell the greater FSM that this sub-FSM has completed
             }
             break;
-  
-        default:
-            pc().printf("DEFAULT: check_pitch (state 10)\n\r"); //debug
-            output_substate = NEUTRAL_CHECK_PITCH;  //a default within the sub-state machine
+ 
+        case NEUTRAL_EXIT :
+            //NEUTRAL_EXIT state is used to tell the greater FSM that this sub-FSM has completed
+            pc().printf("substate: NEUTRAL_EXIT\n\r");
+            break;
+ 
+        default :
+            pc().printf("how did we get to substate default?\n\r"); //debug
+            output_substate = NEUTRAL_EXIT;  //a default within the sub-state machine
             break;    
     }
+    
     return output_substate;
 }
-
-// keyboard currently handles a key at a time
-// returns -1 if not a state command
-// returns a positive number to command a new state
+ 
+// 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
+//and only allows input when the state is "idle"
 void StateMachine::keyboard() {
     char userInput;
-    
+ 
     // check keyboard and make settings changes as requested
     // states can be changed only at the start of a sequence (when the system is in SIT_IDLE)
     if (pc().readable() && (_state == SIT_IDLE)) {
         // get the key
         userInput = pc().getc();    
         
-        isTimeoutRunning = false;   //keyboard resets timer each time it's used
+        // keyboard resets timer each time it's used
+        isTimeoutRunning = false;
         
         pc().printf("KEYBOARD isTimeoutRunning: %d\n\r", isTimeoutRunning);
         
-        //_keyboard_state = SIT_IDLE; //new
-
         // check command against desired control buttons
-        // change state
         if (userInput == 'D' or userInput == 'd') {
             _keyboard_state = DIVE;
         }
@@ -639,7 +637,7 @@
             wait(0.5);
             mbed_reset();
         }
-
+ 
         // change settings        
         else if (userInput == 'Q' or userInput == 'q') {
             pitchCommand -= 0.5;         //decrement the pitch setpoint
@@ -661,6 +659,7 @@
             depthLoop().setCommand(depthCommand);
             pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand());
         }
+ 
         else if (userInput == '-') {
             _timeout -= 10.0;               //decrement the timeout
             pc().printf(">>> timeout decreased: %d\r\n", _timeout);
@@ -670,8 +669,6 @@
             pc().printf(">>> timeout increased: %d\r\n", _timeout);
         }
         
-        // add keyboard commands to move the neutral zero offsets, both bce and batt
-        
         // go to sub-menus for the PID gains (this is blocking)
         else if (userInput == '1') {
             keyboard_menu_BCE_PID_settings();
@@ -704,19 +701,8 @@
         _next_state = _keyboard_state;   //set state at the end of this function
     }
 }
-
-//11/19/2017
-//you want to modify the keyboard to return the values that will be used in the state machine
-//BUT the state machine does not call they keyboard
-//the keyboard runs independently and only allows input when the state is "idle"
-//therefore keyboard should run maybe 1/10th of a second when pc readable
-//and it will change a class variable that says what the current state is
-
-//make it void
-
-
-
-
+ 
+ 
 void StateMachine::keyboard_menu_BCE_PID_settings() {    
     char PID_key;
     float gain_step_size = 0.01;    // modify this to change gain step size
@@ -783,7 +769,7 @@
         }
     }
 }
-
+ 
 void StateMachine::keyboard_menu_BATT_PID_settings() {    
     char PID_key;
     float gain_step_size = 0.01;    // modify this to change gain step size
@@ -796,7 +782,7 @@
     pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
     pc().printf("\n\r(Hit shift + X to exit w/o saving.  Hit shift + S to save.\n\r");
     pc().printf("batt   P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f  \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
-
+ 
     // handle the key presses
     while(1) {
         // get the user's keystroke from either of the two inputs
@@ -850,7 +836,7 @@
         }
     }
 }
-
+ 
 void StateMachine::keyboard_menu_DEPTH_PID_settings() {    
     char PID_key;
     float gain_step_size = 0.01;    // modify this to change gain step size
@@ -930,7 +916,7 @@
     pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
     pc().printf("\n\r(Hit shift + X to exit w/o saving.  Hit shift + S to save.\n\r");
     pc().printf("pitch  P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
-
+ 
     // handle the key presses
     while(1) {
         // get the user's keystroke from either of the two inputs
@@ -983,48 +969,48 @@
         }
     }
 }
-
+ 
 float StateMachine::getDepthCommand() {
     return depthCommand;
 }
-
+ 
 float StateMachine::getPitchCommand() {
     return pitchCommand;
 }
-
+ 
 void StateMachine::setState(int input_state) {
     //pc().printf("input_state: %d\n\r", input_state); //debug
     //_state = input_state;     //changing wrong variable
     _next_state = input_state;
 }
-
+ 
 int StateMachine::getState() {
     return _state;  //return the current state of the system
 }
-
+ 
 void StateMachine::setTimeout(float input_timeout) {
     _timeout = input_timeout;
 }
-
+ 
 void StateMachine::setDepthCommand(float input_depth_command) {
     depthCommand = input_depth_command;
 }
-
+ 
 void StateMachine::setPitchCommand(float input_pitch_command) {
     pitchCommand = input_pitch_command;
 }
-
+ 
 void StateMachine::setNeutralPositions(float batt_pos_mm, float bce_pos_mm) {
     _neutral_buoyancy_batt_pos_mm = batt_pos_mm;
     _neutral_buoyancy_bce_pos_mm = bce_pos_mm;
     
     pc().printf("Neutral Buoyancy Positions: batt: %0.1f, bce: %0.1f\n\r",_neutral_buoyancy_batt_pos_mm,_neutral_buoyancy_bce_pos_mm);
 }
-
+ 
 int StateMachine::timeoutRunning() {
     return isTimeoutRunning;
 }
-
+ 
 //process one state at a time
 void StateMachine::getDiveSequence() {
     //iterate through this sequence using the FSM