most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown
Dependencies: mbed MODSERIAL FATFileSystem
StateMachine/StateMachine.cpp
- Committer:
- tnhnrl
- Date:
- 2018-06-29
- Revision:
- 71:939d179478c4
- Parent:
- 70:0e5180befedd
- Child:
- 72:250b2665755c
File content as of revision 71:939d179478c4:
#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() { // 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) printDebugMenu(); else printSimpleMenu(); 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 TX_MBED_LOG: if (!_isTimeoutRunning) { pc().printf("\r\n\nstate: TX_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("TX_MBED_LOG set to zero\n\r"); //function to read the file and get number of lines (packets to transmit) _timeout = mbedLogger().getNumberOfPacketsInCurrentLog(); mbedLogger().setTransmitPacketNumber(0); //reset to zero } //TIMED OUT if (timer.read() > _timeout) { pc().printf("\r\TX_MBED_LOG: timed out!\r\n"); //STATE _state = SIT_IDLE; timer.reset(); _isTimeoutRunning = false; mbedLogger().closeLogFile(); pc().printf("\r\n\nstate: TX_MBED_LOG (log filed closed)\r\n"); } //What is active? mbedLogger().fsmTransmitData(); mbedLogger().checkForPythonTransmitRequest(); 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::printSimpleMenu() { 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(" G to TX_MBED_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::printDebugMenu() { 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 == '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 == 'G') { wait(1); _keyboard_state = TX_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) { wait(0.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; } }