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:
- 24:c7d9b5bf3829
- Parent:
- 23:434f04ef1fad
- Child:
- 25:249e4d56b27c
--- a/StateMachine/StateMachine.cpp Tue Nov 28 18:00:09 2017 +0000 +++ b/StateMachine/StateMachine.cpp Wed Nov 29 15:19:14 2017 +0000 @@ -2,28 +2,24 @@ #include "StaticDefs.hpp" StateMachine::StateMachine() { - _timeout = 20; // generic timeout for every state, seconds + _timeout = 480; // generic timeout for every state, seconds depthTolerance = 0.25; // depth tolerance for neutral finding exit critera pitchTolerance = 1.0; // pitch angle tolerance for neutral finding exit criteria - bceFloatPosition = 300; // bce position for "float" states - battFloatPosition = 50; // batt position for "broadcast" state + 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 = 3.5; // user keyboard depth - pitchCommand = -20.0; // user keyboard depth + depthCommand = 2.0; // user keyboard depth (default) + pitchCommand = -20.0; // user keyboard pitch (default) - _neutral_sink_timer = 0; - _neutral_rise_timer = 0; - _level_timer = 0; - - previousPosition_mm = bce().getTravelLimit(); // Default is bce fully extended, overwritten in FIND_NEUTRAL + _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_counter = 0; + _multi_dive_counter = 0; _neutral_sub_state_active = false; @@ -39,12 +35,13 @@ _neutral_batt_pos_mm = pitchLoop().getOutputOffset(); //load current neutral buoyancy position offset _state_array_counter = 0; + + _substate = NEUTRAL_FIRST_PITCH; //to start sub-FSM + _previous_substate = -1; //to start sub-FSM } //Finite State Machine (FSM) -void StateMachine::runStateMachine() { - //static bool runFirstNeutral = false; - +void StateMachine::runStateMachine() { // finite state machine ... each state has at least one exit criteria switch (_state) { case SIT_IDLE : @@ -109,13 +106,13 @@ // 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 - //load the current position of the BCE piston into the sub-FSM - //previousPosition_mm = bce().getPosition_mm(); - pc().printf("FN: Neutral sequence, BCE piston at %3.1f\n\r", bce().getPosition_mm()); - - //first iteration goes into Neutral Finding Sub-FSM - runNeutralStateMachine(); //send first state of sub-FSM + //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; + runNeutralStateMachine(); } // how exit? (exit with the timer, if timer still running continue processing sub FSM) @@ -124,17 +121,19 @@ _state = EMERGENCY_CLIMB; //new behavior (if this times out it emergency surfaces) timer.reset(); isTimeoutRunning = false; + + //record this to the NEUTRAL sub-FSM tracker + _state_array[_state_array_counter] = EMERGENCY_CLIMB; //save to state array + _state_array_counter++; } - //what is active? (neutral finding sub-function runs until completion) - if (runNeutralStateMachine() == NEUTRAL_EXIT) { - //if the current state is NEUTRAL_EXIT stop running the sub FSM - + //what is active? (neutral finding sub-function runs until completion) + //check if substate returned exit state, if so stop running the sub-FSM + if (runNeutralStateMachine() == NEUTRAL_EXIT) { //if successful, FIND_NEUTRAL then goes to RISE pc().printf("*************************************** FIND_NEUTRAL sequence complete. Rising.\n\n\r"); _state = RISE; } - break; case DIVE : @@ -362,7 +361,7 @@ isTimeoutRunning = false; //reset multi-dive sequence to start - _state_counter = 0; + _multi_dive_counter = 0; } 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()); @@ -371,7 +370,7 @@ isTimeoutRunning = false; //successful dive-rise sequence CONTINUES the multi-dive sequence - _state_counter++; + _multi_dive_counter++; //UPDATE THE SEQUENCE DATA HERE stateMachine().getDiveSequence(); @@ -386,7 +385,7 @@ else _state = MULTI_DIVE; - //have to stop this with the _state_counter variable! + //have to stop this with the _multi_dive_counter variable! } // what is active? @@ -429,51 +428,52 @@ //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() { - static int substate = NEUTRAL_FIRST_PITCH; //to start +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++; + } - //pc().printf("Neutral FSM: substate: %d [time: %0.1f] (depth: %0.1f, pitch: %0.1f, pitch rate: %0.1f) (bce: %0.1f, batt: %0.1f, prev: %0.1f)\r", substate, timer.read(), depthLoop().getPosition(), pitchLoop().getPosition(), pitchLoop().getVelocity(), bce().getPosition_mm(), batt().getPosition_mm(), previousPosition_mm); - - switch (substate) { + switch (_substate) { case NEUTRAL_SINKING : //start the 10 second timer if (!isSubStateTimerRunning) { - _neutral_sink_timer = timer.read() + 5; //record the time when this block is first entered and add 5 seconds + _neutral_timer = timer.read() + 5; //record the time when this block is first entered and add 5 seconds - pc().printf("\r\n\nSTART NEUTRAL_SINKING (next retraction at %0.1f sec) [current time: %0.1f]\n\r", _neutral_sink_timer, timer.read()); + pc().printf("\r\n\nNEUTRAL_SINKING: Next retraction at %0.1f sec [current time: %0.1f]\n\r", _neutral_timer, timer.read()); // what are the commands? - //move piston at start of sequence (retract 10 mm) - previousPosition_mm = bce().getPosition_mm() - 5; - bce().setPosition_mm(previousPosition_mm); //no depth command - pc().printf("NEUTRAL_SINKING: Retracting piston 10 mm (sent cmd: %0.1f) [CMD actual: %0.1f]\n\r",previousPosition_mm, bce().getSetPosition_mm()); + //move piston at start of sequence (retract 5 mm) + bce().setPosition_mm(bce().getSetPosition_mm() - 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: pitch cmd: %3.1f\r\n",pitchLoop().getCommand()); + + pc().printf("NEUTRAL_SINKING: Retracting 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 } // how exit? //once reached the travel limit, no need to keep trying, so exit - if (bce().getPosition_mm() <= 0) { - pc().printf("\n\rDEBUG: BCE current position is %0.1f mm (NEXT STATE NEUTRAL EXIT)\n\r", bce().getPosition_mm()); - substate = NEUTRAL_EXIT; - } +// 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 +// } //once deeper than the commanded setpoint... else if (depthLoop().getPosition() > depthCommand) { - substate = NEUTRAL_SLOWLY_RISE; // next state + _substate = NEUTRAL_SLOWLY_RISE; // next state isSubStateTimerRunning = false; //reset the sub state timer } // what is active? //once the 10 second timer is complete, reset the timeout so the state one-shot entry will move the setpoint - if (timer.read() >= _neutral_sink_timer) { - pc().printf("\r\n\n NEUTRAL_SINKING TIMER COMPLETE!"); - pc().printf("\r\n\n (BATT POS: %0.1f) retract 10 mm [current time: %3.1f]\r\n", previousPosition_mm, timer.read()); + 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 + isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again } // what is active? @@ -482,42 +482,41 @@ case NEUTRAL_SLOWLY_RISE: if (!isSubStateTimerRunning) { - _neutral_rise_timer = timer.read()+ 5; //record the time when this block is first entered and add 5 seconds + _neutral_timer = timer.read()+ 5; //record the time when this block is first entered and add 5 seconds - pc().printf("\r\n\nSTART NEUTRAL_SLOWLY_RISE (next retraction at %0.1f sec) [current time: %0.1f]\r\n",_neutral_rise_timer,timer.read()); - - isSubStateTimerRunning = true; //disable this block after one iteration + 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 (extend) - previousPosition_mm = bce().getPosition_mm() + 2; - bce().setPosition_mm(previousPosition_mm); //no depth command - pc().printf("NEUTRAL_SLOWLY_RISE: Extending piston 2 mm (sent cmd: %0.1f) [CMD actual: %0.1f]\n\r",previousPosition_mm, bce().getSetPosition_mm()); + bce().setPosition_mm(bce().getSetPosition_mm() + 2); //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: pitch cmd: %3.1f\r\n",pitchLoop().getCommand()); + + 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 } // how exit? - //once at full travel limit and haven't yet risen, time to give up and exit - if (bce().getPosition_mm() >= bce().getTravelLimit()) { - substate = NEUTRAL_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 below 0 ft/s [time: %0.1f]\r\n", timer.read()); - pc().printf("\r\n\n ###################### NEXT STATE NEUTRAL_CHECK_PITCH ###################### \r\n"); - substate = NEUTRAL_CHECK_PITCH; + 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_sink_timer) { - pc().printf("\r\n\nNEUTRAL_SLOWLY_RISE TIMER COMPLETE!"); - pc().printf("\r\n\n (BATT POS: %0.1f) extend 1 mm [timer: %0.1f]\r\n", previousPosition_mm, timer.read()); + 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 + isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again } // what is active? @@ -527,46 +526,43 @@ case NEUTRAL_CHECK_PITCH : // fall thru to next state is desired case NEUTRAL_FIRST_PITCH : // start local state timer and init any other one-shot actions - //pc().printf("\n\rNEUTRAL_FIRST_PITCH or NEUTRAL_CHECK_PITCH\n\r"); if (!isSubStateTimerRunning) { - _level_timer = timer.read() + 10; // record time when this block is entered and add several seconds - pc().printf("\r\n\nSTART NEUTRAL_CHECK_PITCH (next move in %0.1f sec)\r\n",_level_timer - timer.read()); - isSubStateTimerRunning = true; // out of the initialization block + _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? - //pc().printf("Neutral Check Pitch: moving battery in 1mm increments\n\r"); - if (imu().getPitch() > 2) { // nose is high - batt().setPosition_mm(batt().getPosition_mm() + 1); // move battery forward + 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("\n\rNeutral Check Pitch: moving battery FWD in 1mm increments\n\n\r"); } - else if (imu().getPitch() < -2) { // nose is low - batt().setPosition_mm(batt().getPosition_mm() - 1); // move battery aft + 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("\n\rNeutral Check Pitch: moving battery AFT in 1mm increments\n\n\r"); } + + 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("\n\rFound Level (NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH)\n\r"); //debug + 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 (substate == NEUTRAL_FIRST_PITCH) { - pc().printf("\n\rsubstate: NEUTRAL_FIRST_PITCH \n\r"); //debug - - substate = NEUTRAL_SINKING; // next state starts the sinking - - pc().printf("\n\rsubstate: %d \n\r", substate); //debug - + 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 + // save this neutral (not in file) for pitch pitchLoop().setOutputOffset(batt().getPosition_mm()); + pc().printf("substate: NEUTRAL_FIRST_PITCH (next substate: NEUTRAL_SINKING)\n\r"); } // found level and at depth too, so save it all now - else { + else if (_substate == NEUTRAL_CHECK_PITCH) { //save positions locally _neutral_batt_pos_mm = batt().getPosition_mm(); _neutral_bce_pos_mm = bce().getPosition_mm(); @@ -581,50 +577,66 @@ 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; + _substate = NEUTRAL_EXIT; + isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again + } + + else { + pc().printf("\n\rDid not find NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH, how did I get here?!\n\r"); + _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() >= _level_timer) { + 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 + isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again } - - //pc().printf("NEUTRAL_CHECK_PITCH (time: %3.1f) [%0.1f deg, %0.1f deg/s] \n\r", timer.read(), pitchLoop().getPosition(), pitchLoop().getVelocity()); //debug break; //this state could be removed, it is only used as a transition but is needed to stop entering this function case NEUTRAL_EXIT : - //NEUTRAL_EXIT state is used to tell the greater FSM that this sub-FSM has completed pc().printf("substate: NEUTRAL_EXIT\n\r"); break; default : - pc().printf("how did we get to substate default?\n\r"); //debug + pc().printf("how did we get to substate: default?\n\r"); //debug //a default within the sub-state machine - substate = NEUTRAL_EXIT; + _substate = NEUTRAL_EXIT; break; } - //check if the sub-FSM has completed - if (substate == NEUTRAL_EXIT) { - //reset substate to NEUTRAL_FIRST_PITCH (first state) - substate = NEUTRAL_FIRST_PITCH; - //indicate to outer FSM that sub-FSM has completed + // reset the sub-FSM if needed + if (_substate == NEUTRAL_EXIT) { + pc().printf("******************************** EXITING sub-FSM! *******************************\n\n\r"); + + //reset internal sub-state back to first entry conditions + _substate = NEUTRAL_FIRST_PITCH; + isSubStateTimerRunning = false; // reset the sub state timer - _state_array[_state_array_counter] = NEUTRAL_EXIT; //record what state the hardware is currently in - _state_array_counter++; //increment at the end of each run + //record sub-states to view after sequence + _state_array[_state_array_counter] = NEUTRAL_EXIT; //save to state array + _state_array_counter++; - pc().printf("********************************DID YOU EXIT? *******************************\n\n\r"); - return NEUTRAL_EXIT; + //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 { - _state_array[_state_array_counter] = substate; //record what state the hardware is currently in - _state_array_counter++; //increment at the end of each run - return substate; //otherwise continue running through sequence + //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++; + + //record the current substate for comparison + _previous_substate = _substate; + } + + return _substate; // message to calling function of what sub-state it's in } } @@ -743,6 +755,16 @@ pc().printf(">>> timeout increased: %d\r\n", _timeout); } + //new 11/29/2017 + else if (userInput == ';') { + _timeout -= 1.0; //decrement the timeout + pc().printf(">>> NEUTRAL sub-FSM timeout decreased: %d\r\n", _timeout); + } + else if (userInput == '\'') { + _timeout += 1.0; //increment the timeout + pc().printf(">>> NEUTRAL sub-FSM timeout increased: %d\r\n", _timeout); + } + // go to sub-menus for the PID gains (this is blocking) else if (userInput == '1') { keyboard_menu_BCE_PID_settings(); @@ -768,13 +790,29 @@ 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++) { - pc().printf("state #%d: %d\n\r", i, _state_array[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()); } } //when you read the keyboard successfully, change the state _state = _keyboard_state; //set state at the end of this function + + wait_us(100); } } @@ -1085,10 +1123,10 @@ //process one state at a time void StateMachine::getDiveSequence() { //iterate through this sequence using the FSM - currentStateStruct.state = sequenceController().sequenceStructLoaded[_state_counter].state; - currentStateStruct.timeout = sequenceController().sequenceStructLoaded[_state_counter].timeout; - currentStateStruct.depth = sequenceController().sequenceStructLoaded[_state_counter].depth; - currentStateStruct.pitch = sequenceController().sequenceStructLoaded[_state_counter].pitch; + 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 } \ No newline at end of file