File content as of revision 68:8f549749b8ce:

#include "StateMachine.hpp"
#include "StaticDefs.hpp"
 
StateMachine::StateMachine() {
    _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)
 
    _depth_command = 2.0;                        // user keyboard depth (default)
    _pitch_command = -20.0;                      // user keyboard pitch (default)
    _heading_command = 0.0;
    
    //new commands
    _BCE_dive_offset = 0.0;     //starting at the limits
    _BMM_dive_offset = 0.0;
    //new commands
    
    _neutral_timer        = 0;                  //timer used in FIND_NEUTRAL sub-FSM
 
    _state = SIT_IDLE;                          // select starting state here
    _isTimeoutRunning = false;                  // default timer to not running
    _isSubStateTimerRunning = false;            // default timer to not running
    
    _multi_dive_counter = 0;
    
    _neutral_sub_state_active = false;
    
    _depth_KP = depthLoop().getControllerP();  // load current depth value
    _depth_KI = depthLoop().getControllerI();  // load current depth value
    _depth_KD = depthLoop().getControllerD();  // load current depth value
    
    _pitch_KP = pitchLoop().getControllerP();  // load current pitch value
    _pitch_KI = pitchLoop().getControllerI();  // load current pitch value
    _pitch_KD = pitchLoop().getControllerD();  // load current pitch value
    
    _neutral_bce_pos_mm = depthLoop().getOutputOffset(); //load current neutral buoyancy position offset
    _neutral_batt_pos_mm = pitchLoop().getOutputOffset(); //load current neutral buoyancy position offset
    
    _state_array_counter = 1;                   //used to iterate through and record states
    _substate_array_counter = 0;                //used to iterate through and record substates

    _state_array[0] = SIT_IDLE;  //system starts in the SIT_IDLE state, record this
    
    _substate = NEUTRAL_SINKING;                //start sub-FSM in NEUTRAL_SINKING
    _previous_substate = -1;                    //to start sub-FSM
    _previous_state = -1;                       //for tracking FSM states
    
    _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;
    
    _debug_menu_on = false;                     //toggle between debug and simple menu screens
}
 
