Flying Sea Glider
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