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:
- 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