//Finite State Machine (FSM)
int StateMachine::runStateMachine() {   
    static int transmit_packet_number = 1;  //for data transmission
    
    // finite state machine ... each state has at least one exit criteria
    switch (_state) {
    case SIT_IDLE :
    case KEYBOARD:    
        // there actually is no timeout for SIT_IDLE, but this enables some one-shot actions
        if (!_isTimeoutRunning) {
            
            if (_debug_menu_on)
                showDebugMenu();
            else
                showSimpleMenu();
            pc().printf("\r\n\nstate: SIT_IDLE\r\n");
            _isTimeoutRunning = true; 
 
            // what is active?
            bce().pause();
            batt().pause();
                     
            //reset sub FSM
            _isSubStateTimerRunning = false;
            
            //close the MBED file
            _file_closed = true;
        }
        
        // how exit?
        keyboard(); // keyboard function will change the state if needed
        break;
        
    case CHECK_TUNING :                 // state used to check the tuning of the pressure vessel
        // start local state timer and init any other one-shot actions
        if (!_isTimeoutRunning) {
            pc().printf("\r\n\nstate: CHECK_TUNING\r\n");
            timer.reset(); // timer goes back to zero
            timer.start(); // background timer starts running
            _isTimeoutRunning = true; 
            
            // what needs to be started?
            bce().unpause();    //this is now active
            batt().unpause();   //this is now active
 
            // what are the commands? (DRIVE THE MOTORS "DIRECTLY")
            bce().setPosition_mm(_neutral_bce_pos_mm);              //this variable is loaded from the file at initialization
            batt().setPosition_mm(_neutral_batt_pos_mm);            //this variable is loaded from the file at initialization
            
            // getSetPosition_mm is the commanded position in the LinearActuator class
            
            pc().printf("CHECK_TUNING: BCE cmd: %3.1f (BCE current position: %3.1f)\r\n", bce().getSetPosition_mm(), bce().getPosition_mm());
            pc().printf("CHECK_TUNING: BATT cmd: %3.1f (BATT current position: %3.1f)\r\n", batt().getSetPosition_mm(), bce().getPosition_mm());
            
            //triggers logger array
            _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
        }
    
        // how exit?
        if (timer > _timeout) {
            pc().printf("CHECK_TUNING: timed out!\r\n");
            _state = FLOAT_BROADCAST;
            timer.reset();
            _isTimeoutRunning = false;
        }
        
        //WHAT IS ACTIVE?
        // the inner loop position controls are maintaining the positions of the linear actuators
        
        //print status to screen continuously
        pc().printf("CHECK_TUNING: BCE_position: %0.1f, BATT_position: %0.1f (BCE_cmd: %0.1f, BATT_cmd: %0.1f)(depth: %0.1f ft, pitch: %0.1f deg) [%0.1f sec]\r",bce().getPosition_mm(),batt().getPosition_mm(),bce().getSetPosition_mm(),batt().getSetPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition(),timer.read());
        
        checkMotorPositions();
        
        break;
 
    case EMERGENCY_CLIMB :
        // start local state timer and init any other one-shot actions
        if (!_isTimeoutRunning) {
            pc().printf("\r\n\nstate: EMERGENCY_CLIMB\r\n");
            timer.reset(); // timer goes back to zero
            timer.start(); // background timer starts running
            _isTimeoutRunning = true; 
            
            // what needs to be started?
            bce().unpause();
            batt().unpause();
 
            // what are the commands?
            bce().setPosition_mm(bce().getTravelLimit());
            batt().setPosition_mm(10.0);    //pull nose up (0.0 was sketchy)
            
            //triggers logger array
            _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
        }
        
        // how exit?
        if (timer > _timeout) {
            pc().printf("EC: timed out\r\n");
            _state = FLOAT_BROADCAST;
            timer.reset();
            _isTimeoutRunning = false;
        }
        else if (depthLoop().getPosition() < 2.0) { //if the depth is greater than 0.2 feet, go to float broadcast
            _state = FLOAT_BROADCAST;
            timer.reset();
            _isTimeoutRunning = false;
        }
        
        //WHAT IS ACTIVE?
        //print status to screen continuously
        pc().printf("EC: depth: %3.1f, pitch: %0.1f deg [BCE:%0.1f (cmd: %0.1f) BMM:%0.1f (cmd: %0.1f)] [%0.1f sec]\r",depthLoop().getPosition(),pitchLoop().getPosition(),bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),timer.read());
        
        break;
 
    case FIND_NEUTRAL :
        // 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;
            
            // what needs to be started?
            bce().unpause();
            batt().unpause();
            bce().setPosition_mm(_bceFloatPosition);
            batt().setPosition_mm(_neutral_batt_pos_mm);    //set battery to close-to-neutral setting from config file 
            
            //first iteration goes into Neutral Finding Sub-FSM 
            //set the first state of the FSM, and start the sub-FSM
            _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_SINKING;  //save to state array
            _substate_array_counter++;     
                   
            runNeutralStateMachine(); 

            //triggers logger array
            _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)
        if (timer > _timeout) {
            pc().printf("FN: timed out [time: %0.1f sec]\r\n", timer.read());
            _state = EMERGENCY_CLIMB;         //new behavior (if this times out it emergency surfaces)
            timer.reset();
            _isTimeoutRunning = false;
            
            //record this to the NEUTRAL sub-FSM tracker
            _substate_array[_substate_array_counter] = EMERGENCY_CLIMB;  //save to state array
            _substate_array_counter++;
        }
        
        //what is active? (neutral finding sub-function runs until completion)        
        //check if substate returned exit state, if so stop running the sub-FSM
        else if (runNeutralStateMachine() == NEUTRAL_EXIT) { 
            //if successful, FIND_NEUTRAL then goes to RISE
            pc().printf("*************************************** FIND_NEUTRAL sequence complete.  Rising.\r\n\n");
            _state = RISE;
            _isTimeoutRunning = false;
        }
        
        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
            timer.start(); // background timer starts running
            _isTimeoutRunning = true; 
            
            // what needs to be started?
            bce().unpause();
            batt().unpause();
 
            // what are the commands?
            depthLoop().setCommand(_depth_command);
            pitchLoop().setCommand(_pitch_command);
            
            pc().printf("DIVE: depth cmd: %3.1f\r\n",depthLoop().getCommand());
            pc().printf("DIVE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
            
            //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();
            
            //triggers logger array
            _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
        }
 
        // how exit?
        if (timer.read() > _timeout) {
            pc().printf("DIVE: timed out\r\n\n");
            _state = RISE; //new behavior 11/17/2017
            timer.reset();
            _isTimeoutRunning = false;
        }
        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;
        }
 
        // 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] (CMD BCE: %0.1f BATT: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read(),bce().getSetPosition_mm(),batt().getSetPosition_mm());
        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
        }
        
        checkMotorPositions();
        
        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
            timer.start(); // background timer starts running
            _isTimeoutRunning = true; 
            
            // what needs to be started?
            bce().unpause();
            batt().unpause();
 
            // what are the commands?
            depthLoop().setCommand(-1.0);           //make sure to get towards the surface (saw issues at LASR pool)
            pitchLoop().setCommand(-_pitch_command);
                        
            pc().printf("RISE: depth cmd: %0.1f\r\n",depthLoop().getCommand());
            pc().printf("RISE: pitch cmd: %0.1f\r\n",pitchLoop().getCommand());
            
            //create the log file (works only if the file is closed)
            //createNewFile();
            
            //triggers logger array
            _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
        }
 
        // how exit?
        if (timer.read() > _timeout) {
            pc().printf("RISE: timed out\r\n");
            _state = EMERGENCY_CLIMB;
            timer.reset();
            _isTimeoutRunning = false;
        }
        
        //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;
        }
 
        // WHAT IS ACTIVE?
        pc().printf("RISE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) (pitch: %3.1f deg)[%0.2f sec] (CMD BCE: %0.1f BATT: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read(),bce().getSetPosition_mm(),batt().getSetPosition_mm());
        bce().setPosition_mm(depthLoop().getOutput());  //constantly checking the Outer Loop output to move the motors
        batt().setPosition_mm(pitchLoop().getOutput());
         
        checkMotorPositions();
         
        break;
        
// NEW DIVE AND RISE SEQUENCES 
    case POSITION_DIVE :               
        // start local state timer and init any other one-shot actions
        if (!_isTimeoutRunning) {
            pc().printf("\r\n\nstate: POSITION DIVE\r\n");
            timer.reset(); // timer goes back to zero
            timer.start(); // background timer starts running
            _isTimeoutRunning = true; 
            
            // what needs to be started?
            bce().unpause();
            batt().unpause();
            rudder().unpause();
 
            // what are the commands? (using inner loops except for heading outer loop)
            // These actions happen ONCE in the POSITION_DIVE sequence
            batt().setPosition_mm(_neutral_batt_pos_mm - _BMM_dive_offset);
            bce().setPosition_mm(_neutral_bce_pos_mm- _BMM_dive_offset);
            //rudder().setPosition_deg(_heading_command);
            
            headingLoop().setCommand(_heading_command);     //ACTIVE HEADING (mimic of dive and rise code)
            
            pc().printf("PD: BATT cmd: %3.1f\r\n",batt().getSetPosition_mm());  //get the actual commanded position
            pc().printf("PD: BCE cmd: %3.1f\r\n",bce().getSetPosition_mm());    //get the actual commanded position
            pc().printf("PD: heading cmd: %3.1f\r\n",headingLoop().getCommand());
            
            //reset max dive depth
            _max_recorded_depth_dive = -99;            //float to record max depth
            
            //triggers logger array
            _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
        }
 
        // how exit?
        // timer runs out goes to POSITION_RISE
        if (timer.read() > _timeout) {
            pc().printf("PD: timed out\r\n\n");
            _state = POSITION_RISE; //new behavior 11/17/2017
            timer.reset();
            _isTimeoutRunning = false;
        }
        
        // when you reach the dive threshold, surface
        else if (depthLoop().getPosition() > depthLoop().getCommand() - 0.5) { // including offset for low momentum approaches
            pc().printf("PD: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
            _state = POSITION_RISE;
            timer.reset();
            _isTimeoutRunning = false;
        }
 
        // what is active?
        pc().printf("PD: bce pos (cmd pos): %3.1f mm (%0.1f), batt pos: %3.1f mm (%0.1f), rudder: %f (depth: %3.1f ft, pitch: %3.1f, heading: %3.1f [cmd: %0.1f])[%0.2f sec] [imu heading: %0.1f]                                         \r", bce().getPosition_mm(),bce().getSetPosition_mm(), batt().getPosition_mm(),batt().getSetPosition_mm(), rudder().getPosition_pwm(), depthLoop().getPosition(), pitchLoop().getPosition(), headingLoop().getPosition(), headingLoop().getOutput(), timer.read(), imu().getHeading());
        
        if (depthLoop().getPosition() > _max_recorded_depth_dive) {
            _max_recorded_depth_dive = depthLoop().getPosition();    //new max depth recorded when it is larger than previous values
        }
        
        // ACTIVE RUDDER CONTROL
        rudder().setPosition_deg(headingLoop().getOutput());
        
        checkMotorPositions();
        
        break;
    
    case POSITION_RISE :
        // start local state timer and init any other one-shot actions
        
        if (!_isTimeoutRunning) {
            pc().printf("\r\n\nstate: POSITION RISE\r\n");
            timer.reset(); // timer goes back to zero
            timer.start(); // background timer starts running
            _isTimeoutRunning = true; 
            
            // what needs to be started?
            bce().unpause();
            batt().unpause();
 
            // what are the commands? (using inner loops except for heading outer loop)
            pc().printf("PR: <<DEBUG>> CMD BCE: %0.1f, CMD BMM: %0.1f)\r\n", _BCE_dive_offset, _BMM_dive_offset);
            
            batt().setPosition_mm(_neutral_batt_pos_mm + _BMM_dive_offset);          //reversing the BCE and BATT positions
            bce().setPosition_mm(_neutral_bce_pos_mm + _BCE_dive_offset);            //reversing the BCE and BATT positions
            //rudder().setPosition_deg(_heading_command);         //heading stays the same
            
            headingLoop().setCommand(_heading_command);     //ACTIVE HEADING (mimic of dive and rise code)
            
            pc().printf("PR: BATT cmd: %3.1f\r\n",batt().getSetPosition_mm());  //get the actual commanded position
            pc().printf("PR: BCE cmd: %3.1f\r\n",bce().getSetPosition_mm());    //get the actual commanded position
            pc().printf("PR: heading cmd: %3.1f\r\n",headingLoop().getCommand());
            
            //triggers logger array
            _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
        }
 
        // how exit?
        if (timer.read() > _timeout) {
            pc().printf("PR: timed out\r\n");
            _state = EMERGENCY_CLIMB;
            timer.reset();
            _isTimeoutRunning = false;
        }
        else if (depthLoop().getPosition() < 0.5) {
            pc().printf("PR: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
            _state = FLOAT_BROADCAST;
            timer.reset();
            _isTimeoutRunning = false;
        }
 
        // what is active?
        pc().printf("PR: bce pos (cmd pos): %3.1f mm (%0.1f), batt pos: %3.1f mm (%0.1f), rudder: %0.1f (depth: %3.1f ft, pitch: %3.1f, heading: %3.1f [cmd: %0.1f])[%0.2f sec] [imu heading: %0.1f]        \r", bce().getPosition_mm(),bce().getSetPosition_mm(), batt().getPosition_mm(),batt().getSetPosition_mm(), rudder().getPosition_pwm(), depthLoop().getPosition(), pitchLoop().getPosition(), headingLoop().getPosition(), headingLoop().getOutput(), timer.read(), imu().getHeading());
        
        // ACTIVE RUDDER CONTROL
        rudder().setPosition_deg(headingLoop().getOutput());
        
        checkMotorPositions();
         
        break;
// NEW DIVE AND RISE SEQUENCES 
    
    case FLOAT_LEVEL :
        // start local state timer and init any other one-shot actions
        if (!_isTimeoutRunning) {
            pc().printf("\r\n\nstate: FLOAT_LEVEL\r\n");
            timer.reset(); // timer goes back to zero
            timer.start(); // background timer starts running
            _isTimeoutRunning = true; 
            
            // what needs to be started?
            bce().unpause();
            batt().unpause();
 
            // what are the commands?
            bce().setPosition_mm(_bceFloatPosition);
            pitchLoop().setCommand(0.0);
            
            //create the log file (works only if the file is closed)
            //createNewFile();
            
            //triggers logger array
            _is_log_timer_running = true; // reset the sub state timer to do one-shot actions again
        }
        
        // how exit?
        if (timer > _timeout) {
            pc().printf("FL: timed out\r\n");
            _state = FLOAT_BROADCAST;
            timer.reset();
            _isTimeoutRunning = false;
        }
        else if (fabs(imu().getPitch() - pitchLoop().getCommand()) < fabs(_pitchTolerance)) {         //current tolerance is 5 degrees
            pc().printf("FL: pitch: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",imu().getPitch(), pitchLoop().getCommand(), _pitchTolerance);
            _state = FLOAT_BROADCAST;
            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());
        
        break;
    
    case FLOAT_BROADCAST :
        // start local state timer and init any other one-shot actions
        if (!_isTimeoutRunning) {
            pc().printf("\r\n\nstate: FLOAT_BROADCAST\r\n");
            timer.reset(); // timer goes back to zero
            timer.start(); // background timer starts running
            _isTimeoutRunning = true; 
            
            // what needs to be started?
            bce().unpause();
            batt().unpause();
 
            // what are the commands?
            bce().setPosition_mm(_bceFloatPosition);        // 320.0
            batt().setPosition_mm(_battFloatPosition);      // 73.0
            
            //create the log file (works only if the file is closed)
            //createNewFile();
            
            //triggers logger array
            _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
        }
        
        // how exit?
        if (timer > _timeout) {
            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
                  (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());
            _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, heading(IMU): %0.1f (depthLoop POS: %3.1f ft) [%0.1f sec] (CMD BCE: %0.1f BATT: %0.1f)\r", bce().getPosition_mm(), batt().getPosition_mm(), imu().getHeading(), depthLoop().getPosition(), timer.read(), bce().getSetPosition_mm(),batt().getSetPosition_mm());
        
        break;
        
    case MULTI_DIVE :
        // start local state timer and init any other one-shot actions        
        if (!_isTimeoutRunning) {
            pc().printf("\r\n\nstate: MULTI-DIVE\r\n");
            timer.reset(); // timer goes back to zero
            timer.start(); // background timer starts running
            _isTimeoutRunning = true; 
                
            // what needs to be started?
            bce().unpause();
            batt().unpause();
            
            //retrieve commands from structs (loaded from sequence.cfg file)
            float sequence_depth_command = currentStateStruct.depth;
            float sequence_pitch_command = currentStateStruct.pitch;
    
            // what are the commands?            
            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());
            
            //no max depth recording right now
            
            //triggers logger array
            _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
        }
        
        // how exit?
        if (timer > _timeout) {
            pc().printf("\r\n\nMULTI-DIVE: timed out [time: %0.1f]\r\n\n", timer.read());
            _state = MULTI_RISE; //new behavior 11/17/2017
            timer.reset();
            _isTimeoutRunning = false;
        }
        else if (depthLoop().getPosition() > depthLoop().getCommand()) {
            pc().printf("MULTI-DIVE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
            _state = MULTI_RISE;
            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());
        batt().setPosition_mm(pitchLoop().getOutput());
        
        checkMotorPositions();
        
        break;
    
    case MULTI_RISE :
        // start local state timer and init any other one-shot actions
        if (!_isTimeoutRunning) {
            pc().printf("\r\n\nstate: MULTI-RISE\r\n");
            timer.reset(); // timer goes back to zero
            timer.start(); // background timer starts running
            _isTimeoutRunning = true; 
            
            // 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
            stateMachine().getDiveSequence();
            
            //retrieve just pitch command from struct
            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_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();
            
            //triggers logger array
            _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again
        }
        
        // how exit?
        if (timer > _timeout) {
            pc().printf("MULTI-RISE: timed out [time: %0.1f]\r\n\n", timer.read());
            _state = EMERGENCY_CLIMB;
            timer.reset();
            _isTimeoutRunning = false;
            
            //reset multi-dive sequence to start
            _multi_dive_counter = 0;
            
//            //Reload the dive sequence on exit
//            sequenceController().loadSequence();
        }
        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            
            _isTimeoutRunning = false;
            
            //successful dive-rise sequence CONTINUES the multi-dive sequence
            _multi_dive_counter++;
            
            //UPDATE THE SEQUENCE DATA HERE
            stateMachine().getDiveSequence();
            
            //check if this is the end of the dive sequence
            //CHECK BEFORE ANYTHING ELSE that you have reached the "exit" state (FLOAT_BROADCAST)
            if (currentStateStruct.state == FLOAT_BROADCAST) {
//                //Reload the dive sequence on exit
//                sequenceController().loadSequence();
            
                _state = FLOAT_BROADCAST;
            }
            
            else 
                _state = MULTI_DIVE;    //Note: need to test if this else statement is necessary
            
            //have to stop this with the _multi_dive_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());
        bce().setPosition_mm(depthLoop().getOutput());  //constantly checking the Outer Loop output to move the motors
        batt().setPosition_mm(pitchLoop().getOutput()); 
        
        checkMotorPositions();
        
        break; 
        
    case TRANSMIT_MBED_LOG:        
        if (!_isTimeoutRunning) {            
            pc().printf("\r\n\nstate: TRANSMIT_MBED_LOG\r\n");
            timer.reset(); // timer goes back to zero
            timer.start(); // background timer starts running
            _isTimeoutRunning = true; 
            
            //mbedLogger().openFileForTransmit();     //starts _fp file pointer at the beginning of the file
            
            pc().printf("TRANSMIT_MBED_LOG set to zero\n\r");
            
            //get number of packets (lines in the file)
            mbedLogger().getNumberOfPacketsInCurrentLog();
            
            mbedLogger().setTransmitPacketNumber(0); //reset to zero
        }
        
        if (timer.read() > _timeout) {
            //mbedLogger().closeLogFile(); //on timeout close the log file that was opened for reading
            //pc().printf("\r\nTRANSMIT_MBED_LOG: timed out!\r\n");
            
            //STATE
            _state = SIT_IDLE;
            
            timer.reset();
            _isTimeoutRunning = false;
            
            //send a command to the Python GUI to stop looking for data
            //SEND COMMAND WHEN TRANSMISSION ENDS PREMATURELY
            //pc().printf("%c%c%c%c",0x15,0x15,0x15,0x15);
            
            mbedLogger().closeLogFile();   //test 6/27
            pc().printf("\r\n\nstate: TRANSMIT_MBED_LOG (log filed closed)\r\n");
        }
        
        //IF THIS IS ZERO
//        if (mbedLogger().currentPacketNumber() > transmit_packet_number) {
//            pc().printf("mbedLogger().currentPacketNumber() > transmit_packet_number");    
//            
//            //STATE
//            _state = SIT_IDLE;
//            
//            timer.reset();
//            _isTimeoutRunning = false;
//            
//            mbedLogger().closeLogFile(); //on timeout close the log file that was opened for reading
//        }
        
        //What is active?
        
        //boolean function that transmits until it's finished
        if (mbedLogger().fsmTransmitData())
            ;
        else {
            mbedLogger().closeLogFile();
            pc().printf("StateMachine: TRANSMIT_MBED_LOG (closeLogFile)\n\r");
            _state = SIT_IDLE;
        }
        
        break; 
        
    case RECEIVE_SEQUENCE :
        pc().printf("state: RECEIVE_SEQUENCE\r\n");
    
        if (!_isTimeoutRunning) {
            pc().printf("RECEIVE_SEQUENCE _isTimeoutRunning\r\n");
            timer.reset(); // timer goes back to zero
            timer.start(); // background timer starts running
            _isTimeoutRunning = true; 
        }
        
        if (timer.read() > _timeout) {
            pc().printf("RECEIVE_SEQUENCE: timed out!\r\n");
            _state = SIT_IDLE;
            timer.reset();
            _isTimeoutRunning = false;
        }
        
        // what is active?
        pc().printf("Receive sequence active?\r\n");
        
        break;
    
    default :
        pc().printf("DEBUG: SIT_IDLE\r\n");
        _state = SIT_IDLE;
    }
    
    //save the state to print to user
    if (_previous_state != _state) {
        _state_array[_state_array_counter] = _state;  //save to state array
        _state_array_counter++;
        
        _previous_state = _state;
    }
    
    return _state;
//    //if the state is SIT_IDLE, return 0 / false (for recording)
//    if (_state == SIT_IDLE)
//        return 0;
//    else
//        return 1;   //return true to indicate that you're recording
}
 
// output the keyboard menu for user's reference
void StateMachine::showSimpleMenu() {   
    pc().printf("\r\r\n\nSIMPLE KEYBOARD MENU (06/27/2018):\r\r\n");        //make sure depth sensor tares itself on startup
    pc().printf(" Neutral Positions BCE: %0.1f BMM: %0.1f \r\n", _neutral_bce_pos_mm, _neutral_batt_pos_mm);
    
    pc().printf(" V to POSITION DIVE (initiate motor position-based dive cycle)\r\n");
    pc().printf(" J to float level\r\n");
    pc().printf(" B to float at broadcast pitch\r\n");
    pc().printf(" E to initiate emergency climb\r\n");
    pc().printf(" P to print the current log file.\r\n");
    pc().printf(" O to transmit current log file.\r\n");
    pc().printf(" G to MBED_TRANSMIT_LOG\r\n");
    pc().printf(" ~ to erase mbed log file. (clear before logging more than a few runs)\r\n");
    
    pc().printf("k/l to decrease/increase BCE offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BCE_dive_offset, _neutral_bce_pos_mm - _BCE_dive_offset, _neutral_bce_pos_mm + _BCE_dive_offset);
    pc().printf(";/' to decrease/increase BMM offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BMM_dive_offset, _neutral_batt_pos_mm - _BMM_dive_offset, _neutral_batt_pos_mm + _BMM_dive_offset);  
    pc().printf("9/0 to decrease/increase heading setpoint: %0.1f deg\r\n",_heading_command);
    pc().printf("A/S to decrease/increase depth setpoint: %3.1f (Current depth: %0.1f ft)\r\n",_depth_command, depthLoop().getPosition());
    
    pc().printf("T to enter in the timeout (default is 60 seconds): %d s\r\n",_timeout);    
    
    pc().printf(" C See sensor readings (and max recorded depth of dive & neutral sequences)\r\n");
    pc().printf(" 8 STREAM SENSOR STATUS (and channel readings)\r\n");
    
    pc().printf(" '|' to tare the depth sensor (vertical bar)\r\n");
    
    pc().printf(" ? to reset mbed\r\n");
    pc().printf(" * (asterisk) to go to DEBUG keyboard menu\r\n");
}

void StateMachine::showDebugMenu() {
    pc().printf("\r\r\n\nDEBUG KEYBOARD MENU (06/27/2018):\r\r\n");
    pc().printf(" Y to go into CHECK NEUTRAL TUNING (This is on a timer! Uses NEUTRAL positions!)\r\n");
    pc().printf(" V to POSITION DIVE (initiate motor position-based dive cycle)\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");
    pc().printf(" R to initiate rise\r\n");
    pc().printf(" J to float level\r\n");
    pc().printf(" B to float at broadcast pitch\r\n");
    pc().printf(" E to initiate emergency climb\r\n");
    pc().printf(" '|' to tare the depth sensor (vertical bar)\r\n");
    pc().printf(" Z to show FSM and sub-FSM states.\r\n");
    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(" I to receive data.\r\n");
    pc().printf(" O to transmit data.\r\n");
    pc().printf(" ~ to erase mbed log file. (clear before logging more than a few runs)\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 (Current depth: %0.1f ft)\r\n",_depth_command, depthLoop().getPosition());
    pc().printf("9/0 to decrease/increase heading setpoint: %0.1f deg\r\n",_heading_command);
    pc().printf("k/l to decrease/increase BCE offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BCE_dive_offset, _neutral_bce_pos_mm - _BCE_dive_offset, _neutral_bce_pos_mm + _BCE_dive_offset);
    pc().printf(";/' to decrease/increase BMM offset: %3.1f (Dive POS: %0.1f, Rise POS: %0.1f)\r\n",_BMM_dive_offset, _neutral_batt_pos_mm - _BMM_dive_offset, _neutral_batt_pos_mm + _BMM_dive_offset);  
    pc().printf("T to enter in the timeout (default is 60 seconds): %d s\r\n",_timeout);
    
// FIXED WITH NEW CODE
    pc().printf(" 1 BCE PID sub-menu\r\n");
    pc().printf(" 2 BATT PID sub-menu\r\n");
    pc().printf(" 3 Depth PID sub-menu\r\n");
    pc().printf(" 4 Pitch PID sub-menu\r\n");
    pc().printf(" 5 Rudder (servo) sub-menu\r\n");
    pc().printf(" 6 HEADING PID sub-menu\r\n");
    pc().printf(" 7 MANUAL_TUNING sub-menu (does not have a timer!)  *** MOTORS ARE ACTIVE ***\r\n");
    //pc().printf(" 8 VIEW_OUTPUTS sub-menu (does not a have a timer!)\r\n");
    pc().printf(" 8 STREAM SENSOR STATUS (and channel readings)\r\n");
    
// FIXED WITH NEW CODE

    
    pc().printf(" C See sensor readings (and max recorded depth of dive & neutral sequences)\r\n");
    pc().printf(" ? to reset mbed\r\n");
    pc().printf(" * (asterisk) to go to SIMPLE keyboard menu\r\n");
}
 
//FIND_NEUTRAL Sub-Finite State Machine (sub-FSM)
// Note: the sub-FSM only moves the pistons once at the start of each timer loop
//  (timer completes, moves piston, timer completes, moves piston, etc)
int StateMachine::runNeutralStateMachine() {                
    switch (_substate) {
        case NEUTRAL_SINKING :
            //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) (getSetPosition: %0.1f)\r\n", _neutral_timer, timer.read(), pitchLoop().getPosition(), bce().getSetPosition_mm());
                
                // what are the commands? (BCE linear actuator active, no pitch movement)
                bce().setPosition_mm(bce().getSetPosition_mm() - 2.5);  //Troy: There is some strange error where this has to be a hardcoded number.
                
                pc().printf("NEUTRAL_SINKING: Retracting piston %0.1f mm [BCE CMD : %0.1f] [pitch cmd: %0.1f] (pitch: %0.1f)\r\n", _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("\r\nDEBUG: BCE current position is %0.1f mm (NEXT SUBSTATE NEUTRAL EXIT)\r\n", 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 at start)
            pc().printf("BCE current pos: %0.1f mm (BCE setpoint: %0.1f mm) (current depth: %0.1f ft)\r", bce().getPosition_mm(),bce().getSetPosition_mm(),depthLoop().getPosition()); //debug
            
            checkMotorPositions();  //failsafe
            break;
            
        case NEUTRAL_SLOWLY_RISE:
            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_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 (default: extend 2.0 mm)
                bce().setPosition_mm(bce().getSetPosition_mm() + 2.0);  //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 %0.1f mm [BCE CMD : %0.1f] [pitch cmd: %0.1f]\r\n", _neutral_rise_command_mm, bce().getSetPosition_mm(), pitchLoop().getCommand());

                _isSubStateTimerRunning = true;    //disable this block after one iteration
            }
            
            // how exit?
            //once at full travel limit (setPosition) and haven't yet risen, time to give up and exit
            if (bce().getSetPosition_mm() >= bce().getTravelLimit()) {
                _substate = NEUTRAL_EXIT;     
                _isSubStateTimerRunning = false; // reset the sub state timer
            }
            //depth rate or sink rate < 0 ft/s, go to the next substate the next iteration
            else if (depthLoop().getVelocity() < 0) { //less than zero ft/s
                pc().printf("\r\n\nNEUTRAL_SLOWLY_RISE: Sink Rate < 0 ft/s [time: %0.1f]\r\n", timer.read());
                _substate = NEUTRAL_CHECK_PITCH;
                _isSubStateTimerRunning = false; // reset the sub state timer
            }
            
            // 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! [timer: %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)
            pc().printf("depthLoop getOutput: %0.1f\r", depthLoop().getOutput()); //debug
            bce().setPosition_mm(depthLoop().getOutput());
            
            checkMotorPositions();  //failsafe
            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() + 0.5); // move battery forward (using setpoint from linear actuator)
                    pc().printf("\r\nNeutral Check Pitch: moving battery FWD in %0.1f mm increments\r\n\n", _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("\r\nNeutral Check Pitch: moving battery AFT in %0.1f mm increments\r\n\n", _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)\r\n");    //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("\r\n\n>>> Saving Positions: BCE: %0.1f mm, BATT: %0.1f <<<\r\n\n",_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("\r\nDid not find NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH, how did I get here?!\r\n");
                    _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
            }
            
            checkMotorPositions();  //failsafe
            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\r\n");            
            break;
            
        default :
            pc().printf("how did we get to substate: default?\r\n"); //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! *******************************\r\n\n");

        //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
    and only allows input when the state is "idle" */
    
void StateMachine::keyboard() {
    char user_input;
 
    // 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)
    
    //TEST
    int _keyboard_state = 0;   //made this a local variable because it was retaining the last keyboard state
    
    if (pc().readable() && (_state == SIT_IDLE || _state == KEYBOARD)) {        
        // get the key
        user_input = pc().getc();    
        
        //record that the keyboard was used
        _state_array[_state_array_counter] = KEYBOARD;
        _state_array_counter++;
        
        // keyboard has to reset timer each time it's used
        _isTimeoutRunning = false;
        
        // check command against desired control buttons
        
/***************************** DEBUG MENU *****************************/             
        if (_debug_menu_on) {
            
            if (user_input == 'T') {
                pc().printf("Please enter the timeout (timer) value below: \n\r");
                _timeout = fabs(getFloatUserInput());
            }
            
            if (user_input == 'D') {
                _keyboard_state = DIVE;
            }
            else if (user_input == 'N') {
                _keyboard_state = FIND_NEUTRAL;
            }
            else if (user_input == 'M') {
                //currently does not run if there is no file.
                
                //need to add method to Sequence Controller that returns -1 
                //   or some check that insures you cannot run the dive sequence without a file
                
                //load sequence from file
                _multi_dive_counter = 0;
                sequenceController().loadSequence();
                wait(1);    //test if this resets the sequence
                
                stateMachine().getDiveSequence();               //get first sequence on keyboard press
                _keyboard_state = currentStateStruct.state;
                
                pc().printf("Starting Dive Sequence Controller! (state: %d)\r\n", _keyboard_state);  //neutral sequence and dive cycles
            }
            else if (user_input == 'R') {
                _keyboard_state = RISE;
            }
            else if (user_input == 'J') {
                _keyboard_state = FLOAT_LEVEL;
            }
            else if (user_input == 'B') {
                _keyboard_state = FLOAT_BROADCAST;
            }
            else if (user_input == 'E') {
                _keyboard_state = EMERGENCY_CLIMB;
            }
            
            else if (user_input == 'Y') {
                _keyboard_state = CHECK_TUNING;
            }
            
            else if (user_input == 'V') {
                _keyboard_state = POSITION_DIVE;
            }
            
            // some debug tools below
            else if (user_input == 'P') {
                //Print current SD card log file
                //printCurrentSdLog();
                mbedLogger().printCurrentLogFile();        //print the current log file to the screen
            }
            else if (user_input == 'X') {
                mbedLogger().printMbedDirectory();        //print all log files to the screen
            }
            else if (user_input == 'O') {
                mbedLogger().continuouslyTransmitDataNoTimer();         //_keyboard_state = TRANSMIT_MBED_LOG;      //Transmit data (work in progress)
            }
            else if (user_input == 'I') {
                mbedLogger().receiveMissionDataWithTicker();    //receive sequence.txt files
            }
            else if (user_input == '~') {
                pc().printf("ERASING MBED LOG FILE\r\n"); 
                mbedLogger().eraseFile();
            }
            else if (user_input == 'Z') {
                pc().printf("FSG FSM States: \r\n");
                string string_state;
                
                for (int i = 0; i < _state_array_counter; i++) {
                    if (_state_array[i] == SIT_IDLE)
                        string_state = "SIT_IDLE              <END>";
                    else if (_state_array[i] == FIND_NEUTRAL)
                        string_state = "FIND_NEUTRAL";
                    else if (_state_array[i] == DIVE)
                        string_state = "DIVE";
                    else if (_state_array[i] == RISE)
                        string_state = "RISE";
                    else if (_state_array[i] == FLOAT_LEVEL)
                        string_state = "FLOAT_LEVEL";
                    else if (_state_array[i] == FLOAT_BROADCAST)
                        string_state = "FLOAT_BROADCAST";          
                    else if (_state_array[i] == EMERGENCY_CLIMB)
                        string_state = "EMERGENCY_CLIMB";
                    else if (_state_array[i] == MULTI_DIVE)
                        string_state = "MULTI_DIVE";
                    else if (_state_array[i] == MULTI_RISE)
                        string_state = "MULTI_RISE";
                    else if (_state_array[i] == KEYBOARD)
                        string_state = "KEYBOARD";                    
                    pc().printf("State #%d: %d (%s)\r\n", i, _state_array[i], string_state.c_str());
                }
                
                pc().printf("\r\nNeutral sub-FSM States: \r\n");
                string string_substate;
                
                for (int i = 0; i < _substate_array_counter; i++) {
                    if (_substate_array[i] == NEUTRAL_SINKING)
                        string_substate = "NEUTRAL_SINKING";
                    else if (_substate_array[i] == NEUTRAL_SLOWLY_RISE)
                        string_substate = "NEUTRAL_SLOWLY_RISE";
                    else if (_substate_array[i] == NEUTRAL_CHECK_PITCH)
                        string_substate = "NEUTRAL_CHECK_PITCH";
                    else if (_substate_array[i] == NEUTRAL_EXIT)
                        string_substate = "NEUTRAL_EXIT                  <--   ";
                    else if (_substate_array[i] == EMERGENCY_CLIMB)
                        string_substate = " -- > EMERGENCY_CLIMB  <-- ";                
                    pc().printf("Neutral Substate #%d: %d (%s)\r\n", i, _state_array[i], string_substate.c_str());
                }
                pc().printf("\r\n");  //make space between printouts
            }
            else if (user_input == '|') {
                pc().printf("taring depth sensor\r\n");
                pc().printf("Pre-tare:  press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
                wait(0.1);
                depth().tare(); // tares to ambient (do on surface)
                pc().printf("Post-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
            }
            
            else if (user_input == '[' or user_input == '{') {
                _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 (user_input == ']' or user_input == '}') {
                _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 (user_input == '<' or user_input == ',') {
                _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 (user_input == '>' or user_input == '.') {
                _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 (user_input == '?') {
                pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
                wait(0.5);
                mbed_reset();
            }
     
            // change settings        
            else if (user_input == 'Q' or user_input == 'q') {
                _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 (user_input == 'W' or user_input == 'w') {
                _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 (user_input == 'A' or user_input == 'a') {
                _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 (user_input == 'S' or user_input == 's') {
                _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 (user_input == '5') {
                keyboard_menu_RUDDER_SERVO_settings();
            }
            
            else if (user_input == '6') {
                keyboard_menu_HEADING_PID_settings();
            }  
            
            // go to tuning sub-menu
            else if (user_input == '7') {
                keyboard_menu_MANUAL_TUNING();
            }
            
            else if (user_input == '8') {
                keyboard_menu_STREAM_STATUS();  //and channel readings for debugging
            }
            
            else if (user_input == '9') {
                _heading_command -= 5.0;         //decrement the rudder setpoint
                headingLoop().setCommand(_heading_command);
                pc().printf(">>> (-) new HEADING setpoint: %0.3f deg (-)\r\n", headingLoop().getCommand());
            }
            else if (user_input == '0') {
                _heading_command += 5.0;         //increment the rudder setpoint
                headingLoop().setCommand(_heading_command);
                pc().printf(">>> (+) new HEADING setpoint: %0.3f deg (+)\r\n", headingLoop().getCommand());
            }
            else if (user_input == 'U') {
                keyboard_menu_POSITION_READINGS();
            }
            
            // go to sub-menus for the PID gains (this is blocking)
            else if (user_input == '1') {
                keyboard_menu_BCE_PID_settings();
            }
            else if (user_input == '2') {
                keyboard_menu_BATT_PID_settings();
            }
            else if (user_input == '3') {
                keyboard_menu_DEPTH_PID_settings();
            }
            else if (user_input == '4') {
                keyboard_menu_PITCH_PID_settings();
            }
            
            else if (user_input == 'C' or user_input == 'c') {
                
                pc().printf("\r\n\nCURRENT STATUS AND PARAMETERS:\r\n");
                
                // Testing out ADC
                float vref = 5.6;
                float vmeasured = 0;
                unsigned int raw = adc().readCh5();
                vmeasured = ((float)raw)/4095.0*vref;
                pc().printf("raw BCE pos: %d \r\n",adc().readCh0());
                pc().printf("raw BMM pos: %d \r\n",adc().readCh1());
                pc().printf("raw BCE current sense: %d \r\n",adc().readCh2());
                pc().printf("raw BMM current sense: %d \r\n",adc().readCh3());
                pc().printf("raw depth pressure: %d \r\n",adc().readCh4());
                pc().printf("raw vessel pressure: %d \r\n",adc().readCh5());
                pc().printf("raw battery voltage: %d \r\n",adc().readCh6());
                pc().printf("raw board current: %d \r\n",adc().readCh7());
                pc().printf("raw BCE limit switch: %d \r\n",bce().getSwitch());
                pc().printf("raw BMM limit switch: %d \r\n",batt().getSwitch());
                pc().printf("raw vessel pressure: %f %d \r\n",vmeasured,raw);
                // End of ADC Test
                
                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("\r\nNeutral 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, auto_neutral_depth: %0.1f\r\n\n",_max_recorded_depth_neutral, _max_recorded_depth_dive, _max_recorded_auto_neutral_depth);
                
                pc().printf("\r\n");
                pc().printf("bce    P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f  \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
                pc().printf("batt   P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f  \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
                pc().printf("depth  P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
                pc().printf("pitch  P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
            }

//POSITION DIVE COMMANDS
            else if (user_input == 'k') {
                _BCE_dive_offset -= 1.0;
                pc().printf("Decreased BCE dive offset to %0.1f\r\n", _BCE_dive_offset);
            }
            else if (user_input == 'l') {
                _BCE_dive_offset += 1.0;
                pc().printf("Increased BCE dive offset to %0.1f\r\n", _BCE_dive_offset);
            }
            else if (user_input == ';') {
                _BMM_dive_offset -= 1.0;
                pc().printf("Decreased BMM dive offset to %0.1f\r\n", _BMM_dive_offset);
            }
            else if (user_input == '\'') {
                _BMM_dive_offset += 1.0;
                pc().printf("Increased BMM dive offset to %0.1f\r\n", _BMM_dive_offset);
            }
//POSITION DIVE COMMANDS
            
            else if (user_input == '*') {
                pc().printf("SWITCHING TO SIMPLE MENU!\r\n"); 
                wait(2);
                _debug_menu_on = false;
            }
        }   //end of debug menu
/***************************** DEBUG MENU *****************************/
            
/***************************** SIMPLE MENU *****************************/
        else {
            if (user_input == 'T') {
                pc().printf("Please enter the timeout (timer) value below: \n\r");
                _timeout = fabs(getFloatUserInput());
            }      
            if (user_input == 'V') {
                _keyboard_state = POSITION_DIVE;
            }
            else if (user_input == 'N') {
                _keyboard_state = FIND_NEUTRAL;
            }
            else if (user_input == 'J') {
                _keyboard_state = FLOAT_LEVEL;
            }
            else if (user_input == 'B') {
                _keyboard_state = FLOAT_BROADCAST;
            }
            else if (user_input == 'E') {
                _keyboard_state = EMERGENCY_CLIMB;
            }
            
            // some debug tools below
            else if (user_input == 'P') {
                //Print current SD card log file
                //printCurrentSdLog();
                mbedLogger().printCurrentLogFile();        //print the current log file to the screen
            }
            else if (user_input == 'O') {
                mbedLogger().continuouslyTransmitDataNoTimer();
            }
            
            else if (user_input == 'G') {
                _keyboard_state = TRANSMIT_MBED_LOG;
            }
            
            else if (user_input == '~') {
                pc().printf("ERASING MBED LOG FILE\r\n"); 
                mbedLogger().eraseFile();
            }
            
            else if (user_input == 'C' or user_input == 'c') {
                
                pc().printf("\r\n\nCURRENT STATUS AND PARAMETERS:\r\n");
                
                // Testing out ADC
                float vref = 5.6;
                float vmeasured = 0;
                unsigned int raw = adc().readCh5();
                vmeasured = ((float)raw)/4095.0*vref;
                
                //same equations in mbed logger
                float press_xducer_PSI = (22.029*(5.0*adc().readCh5()/4095.0) + 10.884) * 0.145038; // Press_Xducer (on-board)
                float voltage_input = (adc().readCh6()/4095.0) * 5.0 * 7.8;   //tnh
                float current_input = adc().readCh7()/4095.0;
                
                pc().printf("raw BCE pos: %d \r\n",adc().readCh0());
                pc().printf("raw BMM pos: %d \r\n",adc().readCh1());
                pc().printf("raw BCE current sense: %d \r\n",adc().readCh2());
                pc().printf("raw BMM current sense: %d \r\n",adc().readCh3());
                pc().printf("raw depth pressure: %d \r\n",adc().readCh4());
                pc().printf("raw vessel pressure %d (internal psi: %0.1f)\r\n", adc().readCh5(),press_xducer_PSI);
                //pc().printf("raw vessel pressure: %d \r\n",adc().readCh5());
                pc().printf("raw board voltage: %d (%0.1f volts)\r\n",adc().readCh6(),voltage_input);
                pc().printf("raw board current: %d (%0.3f amps)\r\n",adc().readCh7(), current_input);
                pc().printf("raw BCE limit switch: %d \r\n",bce().getSwitch());
                pc().printf("raw BMM limit switch: %d \r\n",batt().getSwitch());
                pc().printf("raw vessel pressure: %f %d \r\n",vmeasured,raw);
                // End of ADC Test
                
                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("\r\nNeutral 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, auto_neutral_depth: %0.1f\r\n\n",_max_recorded_depth_neutral, _max_recorded_depth_dive, _max_recorded_auto_neutral_depth);
                
                pc().printf("\r\n");
                pc().printf("bce    P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f  \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
                pc().printf("batt   P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %6.1f mm, slope %0.5f  \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
                pc().printf("depth  P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
                pc().printf("pitch  P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
            }

            else if (user_input == 'A' or user_input == 'a') {
                _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 (user_input == 'S' or user_input == 's') {
                _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 (user_input == '?') {
                pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
                wait(0.5);
                mbed_reset();
            }
            
            else if (user_input == '|') {
                pc().printf("taring depth sensor\r\n");
                pc().printf("Pre-tare:  press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
                wait(0.1);
                depth().tare(); // tares to ambient (do on surface)
                pc().printf("Post-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
            }
            
//POSITION DIVE COMMANDS
            else if (user_input == 'k') {
                _BCE_dive_offset -= 1.0;
                pc().printf("Decreased BCE dive offset to %0.1f\r\n", _BCE_dive_offset);
            }
            else if (user_input == 'l') {
                _BCE_dive_offset += 1.0;
                pc().printf("Increased BCE dive offset to %0.1f\r\n", _BCE_dive_offset);
            }
            else if (user_input == ';') {
                _BMM_dive_offset -= 1.0;
                pc().printf("Decreased BMM dive offset to %0.1f\r\n", _BMM_dive_offset);
            }
            else if (user_input == '\'') {
                _BMM_dive_offset += 1.0;
                pc().printf("Increased BMM dive offset to %0.1f\r\n", _BMM_dive_offset);
            }
//POSITION DIVE COMMANDS

            else if (user_input == '9') {
                _heading_command -= 5.0;         //decrement the rudder setpoint
                headingLoop().setCommand(_heading_command);
                pc().printf(">>> (-) new HEADING setpoint: %0.3f deg (-)\r\n", headingLoop().getCommand());
            }
            else if (user_input == '0') {
                _heading_command += 5.0;         //increment the rudder setpoint
                headingLoop().setCommand(_heading_command);
                pc().printf(">>> (+) new HEADING setpoint: %0.3f deg (+)\r\n", headingLoop().getCommand());
            }

            else if (user_input == '8') {
                keyboard_menu_STREAM_STATUS();
            }
            
            else if (user_input == '*') {
                pc().printf("SWITCHING TO DEBUG MENU!\r\n"); 
                _debug_menu_on = true;
                wait(2);
            }
        }
/***************************** SIMPLE MENU *****************************/
        
        //when you read the keyboard successfully, change the state
        _state = _keyboard_state;   //set state at the end of this function
        //pc().printf("\r\n\n ********* KEYBOARD STATE: %d *********\r\n\n", _state);
    }
}

void StateMachine::keyboard_menu_STREAM_STATUS() {
    char STATUS_key;
        
    // show the menu
    pc().printf("\r\n8: STATUS DEBUG MENU (EXIT WITH 'X' !)\r\n");
    
    while (1) {
        if (pc().readable()) {
            STATUS_key = pc().getc();   //get each keystroke
        }
        
        else {
            
            wait(1);         
            
            
            pc().printf("BCE POS (CMD): %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS_psi: %0.2f [depth_ft: %0.2f], PITCH: %0.2f, HEADING: %0.2f, rudder_servo_pwm: %0.1f [0(%d),1(%d),2(%d),6(%d),4(%d),5(%d),6(%d),7(%d)] Switch: BCE(%d) BMM(%d)\r",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),depth().getPsi(),depthLoop().getPosition(),imu().getPitch(),imu().getHeading(),rudder().getPosition_pwm(),adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),adc().readCh5(),adc().readCh6(),adc().readCh7(), bce().getHardwareSwitchStatus(),batt().getHardwareSwitchStatus());
            //pc().printf("BCE POS (CMD): %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS_psi: %0.2f (unfiltered: %0.2f) [depth_ft: %0.2f], PITCH: %0.2f, HEADING: %0.2f, rudder_servo_pwm: %0.1f\r",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),depth().getPsi(),depth().getRawPSI(),depthLoop().getPosition(),imu().getPitch(),imu().getHeading(),rudder().getPosition_pwm());
            //pc().printf("\n0(%d),1(%d),2(%d),6(%d),4(%d),5(%d),6(%d),7(%d)\r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),adc().readCh5(),adc().readCh6(),adc().readCh7()); 
            
            
            //pc().printf("(set) BCE POS: %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS: %0.2f )(channel reading: %d) (volt: %0.2f) << %0.2f >> (Pressure: %0.2f (raw: %0.2f) PITCH: %0.2f HEADING: %0.2f)\r\n",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(), depthLoop().getPosition(),depth().readADCCounts(),depth().readVoltage(),depth().getPsi(),depth().getRawPSI(),depth().getRawPSI(),imu().getPitch(),imu().getHeading());
            
            //pc().printf("Neutral Buoyancy Positions: bce: %0.1f, batt: %0.1f\r\n",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
//            pc().printf("depth: %3.1f ft\r\n",depthLoop().getPosition());
//            pc().printf("pitch: %3.1f deg\r\n",imu().getPitch());
//            pc().printf("heading (rudder): %3.1f deg\r\n",rudderLoop().getPosition());                   //for heading
            
//            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("depthLoop().getOutputOffset(): %0.1f\r\n",depthLoop().getOutputOffset());
//            
//            pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand());
//            pc().printf("pitchLoop().getOutputOffset(): %0.1f\r\n",pitchLoop().getOutputOffset());   
            
            continue; // didn't get a user input, so keep waiting for it
        }
    
        // process the keys            
        if (STATUS_key == 'X') {  
            pc().printf("\r\nX: EXITING STATUS DEBUG MENU\r\n");             
            break;  //exit the while loop
        }
        
        else {
            pc().printf("\r\nThis key (%c) does nothing here.                                  ", STATUS_key);
        }
    }
}

void StateMachine::keyboard_menu_RUDDER_SERVO_settings() {
    //load current parameters from the rudder
    float rudder_min_pwm = rudder().getMinPWM();
    float rudder_max_pwm = rudder().getMaxPWM();
    float rudder_ctr_pwm = rudder().getCenterPWM();
    float rudder_min_deg = rudder().getMinDeg();
    float rudder_max_deg = rudder().getMaxDeg();
    
    char RUDDER_PID_key;
 
    // print the menu
    pc().printf("\r\nRUDDER (servo driver) settings (MENU)");
    pc().printf("\r\n(Adjust min/max/center PWM settings with the following keys: N and M and C");
    pc().printf("\r\n(Adjust DEGREE limit settings with the following keys: min = K, max = L");
    pc().printf("\r\n(Hit shift + X to exit w/o saving.  Hit shift + S to save.\r\n");
    pc().printf("RUDDER min pwm: %f, max pwm: %f, center pwm: %f, min deg: %f, max deg: %f\r\n", rudder().getMinPWM(), rudder().getMaxPWM(), rudder().getCenterPWM(), rudder().getMinDeg(), rudder().getMaxDeg());
 
    // handle the key presses
    while(1) {
        // get the user's keystroke from either of the two inputs
        if (pc().readable()) {
            RUDDER_PID_key = pc().getc();
        }
        else {
            wait(0.5);
            pc().printf("RUDDER min pwm: %f, max pwm: %f, center pwm: %f, min deg: %f, max deg: %f\r\n", rudder().getMinPWM(), rudder().getMaxPWM(), rudder().getCenterPWM(), rudder().getMinDeg(), rudder().getMaxDeg());
            continue; // didn't get a user input, so keep waiting for it
        }
 
    // handle the user's key input                
        if (RUDDER_PID_key == 'S') { // user wants to save the modified values
            // set global values
            rudder().setMinPWM(rudder_min_pwm);
            rudder().setMaxPWM(rudder_max_pwm);
            rudder().setCenterPWM(rudder_ctr_pwm);
            rudder().setMinDeg(rudder_min_deg);
            rudder().setMaxDeg(rudder_max_deg);
            
            // save rudder servo driver values for inner loop
            configFileIO().saveRudderData(rudder_min_deg, rudder_max_deg, rudder_ctr_pwm, rudder_min_pwm, rudder_max_pwm);
            pc().printf("RUDDER min pwm: %f, max pwm: %f, center pwm: %f, min deg: %f, max deg: %f\r\n", rudder().getMinPWM(), rudder().getMaxPWM(), rudder().getCenterPWM(), rudder().getMinDeg(), rudder().getMaxDeg());
        }
        else if (RUDDER_PID_key == 'X') {    
            break;  //exit the while loop
        }
            // MIN PWM
        else if (RUDDER_PID_key == 'N') {
            pc().printf(">> Type in rudder_min_pwm with keyboard.\r\n");
            rudder_min_pwm = getFloatUserInput();
        }
    // MAX PWM
        else if (RUDDER_PID_key == 'M') {
            pc().printf(">> Type in rudder_max_pwm with keyboard.\r\n");
            rudder_max_pwm = getFloatUserInput();
        }
    // CENTER PWM
        else if (RUDDER_PID_key == 'C') {
            pc().printf(">> Type in rudder_ctr_pwm with keyboard.\r\n");
            rudder_ctr_pwm = getFloatUserInput();
        }
    // MIN DEG
        else if (RUDDER_PID_key == 'K') {
            pc().printf(">> Type in rudder_min_deg with keyboard.\r\n");
            rudder_min_deg = getFloatUserInput();
        }
    // MAX DEG
        else if (RUDDER_PID_key == 'L') {
            pc().printf(">> Type in rudder_max_deg with keyboard.\r\n");
            rudder_max_deg = getFloatUserInput();
        }
        
        else {
            pc().printf("RUDDER SETUP: [%c] This key does nothing here.                           \r", RUDDER_PID_key);
        }
    }
}

void StateMachine::keyboard_menu_HEADING_PID_settings() {    
    char HEADING_PID_key;
    
    float heading_KP = headingLoop().getControllerP();
    float heading_KI = headingLoop().getControllerI();
    float heading_KD = headingLoop().getControllerD();    
    float heading_offset_deg = headingLoop().getOutputOffset();
 
    // print the menu
    pc().printf("\n\rHEADING (rudder outer loop) PID gain settings (MENU)");
    pc().printf("\n\r   Adjust PID settings with the following keys: P and I and D");
    pc().printf("\n\r   Adjust zero offset with O (oh)");
    pc().printf("\n\r(Hit shift + X to exit w/o saving.  Hit shift + S to save.\n\r");
    pc().printf("HEADING P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", headingLoop().getControllerP(), headingLoop().getControllerI(), headingLoop().getControllerD(), headingLoop().getOutputOffset());
 
    // handle the key presses
    while(1) {
        // get the user's keystroke from either of the two inputs
        if (pc().readable()) {
            HEADING_PID_key = pc().getc();
        }
        else {
            continue; // didn't get a user input, so keep waiting for it
        }
 
        // handle the user's key input     
        if (HEADING_PID_key == 'S') { // user wants to save the modified values
            // set global values
            headingLoop().setControllerP(heading_KP);
            headingLoop().setControllerI(heading_KI);
            headingLoop().setControllerD(heading_KD);
            headingLoop().setOutputOffset(heading_offset_deg);
            
            // save pitch PID values for outer loop (must save neutral position also)
            configFileIO().saveHeadingData(heading_KP, heading_KI, heading_KD, heading_offset_deg);    //_neutral_heading_pos_deg);
            pc().printf("HEADING P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", headingLoop().getControllerP(), headingLoop().getControllerI(), headingLoop().getControllerD(), headingLoop().getOutputOffset());
        }
        else if (HEADING_PID_key == 'X') {    
            break;  //exit the while loop
        }
        
        else if (HEADING_PID_key == 'P') {
            heading_KP = getFloatUserInput();;
        }
        else if (HEADING_PID_key == 'I') {
            heading_KI = getFloatUserInput();
        }
        else if (HEADING_PID_key == 'D') {
            heading_KD = getFloatUserInput();
        }
        else if (HEADING_PID_key == 'O') {
            heading_offset_deg = getFloatUserInput();
        }
        
        else {
            pc().printf("HEADING SETUP: [%c] This key does nothing here.                           \r", HEADING_PID_key);
        }
    }
}

void StateMachine::keyboard_menu_COUNTS_STATUS() {
    
}

void StateMachine::keyboard_menu_MANUAL_TUNING() {
    char TUNING_key;
        
    // show the menu
    pc().printf("\r\n7: MANUAL TUNING MENU (EXIT WITH 'X' !) (Pause and Unpause rudder ticker with P and U\n");
    pc().printf("\r\n(Adjust BCE and BATT positions in real-time.  Timeout NOT running! (decrease/increase BCE with A/S, BATT with Q/W, RUDDER with E/R)\r\n");
    pc().printf("\r\nMANUAL_TUNING: BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: % 0.1f deg, IMU heading: %0.1f deg)                                                  \n\n\r",bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(), imu().getHeading());
    
    //made these into internal parameters
    float _tuning_bce_pos_mm = 300.0;   //safe starting position
    float _tuning_batt_pos_mm = 60.0;   //safe starting position
    float _tuning_rudder_pos_deg = 0.0; //safe starting position
    
    //immediately start at those positions
    bce().setPosition_mm(_tuning_bce_pos_mm);
    batt().setPosition_mm(_tuning_batt_pos_mm);
    rudder().setPosition_deg(_tuning_rudder_pos_deg);
    
    // what needs to be started?
    bce().unpause();    //this is now active
    batt().unpause();   //this is now active
    rudder().unpause();
    
    while (1) {                
        if (pc().readable()) {
            TUNING_key = pc().getc();   //get each keystroke
        }
        
        else {   
            pc().printf("MT: POS (CMD) BCE %3.1f mm (%3.1f mm), BATT %3.1f mm (%3.1f mm) SERVO: %0.1f deg, (%0.1f pwm) PITCH: %0.1f,HEADING: %0.1f                                  \r", bce().getPosition_mm(), bce().getSetPosition_mm(), batt().getPosition_mm(), batt().getSetPosition_mm(), rudder().getPosition_deg(), rudder().getPosition_pwm(),imu().getPitch(),imu().getHeading());         
            continue; // didn't get a user input, so keep waiting for it
        }
    
        // process the keys            
        if (TUNING_key == 'X') {    
            // STOP THE MOTORS BEFORE LEAVING! (Just in case.)
            bce().pause();
            batt().pause();
            rudder().pause();
            
            //right now the rudder is always active................................................hmm
            //deactivate the pin? new/delete?
            
            break;  //exit the while loop
        }
        
        //Buoyancy Engine
        else if (TUNING_key == 'A') {
            _tuning_bce_pos_mm = _tuning_bce_pos_mm - 1.0;
            bce().setPosition_mm(_tuning_bce_pos_mm);              //this variable is loaded from the file at initialization
            pc().printf("\r\nMANUAL_TUNING: (BCE CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",bce().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
        }
        
        else if (TUNING_key == 'S') {
            _tuning_bce_pos_mm = _tuning_bce_pos_mm + 1.0;
            bce().setPosition_mm(_tuning_bce_pos_mm);              //this variable is loaded from the file at initialization
            pc().printf("\r\nMANUAL_TUNING: (BCE CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",bce().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
        }
        
        //BATTERY
        else if (TUNING_key == 'Q') {
            _tuning_batt_pos_mm = _tuning_batt_pos_mm - 1.0;
            batt().setPosition_mm(_tuning_batt_pos_mm);              //this variable is loaded from the file at initialization
            pc().printf("\r\nMANUAL_TUNING: (BATT CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",batt().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
        }
        
        else if (TUNING_key == 'W') {
            _tuning_batt_pos_mm = _tuning_batt_pos_mm + 1.0;
            batt().setPosition_mm(_tuning_batt_pos_mm);              //this variable is loaded from the file at initialization
            pc().printf("\r\nMANUAL_TUNING: (BATT CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",batt().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
        }
        
        else if (TUNING_key == 'c' or TUNING_key == 'C') {
            pc().printf("MANUAL_TUNING: (current stats) BCE pos: %3.1f mm (cmd: %3.1f mm), BATT pos: %3.1f mm (cmd: %3.1f mm) SERVO: %0.1f deg, %f pwm\r", bce().getPosition_mm(), bce().getSetPosition_mm(), batt().getPosition_mm(), batt().getSetPosition_mm(), rudder().getPosition_deg(), rudder().getPosition_pwm());
        }
        
        //RUDER
        else if (TUNING_key == 'E') {
            _tuning_rudder_pos_deg = _tuning_rudder_pos_deg - 0.5;
            rudder().setPosition_deg(_tuning_rudder_pos_deg);
            pc().printf("MANUAL_TUNING: RUDDER CHANGE %0.1f deg [servo pwm: %f, %0.1f deg] (headingLoop heading: % 0.1f deg, IMU heading: %0.1f deg)\r\n", _tuning_rudder_pos_deg, rudder().getPosition_pwm(), rudder().getPosition_deg(), headingLoop().getPosition(), imu().getHeading());
        }
        
        else if (TUNING_key == 'R') {
            _tuning_rudder_pos_deg = _tuning_rudder_pos_deg + 0.5;
            rudder().setPosition_deg(_tuning_rudder_pos_deg);
            pc().printf("MANUAL_TUNING: RUDDER CHANGE %0.1f deg [servo pwm: %f, %0.1f deg] (headingLoop heading: % 0.1f deg, IMU heading: %0.1f deg)\r\n", _tuning_rudder_pos_deg, rudder().getPosition_pwm(), rudder().getPosition_deg(), headingLoop().getPosition(), imu().getHeading());
        }
        
        else if (TUNING_key == 'P') {
            rudder().pause();
        }
        
        else if (TUNING_key == 'U') {
            rudder().unpause();
        }
        
        else {
            pc().printf("\r\nMANUAL_TUNING: [%c] This key does nothing here.                                  \r", TUNING_key);
        }            
    }
}

void StateMachine::keyboard_menu_CHANNEL_READINGS() {
    char TUNING_key;
        
    // show the menu
    pc().printf("\r\n8: CHANNEL READINGS (EXIT WITH 'X' !)");
    
    while (1) {
        if (pc().readable()) {
            TUNING_key = pc().getc();   //get each keystroke
        }
        
                // process the keys            
        if (TUNING_key == 'X') {    
            // STOP THE MOTORS BEFORE LEAVING! (Just in case.)
            bce().pause();
            batt().pause();
            
            break;  //exit the while loop
        }
        
        else {
            wait(0.5);                    
            pc().printf("0(%d),1(%d),2(%d),6(%d),4(%d),5(%d),6(%d),7(%d)\r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),adc().readCh5(),adc().readCh6(),adc().readCh7()); 
            continue; // didn't get a user input, so keep waiting for it
        }            
    }
}

void StateMachine::keyboard_menu_POSITION_READINGS() {
    char TUNING_key;
        
    // show the menu
    pc().printf("\r\n9: BCE and BMM POSITION READINGS (EXIT WITH 'X' !)");
    
    while (1) {
        if (pc().readable()) {
            TUNING_key = pc().getc();   //get each keystroke
        }
        
                // process the keys            
        if (TUNING_key == 'X') {    
            // STOP THE MOTORS BEFORE LEAVING! (Just in case.)
            bce().pause();
            batt().pause();
            
            break;  //exit the while loop
        }
        
        else {
            // Testing out ADC
            wait(0.5);
            float vref = 5.6;
            float vmeasured = 0;
            unsigned int raw = adc().readCh5();
            vmeasured = ((float)raw)/4095.0*vref;
            
                    
            //pc().printf("BCE POS(%d),BMM POS(%d), BCE CUR(%d), BMM CUR(%d), Depth Pressure (%d) \r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4());
            
            pc().printf("BCE POS(%d), BMM POS(%d), BCE CUR(%d), BMM CUR(%d), Depth Pressure (%d)  << POS: BCE %0.2f, BATT %0.2f >>\r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),bce().getPosition_mm(),batt().getPosition_mm());
            
            //pc().printf("vessel pressure(%d) batt voltage(%d) board current(%d) LIMIT: BCE(%d) BMM(%d) (HW reading limit: BCE(%d) BMM (%d) \r\n",adc().readCh5(),adc().readCh6(),adc().readCh7(),bce().getSwitch(), batt().getSwitch(),bce().getSwitchState(),batt().getSwitchState());
            //pc().printf("LIMIT: BCE(%d) BMM(%d) << HW reading limit: BCE(%d) BMM (%d) >> \r\n",bce().getSwitch(), batt().getSwitch(),bce().getHWSwitchReading(),batt().getHWSwitchReading());
            //pc().printf("raw vessel pressure: %f %d \r\n",vmeasured,raw);
            // End of ADC Test
            
//            pc().printf("raw BCE pos: %d \r\n",adc().readCh0());
//            pc().printf("raw BMM pos: %d \r\n",adc().readCh1());
//            pc().printf("raw BCE current sense: %d \r\n",adc().readCh2());
//            pc().printf("raw BMM current sense: %d \r\n",adc().readCh3());
//            pc().printf("raw depth pressure: %d \r\n",adc().readCh4());
//            pc().printf("raw vessel pressure: %d \r\n",adc().readCh5());
//            pc().printf("raw battery voltage: %d \r\n",adc().readCh6());
//            pc().printf("raw board current: %d \r\n",adc().readCh7());
            
            continue; // didn't get a user input, so keep waiting for it
        }            
    }
}
 
void StateMachine::keyboard_menu_BCE_PID_settings() {    
    char PID_key;
    
    float bce_KP = bce().getControllerP();  // load current value
    float bce_KI = bce().getControllerI();  // load current global value
    float bce_KD = bce().getControllerD();  // load current global value
 
    // show the menu
    pc().printf("\n\rBuoyancy Engine PID gain settings (MENU)");
    pc().printf("\n\r(Adjust PID settings with the following keys: P and I and D");
    pc().printf("\n\r(Hit shift + X to exit w/o saving.  Hit shift + S to save.)\n\n\n\r");
    pc().printf("bce    P: %3.3f, I: %3.3f, D %3.3f, zero %d, limit %3.0f mm, slope %3.3f  \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
    
    // handle the key presses
    while(1) {
        // get the user's keystroke from either of the two inputs
        if (pc().readable()) {
            PID_key = pc().getc();
        }
        else {
            continue; // didn't get a user input, so keep waiting for it
        }
    
        // handle the user's key input
        if (PID_key == 'S') { // user wants to save these modified values
            // set values
            bce().setControllerP(bce_KP);
            bce().setControllerI(bce_KI);
            bce().setControllerD(bce_KD);  
 
            // save to "BATT.TXT" file
            configFileIO().saveBCEData(bce_KP, bce_KI, bce_KD);
            pc().printf("bce    P: %3.3f, I: %3.3f, D %3.3f, zero %d, limit %3.0f mm, slope %3.3f  \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
        }
        else if (PID_key == 'X') {    
            break;  //exit the while loop
        }
        else if (PID_key == 'P') {
            pc().printf(">> Type in proportional gain with keyboard.\n\r");
            bce_KP = getFloatUserInput();
        }
        else if (PID_key == 'I') {
            pc().printf(">> Type in integral gain with keyboard.\n\r");
            bce_KI = getFloatUserInput();
        }
        else if (PID_key == 'D') {
            pc().printf(">> Type in derivative gain with keyboard.\n\r");
            bce_KD = getFloatUserInput();
        }
        else {
            pc().printf("\n\rBCE: [%c] This key does nothing here.                                  \r", PID_key);
        }
    }
}

void StateMachine::keyboard_menu_BATT_PID_settings() {    
    char PID_key;
    
    float batt_KP = batt().getControllerP(); // load current global value
    float batt_KI = batt().getControllerI(); // load current global value
    float batt_KD = batt().getControllerD(); // load current global value
 
    // print the menu
    pc().printf("\n\rBattery Motor PID gain settings (MENU)");
    pc().printf("\n\r(Adjust PID settings with the following keys: P and I and D");
    pc().printf("\n\r(Hit shift + X to exit w/o saving.  Hit shift + S to save.)\n\n\n\r");
    pc().printf("batt    P: %3.3f, I: %3.3f, D %3.3f, zero %d, 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
        if (pc().readable()) {
            PID_key = pc().getc();
        }
        else {
            continue; // didn't get a user input, so keep waiting for it
        }
    
        // handle the user's key input
        if (PID_key == 'S') { // user wants to save these modified values
            // set values
            batt().setControllerP(batt_KP);
            batt().setControllerI(batt_KI);
            batt().setControllerD(batt_KD);  
 
            // save to "BATT.TXT" file
            configFileIO().saveBattData(batt_KP, batt_KI, batt_KD);
            pc().printf("batt    P: %3.3f, I: %3.3f, D %3.3f, zero %d, limit %3.0f mm, slope %3.3f  \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
        }
        else if (PID_key == 'X') {    
            break;  //exit the while loop
        }
        else if (PID_key == 'P') {
            pc().printf(">> Type in proportional gain with keyboard.\n\r");
            batt_KP = getFloatUserInput();
        }
        else if (PID_key == 'I') {
            pc().printf(">> Type in integral gain with keyboard.\n\r");
            batt_KI = getFloatUserInput();
        }
        else if (PID_key == 'D') {
            pc().printf(">> Type in derivative gain with keyboard.\n\r");
            batt_KD = getFloatUserInput();
        }
        else {
            pc().printf("\n\rBATT: [%c] This key does nothing here.                                  \r", PID_key);
        }
    }
}
 
void StateMachine::keyboard_menu_DEPTH_PID_settings() {    
    char PID_key;
    
    float depth_KP = depthLoop().getControllerP();       // load current depth value
    float depth_KI = depthLoop().getControllerI();       // load current depth value
    float depth_KD = depthLoop().getControllerD();       // load current depth value
 
    // print the menu
    pc().printf("\n\rDEPTH (Buoyancy Engine O.L.) PID gain settings (MENU)");
    pc().printf("\n\r(Adjust PID settings with the following keys: P and I and D");
    pc().printf("\n\r(Hit shift + X to exit w/o saving.  Hit shift + S to save.)\n\n\n\r");
    pc().printf("DEPTH P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
    
    // handle the key presses
    while(1) {
        // get the user's keystroke from either of the two inputs
        if (pc().readable()) {
            PID_key = pc().getc();
        }
        else {
            continue; // didn't get a user input, so keep waiting for it
        }
    
        // handle the user's key input
        if (PID_key == 'S') { // user wants to save these modified values
            // set values
            depthLoop().setControllerP(depth_KP);
            depthLoop().setControllerI(depth_KI);
            depthLoop().setControllerD(depth_KD);  
 
            // save to "DEPTH.TXT" file
            configFileIO().saveDepthData(depth_KP, depth_KI, depth_KD, _neutral_bce_pos_mm);
            pc().printf("DEPTH P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
            
            //set class variables that will be used in find neutral sequence
            _depth_KP = depthLoop().getControllerP();       // load current depth value
            _depth_KI = depthLoop().getControllerI();       // load current depth value
            _depth_KD = depthLoop().getControllerD();       // load current depth value
        }
        else if (PID_key == 'X') {    
            break;  //exit the while loop
        }
        else if (PID_key == 'P') {
            pc().printf(">> Type in proportional gain with keyboard.\n\r");
            depth_KP = getFloatUserInput();
        }
        else if (PID_key == 'I') {
            pc().printf(">> Type in integral gain with keyboard.\n\r");
            depth_KI = getFloatUserInput();
        }
        else if (PID_key == 'D') {
            pc().printf(">> Type in derivative gain with keyboard.\n\r");
            depth_KD = getFloatUserInput();
        }
        else {
            pc().printf("\n\rDEPTH: [%c] This key does nothing here.                                  \r", PID_key);
        }
    }
}
 
void StateMachine::keyboard_menu_PITCH_PID_settings() {    
    char PID_key;
    
    float pitch_KP = pitchLoop().getControllerP();       // load current pitch value
    float pitch_KI = pitchLoop().getControllerI();       // load current pitch value
    float pitch_KD = pitchLoop().getControllerD();       // load current pitch value
 
    // print the menu
    pc().printf("\n\rPITCH (Battery Motor O.L.) PID gain settings (MENU)");
    pc().printf("\n\r(Adjust PID settings with the following keys: P and I and D");
    pc().printf("\n\r(Hit shift + X to exit w/o saving.  Hit shift + S to save.)\n\n\n\r");
    pc().printf("PITCH P: %3.3f, I: %3.3f, D %3.3f, 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
        if (pc().readable()) {
            PID_key = pc().getc();
        }
        else {
            continue; // didn't get a user input, so keep waiting for it
        }
    
        // handle the user's key input
        if (PID_key == 'S') { // user wants to save these modified values
            // set values
            pitchLoop().setControllerP(pitch_KP);
            pitchLoop().setControllerI(pitch_KI);
            pitchLoop().setControllerD(pitch_KD);  
 
            // save to "PITCH.TXT" file
            configFileIO().savePitchData(pitch_KP, pitch_KI, pitch_KD, _neutral_batt_pos_mm);
            pc().printf("PITCH P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
                
            _pitch_KP = pitchLoop().getControllerP();       // load current pitch value
            _pitch_KI = pitchLoop().getControllerI();       // load current pitch value
            _pitch_KD = pitchLoop().getControllerD();       // load current pitch value
        }
        else if (PID_key == 'X') {    
            break;  //exit the while loop
        }
        else if (PID_key == 'P') {
            pc().printf(">> Type in proportional gain with keyboard.\n\r");
            pitch_KP = getFloatUserInput();
        }
        else if (PID_key == 'I') {
            pc().printf(">> Type in integral gain with keyboard.\n\r");
            pitch_KI = getFloatUserInput();
        }
        else if (PID_key == 'D') {
            pc().printf(">> Type in derivative gain with keyboard.\n\r");
            pitch_KD = getFloatUserInput();
        }
        else {
            pc().printf("\n\rPITCH: [%c] This key does nothing here.                                  \r", PID_key);
        }
    }
}
 
float StateMachine::getDepthCommand() {
    return _depth_command;
}
 
float StateMachine::getPitchCommand() {
    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) {
    _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) {
    _depth_command = input_depth_command;
}
 
void StateMachine::setPitchCommand(float input_pitch_command) {
    _pitch_command = input_pitch_command;
}
 
void StateMachine::setNeutralPositions(float batt_pos_mm, float bce_pos_mm) {
    _neutral_batt_pos_mm = batt_pos_mm;
    _neutral_bce_pos_mm = bce_pos_mm;
    
    pc().printf("Neutral Buoyancy Positions: batt: %0.1f, bce: %0.1f\r\n",_neutral_batt_pos_mm,_neutral_bce_pos_mm);
}
 
int StateMachine::timeoutRunning() {
    return _isTimeoutRunning;
}
 
//process one state at a time
void StateMachine::getDiveSequence() {
    //iterate through this sequence using the FSM
    currentStateStruct.state = sequenceController().sequenceStructLoaded[_multi_dive_counter].state;
    currentStateStruct.timeout = sequenceController().sequenceStructLoaded[_multi_dive_counter].timeout;
    currentStateStruct.depth = sequenceController().sequenceStructLoaded[_multi_dive_counter].depth;
    currentStateStruct.pitch = sequenceController().sequenceStructLoaded[_multi_dive_counter].pitch;
    
    _timeout = currentStateStruct.timeout;  //set timeout before exiting this function
}

void StateMachine::printCurrentSdLog() {
    pc().printf("SD card log work in progress\r\n");
    //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("\r\n\nTRANSMIT timer running...\r\n\n");    //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)\r\n", timer.read());
    }
    if (timer.read() >= transmit_timer) {
        is_transmit_timer_running = false; // reset the sub state timer to do one-shot actions again
    }
}

float * StateMachine::dataArray() {
    //return the array to a calling function
    return _data_log;
}

// 06/06/2018
float StateMachine::getFloatUserInput() {
    float float_conversion = 0.0;
    
    while(1) {
        bool valid_input = false;                   //flag for valid or invalid input
        
        pc().printf("\n\rPlease enter your number below and press ENTER:\r\n");
        char user_string [80];                      //variable to store input as a character array
    
        pc().scanf("%s", user_string);              //read formatted data from stdin
        pc().printf("\n\n\ruser_string was <%s>\r\n", user_string);
        
        //check through the string for invalid characters (decimal values 43 through 57)
        for (int c = 0; c < strlen(user_string); c++) {
            //pc().printf("character is [%c]\r\n", user_string[c]);   //debug
            if (user_string[c] >= 43 and user_string[c] <= 57) {
                //pc().printf("VALID CHARACTER!\r\n"); //debug
                ;
            }
            else {
                pc().printf("INVALID INPUT!\r\n");
                break;
            }
            
            if (c == (strlen(user_string) - 1)) {
                valid_input = true;
            }
        }
        
        if (valid_input) {
            float_conversion = atof(user_string);
            pc().printf("VALID INPUT!  Your input was: %3.3f (PRESS \"S\" (shift + S) to save!)\r\n", float_conversion);
            break;
        }
    }
    
    return float_conversion;
}

//added because channel readings failed multiple times (if they fail, motors could break parts)
void StateMachine::checkMotorPositions() {
    
    /*  1. If channel readings fail, calculations will give negative positions
        2. If the potentiometers are reporting negative positions, send the system into float broadcast
        3. The motors will move in the correct direction and if something is wrong the will go to their full extension but be at the surface */
    
    if (bce().getPosition_mm() < 0 or  batt().getPosition_mm() < 0 ) {
        //_state = FLOAT_BROADCAST;        
    }
}