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
Diff: StateMachine/StateMachine.cpp
- Revision:
- 28:16c83a2fdefa
- Parent:
- 27:0a5b90cd65d6
- Child:
- 29:6c030ac0c362
- Child:
- 30:2964617e7676
--- a/StateMachine/StateMachine.cpp Wed Nov 29 22:37:47 2017 +0000 +++ b/StateMachine/StateMachine.cpp Fri Dec 01 22:43:14 2017 +0000 @@ -4,19 +4,19 @@ StateMachine::StateMachine() { _timeout = 480; // generic timeout for every state, seconds - pitchTolerance = 1.0; // pitch angle tolerance for neutral finding exit criteria + _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) + _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) - depthCommand = 2.0; // user keyboard depth (default) - pitchCommand = -20.0; // user keyboard pitch (default) + _depthCommand = 2.0; // user keyboard depth (default) + _pitchCommand = -20.0; // user keyboard pitch (default) - _neutral_timer = 0; //timer used in FIND_NEUTRAL sub-FSM + _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 + _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; @@ -33,10 +33,17 @@ _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 = 0; + _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_FIRST_PITCH; //to start sub-FSM - _previous_substate = -1; //to start sub-FSM + _substate = NEUTRAL_FIRST_PITCH; //to start sub-FSM + _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 } //Finite State Machine (FSM) @@ -44,18 +51,19 @@ // 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 (!_isTimeoutRunning) { showMenu(); pc().printf("\r\n\nstate: SIT_IDLE\r\n"); - isTimeoutRunning = true; + _isTimeoutRunning = true; // what is active? bce().pause(); batt().pause(); //reset sub FSM - isSubStateTimerRunning = false; + _isSubStateTimerRunning = false; } // how exit? @@ -64,11 +72,11 @@ case EMERGENCY_CLIMB : // start local state timer and init any other one-shot actions - if (!isTimeoutRunning) { + 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; + _isTimeoutRunning = true; // what needs to be started? bce().unpause(); @@ -84,33 +92,39 @@ pc().printf("EC: timed out\r\n"); _state = FLOAT_BROADCAST; timer.reset(); - isTimeoutRunning = false; + _isTimeoutRunning = false; } else if (depthLoop().getPosition() < 2.0) { //if the depth is greater than 0.2 feet, go to float broadcast pc().printf("EC: depth: %3.1f, cmd: 0.5 [%0.1f sec]\r",depthLoop().getPosition(), timer.read()); _state = FLOAT_BROADCAST; timer.reset(); - isTimeoutRunning = false; + _isTimeoutRunning = false; } break; case FIND_NEUTRAL : // start local state timer and init any other one-shot actions - if (!isTimeoutRunning) { + if (!_isTimeoutRunning) { pc().printf("\r\n\nstate: FIND_NEUTRAL\n\r"); timer.reset(); // timer goes back to zero timer.start(); // background timer starts running - isTimeoutRunning = true; + _isTimeoutRunning = true; // what needs to be started? bce().unpause(); batt().unpause(); - bce().setPosition_mm(bceFloatPosition); + 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_FIRST_PITCH; _previous_substate = -1; + _substate = NEUTRAL_FIRST_PITCH; + _previous_substate = -1; + + //save this state to the array + _substate_array[_substate_array_counter] = NEUTRAL_FIRST_PITCH; //save to state array + _substate_array_counter++; + runNeutralStateMachine(); } @@ -119,11 +133,11 @@ 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; + _isTimeoutRunning = false; //record this to the NEUTRAL sub-FSM tracker - _state_array[_state_array_counter] = EMERGENCY_CLIMB; //save to state array - _state_array_counter++; + _substate_array[_substate_array_counter] = EMERGENCY_CLIMB; //save to state array + _substate_array_counter++; } //what is active? (neutral finding sub-function runs until completion) @@ -137,21 +151,24 @@ case DIVE : // start local state timer and init any other one-shot actions - if (!isTimeoutRunning) { + 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; + _isTimeoutRunning = true; // what needs to be started? bce().unpause(); batt().unpause(); // what are the commands? - depthLoop().setCommand(depthCommand); - pitchLoop().setCommand(pitchCommand); + depthLoop().setCommand(_depthCommand); + pitchLoop().setCommand(_pitchCommand); pc().printf("DIVE: depth cmd: %3.1f\r\n",depthLoop().getCommand()); pc().printf("DIVE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand()); + + //reset max dive depth + _max_recorded_depth_dive = -99; //float to record max depth } // how exit? @@ -159,36 +176,40 @@ pc().printf("DIVE: timed out\n\n\r"); _state = RISE; //new behavior 11/17/2017 timer.reset(); - isTimeoutRunning = false; + _isTimeoutRunning = false; } else if (depthLoop().getPosition() > depthLoop().getCommand()) { pc().printf("DIVE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand()); _state = RISE; timer.reset(); - isTimeoutRunning = false; + _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]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read()); bce().setPosition_mm(depthLoop().getOutput()); batt().setPosition_mm(pitchLoop().getOutput()); + + if (depthLoop().getPosition() > _max_recorded_depth_dive) { //debug + _max_recorded_depth_dive = depthLoop().getPosition(); //new max depth recorded + } break; case RISE : // start local state timer and init any other one-shot actions - if (!isTimeoutRunning) { + 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; + _isTimeoutRunning = true; // what needs to be started? bce().unpause(); batt().unpause(); // what are the commands? - depthLoop().setCommand(0.5); - pitchLoop().setCommand(-pitchCommand); + depthLoop().setCommand(-1.0); //make sure to get towards the surface (saw issues at LASR pool) + pitchLoop().setCommand(-_pitchCommand); pc().printf("RISE: depth cmd: 0.0\r\n"); pc().printf("RISE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand()); } @@ -198,13 +219,13 @@ pc().printf("RISE: timed out\r\n"); _state = EMERGENCY_CLIMB; timer.reset(); - isTimeoutRunning = false; + _isTimeoutRunning = false; } - else if (depthLoop().getPosition() < depthLoop().getCommand()) { + else if (depthLoop().getPosition() < 0.5) { //removed depthLoop().getCommand() pc().printf("RISE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand()); - _state = FLOAT_LEVEL; + _state = FLOAT_BROADCAST; timer.reset(); - isTimeoutRunning = false; + _isTimeoutRunning = false; } // what is active? @@ -215,18 +236,18 @@ case FLOAT_LEVEL : // start local state timer and init any other one-shot actions - if (!isTimeoutRunning) { + 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; + _isTimeoutRunning = true; // what needs to be started? bce().unpause(); batt().unpause(); // what are the commands? - bce().setPosition_mm(bceFloatPosition); + bce().setPosition_mm(_bceFloatPosition); pitchLoop().setCommand(0.0); } @@ -235,13 +256,13 @@ pc().printf("FL: timed out\r\n"); _state = FLOAT_BROADCAST; timer.reset(); - isTimeoutRunning = false; + _isTimeoutRunning = false; } - else if (fabs(imu().getPitch() - pitchLoop().getCommand()) < fabs(pitchTolerance)) { - pc().printf("FL: pitch: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",imu().getPitch(), pitchLoop().getCommand(), pitchTolerance); + 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; + _isTimeoutRunning = false; } // what is active? @@ -251,19 +272,19 @@ case FLOAT_BROADCAST : // start local state timer and init any other one-shot actions - if (!isTimeoutRunning) { + 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; + _isTimeoutRunning = true; // what needs to be started? bce().unpause(); batt().unpause(); // what are the commands? - bce().setPosition_mm(bceFloatPosition); - batt().setPosition_mm(battFloatPosition); + bce().setPosition_mm(_bceFloatPosition); + batt().setPosition_mm(_battFloatPosition); } // how exit? @@ -271,14 +292,14 @@ pc().printf("FB: timed out\r\n"); _state = SIT_IDLE; timer.reset(); - isTimeoutRunning = false; + _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(); - isTimeoutRunning = false; + _isTimeoutRunning = false; } // what is active? @@ -287,23 +308,23 @@ case MULTI_DIVE : // start local state timer and init any other one-shot actions - if (!isTimeoutRunning) { + 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; + _isTimeoutRunning = true; // what needs to be started? bce().unpause(); batt().unpause(); //retrieve commands from structs (loaded from sequence.cfg file) - float sequenceDepthCommand = currentStateStruct.depth; - float sequencePitchCommand = currentStateStruct.pitch; + float sequence_depthCommand = currentStateStruct.depth; + float sequence_pitchCommand = currentStateStruct.pitch; // what are the commands? - depthLoop().setCommand(sequenceDepthCommand); - pitchLoop().setCommand(sequencePitchCommand); + depthLoop().setCommand(sequence_depthCommand); + pitchLoop().setCommand(sequence_pitchCommand); pc().printf("MULTI-DIVE: depth cmd: %3.1f ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand()); } @@ -312,13 +333,13 @@ pc().printf("\n\n\rMULTI-DIVE: timed out [time: %0.1f]\n\n\r", timer.read()); _state = MULTI_RISE; //new behavior 11/17/2017 timer.reset(); - isTimeoutRunning = false; + _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; + _isTimeoutRunning = false; } // what is active? @@ -329,11 +350,11 @@ case MULTI_RISE : // start local state timer and init any other one-shot actions - if (!isTimeoutRunning) { + 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; + _isTimeoutRunning = true; // what needs to be started? bce().unpause(); @@ -344,11 +365,11 @@ stateMachine().getDiveSequence(); //retrieve just pitch command from struct - float sequencePitchCommand = currentStateStruct.pitch; + float sequence_pitchCommand = currentStateStruct.pitch; // what are the commands? (send back to 0.5 feet, not surface) // 11/21/2017 depthLoop().setCommand(0.5); - pitchLoop().setCommand(-sequencePitchCommand); + pitchLoop().setCommand(-sequence_pitchCommand); pc().printf("MULTI-RISE: depth cmd: 0.0 ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand()); } @@ -357,7 +378,7 @@ pc().printf("MULTI-RISE: timed out [time: %0.1f]\n\n\r", timer.read()); _state = EMERGENCY_CLIMB; timer.reset(); - isTimeoutRunning = false; + _isTimeoutRunning = false; //reset multi-dive sequence to start _multi_dive_counter = 0; @@ -366,7 +387,7 @@ pc().printf("MULTI-RISE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand()); //going to next state - isTimeoutRunning = false; + _isTimeoutRunning = false; //successful dive-rise sequence CONTINUES the multi-dive sequence _multi_dive_counter++; @@ -377,7 +398,7 @@ //check if this is the end of the dive sequence //CHECK BEFORE ANYTHING ELSE that you have reached the "exit" state (Float_Level) if (currentStateStruct.state == FLOAT_LEVEL) { - _state = FLOAT_LEVEL; + _state = FLOAT_BROADCAST; return; } @@ -396,7 +417,15 @@ default : pc().printf("DEBUG: SIT_IDLE\n\r"); _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; + } } // output the keyboard menu for user's reference @@ -409,49 +438,44 @@ pc().printf(" L 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(" H to run homing sequence on both BCE and Batt\r\n"); + //pc().printf(" H to run homing sequence on both BCE and Batt\r\n"); pc().printf(" T to tare the depth sensor\r\n"); + pc().printf(" Z to show FSM and sub-FSM states.\r\n"); pc().printf("[/] to change bce neutral position\r\n"); pc().printf("</> to change batt neutral position\r\n"); - pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",pitchCommand); - pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",depthCommand); + pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",_pitchCommand); + pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",_depthCommand); pc().printf("+/- to decrease/increase timeout: %d s\r\n",_timeout); 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(" C See sensor readings\r\n"); + pc().printf(" C See sensor readings (and max recorded depth of dive & neutral sequences)\r\n"); pc().printf(" ? to reset mbed\r\n"); } //Find Neutral sub finite state machine // Note: the sub-fsm only moves the pistons once at the start of each timer loop // (timer completes, move piston, timer completes, move piston, etc) -int StateMachine::runNeutralStateMachine() { - //used to log sub-FSM states - if (_substate == NEUTRAL_FIRST_PITCH) { - _state_array[_state_array_counter] = NEUTRAL_FIRST_PITCH; //save to state array - _state_array_counter++; - } - +int StateMachine::runNeutralStateMachine() { switch (_substate) { case NEUTRAL_SINKING : //start the 10 second timer - if (!isSubStateTimerRunning) { + 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]\n\r", _neutral_timer, timer.read()); + pc().printf("\r\n\nNEUTRAL_SINKING: Next retraction at %0.1f sec [current time: %0.1f] (pitch: %0.1f)\n\r", _neutral_timer, timer.read(), pitchLoop().getPosition()); // what are the commands? - //move piston at start of sequence (retract 5 mm) - bce().setPosition_mm(bce().getSetPosition_mm() - 5); //no depth command + //move piston at start of sequence (retract 2.5 mm) + bce().setPosition_mm(bce().getSetPosition_mm() - 2.5); //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_SINKING: Retracting piston 5 mm [BCE CMD : %0.1f] [pitch cmd: %0.1f]\n\r", bce().getSetPosition_mm(), pitchLoop().getCommand()); + pc().printf("NEUTRAL_SINKING: Retracting piston 5 mm [BCE CMD : %0.1f] [pitch cmd: %0.1f] (pitch: %0.1f)\n\r", bce().getSetPosition_mm(), pitchLoop().getCommand(), pitchLoop().getPosition()); - isSubStateTimerRunning = true; //disable this block after one iteration + _isSubStateTimerRunning = true; //disable this block after one iteration } // how exit? @@ -459,12 +483,12 @@ if (bce().getPosition_mm() <= 0) { pc().printf("\n\rDEBUG: BCE current position is %0.1f mm (NEXT SUBSTATE NEUTRAL EXIT)\n\r", bce().getPosition_mm()); _substate = NEUTRAL_EXIT; - isSubStateTimerRunning = false; // reset the sub state timer + _isSubStateTimerRunning = false; // reset the sub state timer } //once deeper than the commanded setpoint... - else if (depthLoop().getPosition() > depthCommand) { + else if (depthLoop().getPosition() > _depthCommand) { _substate = NEUTRAL_SLOWLY_RISE; // next state - isSubStateTimerRunning = false; //reset the sub state timer + _isSubStateTimerRunning = false; //reset the sub state timer } // what is active? @@ -472,7 +496,7 @@ if (timer.read() >= _neutral_timer) { pc().printf("\r\n\n NEUTRAL_SINKING TIMER COMPLETE! Retracting BCE piston 5 mm [current time: %0.1f]\r\n", timer.read()); - isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again + _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again } // what is active? @@ -480,7 +504,7 @@ break; case NEUTRAL_SLOWLY_RISE: - if (!isSubStateTimerRunning) { + 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()); @@ -494,20 +518,20 @@ pc().printf("NEUTRAL_SLOWLY_RISE: Extending BCE piston 5 mm [BCE CMD : %0.1f] [pitch cmd: %0.1f]\n\r", bce().getSetPosition_mm(), pitchLoop().getCommand()); - isSubStateTimerRunning = true; //disable this block after one iteration + _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 + _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 + _isSubStateTimerRunning = false; // reset the sub state timer } // what is active? @@ -515,7 +539,7 @@ if (timer.read() >= _neutral_timer) { pc().printf("\r\n\n NEUTRAL_SLOWLY_RISE TIMER COMPLETE! Extending 1 mm [timer: %0.1f]\r\n", timer.read()); - isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again + _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again } // what is active? @@ -526,7 +550,7 @@ case NEUTRAL_FIRST_PITCH : // start local state timer and init any other one-shot actions - if (!isSubStateTimerRunning) { + 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()); @@ -540,19 +564,23 @@ pc().printf("\n\rNeutral Check Pitch: moving battery AFT in 1mm increments\n\n\r"); } - isSubStateTimerRunning = true; //disable this block after one iteration + _isSubStateTimerRunning = true; //disable this block after one iteration } - // how exit? + // 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)\n\r"); //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 + } + if (_substate == NEUTRAL_FIRST_PITCH) { _substate = NEUTRAL_SINKING; // next state starts the sinking - isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again + _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again // save this neutral (not in file) for pitch pitchLoop().setOutputOffset(batt().getPosition_mm()); @@ -577,7 +605,7 @@ pc().printf("\n\rSaving Positions: BCE: %0.1f mm, BATT: %0.1f\n\n\r",_neutral_bce_pos_mm,_neutral_batt_pos_mm); _substate = NEUTRAL_EXIT; - isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again + _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again } else { @@ -591,7 +619,7 @@ 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 + _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again } break; @@ -613,11 +641,11 @@ //reset internal sub-state back to first entry conditions _substate = NEUTRAL_FIRST_PITCH; - isSubStateTimerRunning = false; // reset the sub state timer + _isSubStateTimerRunning = false; // reset the sub state timer //record sub-states to view after sequence - _state_array[_state_array_counter] = NEUTRAL_EXIT; //save to state array - _state_array_counter++; + _substate_array[_substate_array_counter] = NEUTRAL_EXIT; //save to state array + _substate_array_counter++; //reset _previous_substate on exit (has to be done in FIND_NEUTRAL if emergency exit) _previous_substate = -1; @@ -628,8 +656,8 @@ else { //record sub-states to view after sequence (when changed) if (_previous_substate != _substate) { - _state_array[_state_array_counter] = _substate; //save current state to state array - _state_array_counter++; + _substate_array[_substate_array_counter] = _substate; //save current state to state array + _substate_array_counter++; //record the current substate for comparison _previous_substate = _substate; @@ -650,12 +678,16 @@ int _keyboard_state = -1; //made this a local variable because it was retaining the last keyboard state - if (pc().readable() && _state == SIT_IDLE) { + if (pc().readable() && (_state == SIT_IDLE || _state == KEYBOARD)) { // get the key userInput = 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; + _isTimeoutRunning = false; // check command against desired control buttons if (userInput == 'D' or userInput == 'd') { @@ -687,10 +719,58 @@ else if (userInput == 'E' or userInput == 'e') { _keyboard_state = EMERGENCY_CLIMB; } - else if (userInput == 'H' or userInput == 'h') { - pc().printf("running homing procedure\r\n"); - bce().unpause(); bce().homePiston(); bce().pause(); - batt().unpause(); batt().homePiston(); batt().pause(); +// else if (userInput == 'H' or userInput == 'h') { +// pc().printf("running homing procedure\r\n"); +// bce().unpause(); bce().homePiston(); bce().pause(); +// batt().unpause(); batt().homePiston(); batt().pause(); +// } + else if (userInput == 'z' or userInput == 'Z') { + pc().printf("FSG FSM States: \n\r"); + 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)\n\r", i, _state_array[i], string_state.c_str()); + } + + pc().printf("\n\rNeutral sub-FSM States: \n\r"); + string string_substate; + + for (int i = 0; i < _substate_array_counter; i++) { + if (_substate_array[i] == NEUTRAL_FIRST_PITCH) + string_substate = "NEUTRAL_FIRST_PITCH"; + else 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)\n\r", i, _state_array[i], string_substate.c_str()); + } + pc().printf("\n\r"); //make space between printouts } else if (userInput == 'T' or userInput == 't') { pc().printf("taring depth sensor\r\n"); @@ -725,26 +805,25 @@ // change settings else if (userInput == 'Q' or userInput == 'q') { - pitchCommand -= 0.5; //decrement the pitch setpoint - pitchLoop().setCommand(pitchCommand); + _pitchCommand -= 0.5; //decrement the pitch setpoint + pitchLoop().setCommand(_pitchCommand); pc().printf(">>> new pitch angle setpoint: %0.3f deg (decreased)\r\n", pitchLoop().getCommand()); } else if (userInput == 'W' or userInput == 'w') { - pitchCommand += 0.5; //increment the pitch setpoint - pitchLoop().setCommand(pitchCommand); + _pitchCommand += 0.5; //increment the pitch setpoint + pitchLoop().setCommand(_pitchCommand); pc().printf(">>> new pitch angle setpoint: %0.3f deg (increased)\r\n", pitchLoop().getCommand()); } else if (userInput == 'A' or userInput == 'a') { - depthCommand -= 0.5; //decrement the depth setpoint - depthLoop().setCommand(depthCommand); + _depthCommand -= 0.5; //decrement the depth setpoint + depthLoop().setCommand(_depthCommand); pc().printf(">>> new depth (ft) setpoint: %0.3f ft (sink)\r\n", depthLoop().getCommand()); } else if (userInput == 'S' or userInput == 's') { - depthCommand += 0.5; //increment the depth setpoint - depthLoop().setCommand(depthCommand); + _depthCommand += 0.5; //increment the depth setpoint + depthLoop().setCommand(_depthCommand); pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand()); } - else if (userInput == '-') { _timeout -= 10.0; //decrement the timeout pc().printf(">>> timeout decreased: %d\r\n", _timeout); @@ -777,29 +856,14 @@ 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("Neutral sub FSM states: \n\r"); - string string_state; - - for (int i = 0; i < _state_array_counter; i++) { - if (_state_array[i] == NEUTRAL_FIRST_PITCH) - string_state = "NEUTRAL_FIRST_PITCH"; - else if (_state_array[i] == NEUTRAL_SINKING) - string_state = "NEUTRAL_SINKING"; - else if (_state_array[i] == NEUTRAL_SLOWLY_RISE) - string_state = "NEUTRAL_SLOWLY_RISE"; - else if (_state_array[i] == NEUTRAL_CHECK_PITCH) - string_state = "NEUTRAL_CHECK_PITCH"; - else if (_state_array[i] == NEUTRAL_EXIT) - string_state = "NEUTRAL_EXIT <-- "; - else if (_state_array[i] == EMERGENCY_CLIMB) - string_state = "EMERGENCY_CLIMB <-- "; - pc().printf("state #%d: %d (%s)\n\r", i, _state_array[i], string_state.c_str()); - } + pc().printf("Latest Neutral Buoyancy Positions: batt: %0.1f, bce: %0.1f\r\n",_neutral_batt_pos_mm,_neutral_bce_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\n\n\r",_max_recorded_depth_neutral, _max_recorded_depth_dive); } //when you read the keyboard successfully, change the state - _state = _keyboard_state; //set state at the end of this function + _state = _keyboard_state; //set state at the end of this function } } @@ -1067,16 +1131,14 @@ } float StateMachine::getDepthCommand() { - return depthCommand; + return _depthCommand; } float StateMachine::getPitchCommand() { - return pitchCommand; + return _pitchCommand; } - + void StateMachine::setState(int input_state) { - //pc().printf("input_state: %d\n\r", input_state); //debug - //_state = input_state; //changing wrong variable _state = input_state; } @@ -1089,11 +1151,11 @@ } void StateMachine::setDepthCommand(float input_depth_command) { - depthCommand = input_depth_command; + _depthCommand = input_depth_command; } void StateMachine::setPitchCommand(float input_pitch_command) { - pitchCommand = input_pitch_command; + _pitchCommand = input_pitch_command; } void StateMachine::setNeutralPositions(float batt_pos_mm, float bce_pos_mm) { @@ -1104,7 +1166,7 @@ } int StateMachine::timeoutRunning() { - return isTimeoutRunning; + return _isTimeoutRunning; } //process one state at a time