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:
- 87:6d95f853dab3
- Parent:
- 85:dd8176285b6e
- Child:
- 88:1813f583cee9
--- a/StateMachine/StateMachine.cpp Thu May 02 20:34:16 2019 +0000
+++ b/StateMachine/StateMachine.cpp Wed May 08 13:24:04 2019 +0000
@@ -9,18 +9,25 @@
_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)
-
+
+ _disconnect_batt_pos_mm = _battFloatPosition; // all the way forward
+ _batt_flying_pos_mm = 22.0;
+ _timeout_splashdown = 120; // two minutes??
+ _motorDisconnect_triggered = 0;
_depth_command = 2.0; // user keyboard depth (default)
_pitch_command = -20.0; // user keyboard pitch (default)
_heading_command = 0.0;
-
+ _start_swim_entry = SIT_IDLE;
+ _neutral_entry_state = SIT_IDLE;
//new commands
_BCE_dive_offset = 0.0; //starting at the limits
_BMM_dive_offset = 0.0;
//new commands
_neutral_timer = 0; //timer used in FIND_NEUTRAL sub-FSM
+ _neutral_success = 0; // used if find neutral has not been run and neutral_setval is also not 1 from neutral.txt file
//////////////////////////////
+
_state = SIT_IDLE; // select starting state here
_isTimeoutRunning = false; // default timer to not running
_isSubStateTimerRunning = false; // default timer to not running
@@ -79,7 +86,51 @@
_isTimeoutRunning = 1;
}
keyboard(); //keyboard function now needs to know about this state and its timeout
- break;
+ break;
+ case FLYING_IDLE: // wait for some signal and the move the battery by a known amount
+ // then wait until you drop to START_SWIM on a timeout
+ if (!_isTimeoutRunning) { // start in the correct state, tell it once where to move
+ _fsm_timer.reset();
+ _fsm_timer.start();
+ _isTimeoutRunning = 1;
+ batt().unpause();
+ batt().setPosition_mm(_batt_flying_pos_mm); // do nothing - may include not bothering to log data - yes both SIT_IDLE and FLYING_IDLE do not log 27mar2019
+ sprintf(buf, "FLYING_IDLE started in statemachine\n\n\r");
+ mbedLogger().appendDiagFile(buf,0);
+ }
+ if (motorDisconnect().read() || (_fsm_timer > 13)) { // for real case, only test is motorDisconnect().read(), timeout check is for testing only
+ _motorDisconnect_triggered = 1; // once hardware has switched once, do not need it again
+ // move the battery
+ // reset and start the timer for post disconnect wait
+ sprintf(buf, "FLYING_IDLE: motorDisconnect.read() at timeout=13sec, to end\n\r");
+ mbedLogger().appendDiagFile(buf,0);
+ _fsm_timer.reset();
+ _fsm_timer.start();
+ _isTimeoutRunning = 1;
+ batt().unpause(); //this is now active
+ batt().setPosition_mm(_disconnect_batt_pos_mm);
+
+ }
+ _timeout_splashdown = 6; // timeout_splashdown reduced to 6 seconds for testing, usually 120 sec
+ if( _motorDisconnect_triggered && _isTimeoutRunning) {
+ // how end - timeout
+ if (_fsm_timer > _timeout_splashdown) { // timeout_splashdown reduced to 6 seconds for testing, usually 120 sec
+ xbee().printf("FLYING_IDLE ended: timed out! Go to START_SWIM \r\n"); // go to start_swim
+ //tare the pressure sensor
+ depth().tare(); // tares to ambient (do on surface)
+ _state = START_SWIM; // alternately, go into the legStruct and set start_swim now? rather than leg, or leg_position_dive
+ _start_swim_entry = FLYING_IDLE;
+ sprintf(buf, "FLYING_IDLE ended on splashdown timeout go to Start_swim\n\n\r");
+ mbedLogger().appendDiagFile(buf,3);
+
+ //sprintf(buf, "FLYING_IDLE ended on splashdown timeout. FOR testing go to ENDLEG_WAIT instead of go to Start_swim\n\n\r");
+ //mbedLogger().appendDiagFile(buf,0);
+ //_state = ENDLEG_WAIT;
+ _fsm_timer.reset();
+ _isTimeoutRunning = false;
+ }
+ }
+ break;
case SIT_IDLE: // sit_idle and fb_exit fall through to keyboard actions, since they do not have break statements
case FB_EXIT:
case KEYBOARD :
@@ -177,6 +228,8 @@
_state = FLOAT_BROADCAST;
_fsm_timer.reset(); _yotimer.reset();
_isTimeoutRunning = false;
+ sprintf(buf, "EMERGENCY_CLIMB - at surface ... now go to FLOAT_BROADCAST\n\n\r");
+ mbedLogger().appendDiagFile(buf,3);
}
//WHAT IS ACTIVE?
@@ -219,8 +272,14 @@
// how exit? (exit with the timer, if timer still running continue processing sub FSM)
if (_fsm_timer > _timeout) {
xbee().printf("FN: timed out [time: %0.1f sec]\r\n", _fsm_timer.read());
+ sprintf(buf, "FIND_NEUTRAL - timed out roll=%f depth = %f now exit to EMERG_CLIMB\n\n", imu().getRoll(), depthLoop().getPosition());
+ mbedLogger().appendDiagFile(buf,3);
_state = EMERGENCY_CLIMB; //new behavior (if this times out it emergency surfaces)
_fsm_timer.reset();
+ sprintf(buf, "FN: timed out [time:%0.1f sec], _neutral_success = -1 for info to float_broadcast\n", _fsm_timer.read());
+ mbedLogger().appendDiagFile(buf,3);
+ _fsm_timer.reset();
+ _neutral_success = -1;
_isTimeoutRunning = false;
//record this to the NEUTRAL sub-FSM tracker
@@ -228,15 +287,28 @@
_substate_array_counter++;
}
+
//what is active? (neutral finding sub-function runs until completion)
//check if substate returned exit state, if so stop running the sub-FSM
- else if (runNeutralStateMachine() == NEUTRAL_EXIT) {
+
+ else if (runNeutralStateMachine() == NEUTRAL_EXIT) { // but this line will repeatedly runneutralstatemachine as long as the overall state is FIND_NEUTRAL
//if successful, FIND_NEUTRAL then goes to RISE
xbee().printf("*************************************** FIND_NEUTRAL sequence complete. Rising.\r\n\n");
- _state = RISE;
+ sprintf(buf, "FIND_NEUTRAL - at Neutral EXIT now go to RISE\n\n");
+ mbedLogger().appendDiagFile(buf,3);
+ if(_neutral_success == 0 ) { // failed in finding neutral - bad exit states
+ configFileIO().report_no_neutral_found( bce().getPosition_mm(), batt().getPosition_mm()); // this is a message "no_float.txt"
+ //to the raspberry Pi control computer
+ }
+ _state = RISE; // adjust this to leg_position_dive with an if statement - if not sent via keyboard
+ // also set flags/structures that neutral was found successfully.
+ if (_neutral_entry_state == START_SWIM ) {
+ _state = EMERGENCY_CLIMB; finish_leg =1;
+ sprintf(buf, "FN neutral_EXIT: but entry via start_swim, so radio call is next\n\n\r");
+ mbedLogger().appendDiagFile(buf,3);
+ } //exit case when not entered by keyboard
_isTimeoutRunning = false;
}
-
break;
case DIVE :
@@ -295,7 +367,128 @@
}
break;
-
+ case START_SWIM :
+ // start local state timer and init any other one-shot actions
+ if (!_isTimeoutRunning) {
+ xbee().printf("\r\n\nstate: START_SWIM\r\n");
+ _fsm_timer.reset(); // timer goes back to zero
+ _fsm_timer.start(); // background timer starts running
+ sprintf(buf, "START_SWIM begun \n\r");
+ mbedLogger().appendDiagFile(buf,3);
+ _yotimer.reset();
+ _yotimer.start();
+ _isTimeoutRunning = true;
+ //tare the pressure sensor
+ depth().tare(); // tares to ambient (do on surface) HOW certain am I that this tare only happens at surface?
+ if( fabs(imu().getRoll()) > 70 && depthLoop().getPosition() < 9.0) { //inversion operations
+ // what needs to be started?
+ bce().unpause();
+ batt().unpause();
+ rudder().unpause();
+ _depth_command = 10; // this should be meters depth
+ _heading_command = 90;
+ _timeout = 180;
+ sprintf(buf, "START_SWIM found upside down, starting to descend imu().getRoll()= %f \n\r", imu().getRoll());
+ mbedLogger().appendDiagFile(buf,3);
+
+ // what are the commands? (using inner loops except for heading outer loop)
+ // These actions happen ONCE in the POSITION_DIVE sequence
+
+ //DEPTH and HEADING COMMANDS first
+ depthLoop().setCommand(_depth_command);
+ headingLoop().setCommand(_heading_command); //ACTIVE HEADING (mimic of dive and rise code)
+
+ // move BCE and batt to new target positions
+ bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors
+ // depthloop().getOutput() will tell bce() to basically hold at neutral if you are at the desired depth ( assuming that zeroOffset for depth is correct)
+
+ batt().setPosition_mm(pitchLoop().getOutput()); //these together should hold a depth, rather than turn around and rise
+ //batt().setPosition_mm(_neutral_batt_pos_mm + _BMM_dive_offset); // dare not use neutral postions before knowing what neutral is
+ //bce().setPosition_mm(_neutral_bce_pos_mm - _BCE_dive_offset); // dare not use neutral postions before knowing what neutral is
+
+
+
+ xbee().printf("START_SWIM: inversion BATT cmd: %3.1f\r\n",batt().getSetPosition_mm()); //g
+ xbee().printf("START_SWIM: inversion BCE cmd: %3.1f\r\n",bce().getSetPosition_mm()); //g
+ xbee().printf("START_SWIM: inversion starting roll = : %3.1f\r\n",imu().getRoll() ); //g
+ } //end of inversion section
+ if( fabs(imu().getRoll()) >70 && depthLoop().getPosition() > 9.0) { //still inverted
+ // what needs to be started?
+ bce().unpause();
+ batt().unpause();
+ rudder().unpause();
+ _depth_command = 9;
+ _heading_command = 90;
+ _timeout = 120;
+ _pitch_command = 0;
+
+ //DEPTH COMMAND
+ depthLoop().setCommand(_depth_command);
+ headingLoop().setCommand(_heading_command); //ACTIVE HEADING (mimic of dive and rise code)
+
+ // what are the commands? (using inner loops except for heading outer loop)
+ // These actions happen ONCE in the POSITION_DIVE sequence
+ //batt().setPosition_mm(_neutral_batt_pos_mm - _BMM_dive_offset); // is this right ?? looks like climb commands
+ //bce().setPosition_mm(_neutral_bce_pos_mm + _BCE_dive_offset); // dare not use neutral postions before knowing what neutral is
+ bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors
+ batt().setPosition_mm(pitchLoop().getOutput()); //these together should hold a depth, rather than turn around and rise
+
+
+ xbee().printf("START_SWIM: inversion BATT cmd: %3.1f\r\n",batt().getSetPosition_mm()); //g
+ xbee().printf("START_SWIM: inversion BCE cmd: %3.1f\r\n",bce().getSetPosition_mm()); //g
+ xbee().printf("START_SWIM: inversion starting roll = : %3.1f\r\n",imu().getRoll() ); //g
+ sprintf(buf, "START_SWIM still upside down but depth>9m, set up to limit descent: depth = %f roll=%f\n\n\r", depthLoop().getPosition(), imu().getRoll());
+ mbedLogger().appendDiagFile(buf,3);
+ }
+ if( fabs(imu().getRoll()) <70 && _start_swim_entry == FLYING_IDLE ) { // starts out right side up, still inside first call to start_swim
+ finish_leg = 1; //this works to get to FB_EXIT,
+ _state = FLOAT_BROADCAST;
+ _fsm_timer.reset(); _yotimer.reset();
+ _isTimeoutRunning = false;
+ sprintf(buf, "START_SWIM now upright, called by flying_idle, so end now. roll= %f depth = %f\n\r", imu().getRoll(), depthLoop().getPosition());
+ mbedLogger().appendDiagFile(buf,3);
+ }
+ if( fabs(imu().getRoll()) <70 && _start_swim_entry != FLYING_IDLE ) { // starts out right side up, on second on/off cycle
+ _state = FIND_NEUTRAL;
+ _neutral_entry_state = START_SWIM;
+ _fsm_timer.reset(); _yotimer.reset();
+ _isTimeoutRunning = false;
+ sprintf(buf, "START_SWIM not called by FLYING_IDLE, so go to find_neutral: depth= %f\n\r", depthLoop().getPosition());
+ mbedLogger().appendDiagFile(buf,3);
+ }
+ } //end of timeout running ==0
+
+ // how exit? keep diving and watching depth while also watching imu().getRoll()
+ if ((_fsm_timer > _timeout ) || (_yotimer > _yo_time)) { // this is bad, still upside down.
+ xbee().printf("start_swim: timed out\r\n");
+ sprintf(buf, "start_swim timed out on yo_time or timeout - still upside down bad - go to FLOAT_BROADCAST roll=%f \n\n\r", imu().getRoll());
+ mbedLogger().appendDiagFile(buf,3);
+ configFileIO().report_still_inverted( fabs(imu().getRoll()), _yo_time); // tells how long you waited, puts in file "inverted.txt"
+ _state = FLOAT_BROADCAST;
+ _fsm_timer.reset(); _yotimer.reset();
+ _isTimeoutRunning = false;
+ }
+ else if (fabs(imu().getRoll()) <30 ) { // pretty much righted itself go to FIND_NEUTRAL
+ _state = FIND_NEUTRAL;
+ _neutral_entry_state = START_SWIM;
+ sprintf(buf, "START_SWIM - turned self upright roll=%f depth = %f , now go toFIND_NEUTRAL\n\n\r", imu().getRoll(), depthLoop().getPosition());
+ mbedLogger().appendDiagFile(buf,3);
+ // print message ?
+ _fsm_timer.reset(); _yotimer.reset();
+ _isTimeoutRunning = false;
+ }
+
+ //WHAT IS ACTIVE?
+ //print status to screen continuously
+ xbee().printf("START_SWIM: BcePos (cmd):%6.1f mm(%0.1f), BattPos:%6.1f mm(%0.1f), RUD_deg_cmd: %5.1f <<current depth:%6.1f m [cmd:%6.1f]), pitch:%6.1f deg [cmd:%6.1f], heading_imu:%6.1f deg>>[%0.2f sec] \r",
+ bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_deg(),
+ depthLoop().getPosition(),depthLoop().getCommand(),pitchLoop().getPosition(),pitchLoop().getCommand(),imu().getHeading(),_fsm_timer.read());
+
+ bce().setPosition_mm(depthLoop().getOutput()); //constantly checking the Outer Loop output to move the motors
+ batt().setPosition_mm(pitchLoop().getOutput()); //these together should hold a depth, rather than turn around and rise
+
+ break;
+
case RISE :
// start local state timer and init any other one-shot actions
@@ -332,6 +525,8 @@
//did not work correctly in bench test (stuck in rise state)
else if (depthLoop().getPosition() < 0.5) {
xbee().printf("RISE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
+ sprintf(buf, "in RISE: at surface ... depth =%g ... now go to FLOAT_BROADCAST\n\n\r", depthLoop().getPosition());
+ mbedLogger().appendDiagFile(buf,3);
_state = FLOAT_BROADCAST;
_fsm_timer.reset();
_isTimeoutRunning = false;
@@ -777,6 +972,8 @@
if (_fsm_timer > _timeout || ( _yotimer > _state_transition_time)) {
xbee().printf("FB: timed out\r\n");
+ sprintf(buf, "in FLOAT_BRADCAST timed out ... go to SIT_IDLE\n\n");
+ mbedLogger().appendDiagFile(buf,3);
_state = SIT_IDLE;
_fsm_timer.reset();
@@ -784,7 +981,11 @@
//mbedLogger().closeFile();
_isTimeoutRunning = false;
- if(finish_leg == 1) { _state = ENDLEG_WAIT; } // allows wait at surface for xbee messaging to not close program
+ if(finish_leg == 1) {
+ _state = ENDLEG_WAIT;
+ sprintf(buf, "in FLOAT_BROADCAST still timed out .... but also finish_leg==1 so go to ENDLEG_WAIT\n\n");
+ mbedLogger().appendDiagFile(buf,3);
+ } // allows wait at surface for xbee messaging to not close program
}
//fix on float_broadcast to account for BCE stopping early in current hardware
@@ -1095,7 +1296,8 @@
//FIND_NEUTRAL sub-Finite State Machine (sub-FSM)
// Note: the sub-FSM only moves the pistons once at the start of each timer loop
// (timer completes, moves piston, timer completes, moves piston, etc)
-int StateMachine::runNeutralStateMachine() {
+int StateMachine::runNeutralStateMachine() {
+ char buf[256];
switch (_substate) {
case NEUTRAL_SINKING :
//start the 10 second timer
@@ -1116,16 +1318,23 @@
//once reached the travel limit, no need to keep trying, so exit
if (bce().getPosition_mm() <= 0) {
xbee().printf("\r\nDEBUG: BCE current position is %0.1f mm (NEXT SUBSTATE NEUTRAL EXIT)\r\n", bce().getPosition_mm());
+ sprintf(buf, "in RUNneutral: state failed at bce.getPosition()\n\n");
+ mbedLogger().appendDiagFile(buf,3);
_substate = NEUTRAL_EXIT;
+ _neutral_success = 0;
_isSubStateTimerRunning = false; // reset the sub state timer
}
+
//Troy: Pressure vessel went beyond set depth limit, goes to next state
//once deeper than the commanded setpoint...
else if (depthLoop().getPosition() > _depth_command) {
_substate = NEUTRAL_SLOWLY_RISE; // next state
_isSubStateTimerRunning = false; //reset the sub state timer
}
-
+ else if ( (depthLoop().getVelocity() > 0.05) && (depthLoop().getPosition() > 0.6 * _depth_command) ) { // need to code in FILTERED vertical velocity
+ _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 (_fsm_timer.read() >= _neutral_timer) {
@@ -1164,7 +1373,10 @@
// 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;
+ _substate = NEUTRAL_EXIT;
+ sprintf(buf, "in RUNneutral: SLOWLY RISE state failed at bce.getsetPosition()\n\n");
+ mbedLogger().appendDiagFile(buf,3);
+ _neutral_success =0; //another failed state
_isSubStateTimerRunning = false; // reset the sub state timer
}
//Troy: Depth rate will go negative as the pressure vessel starts rising
@@ -1173,8 +1385,7 @@
xbee().printf("\r\n\nNEUTRAL_SLOWLY_RISE: Sink Rate < 0 m/s [time: %0.1f]\r\n", _fsm_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 (_fsm_timer.read() >= _neutral_timer) {
@@ -1236,13 +1447,20 @@
xbee().printf("\r\n\n>>> Saving Positions: BCE: %0.1f mm, BATT: %0.1f <<<\r\n\n",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
+ _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
_substate = NEUTRAL_EXIT;
- _isSubStateTimerRunning = false; // reset the sub state timer to do one-shot actions again
+ sprintf(buf, "in RUNneutral: state succeeded! at NEUTRAL_CHECK PITCH line 1452\n\n");
+ mbedLogger().appendDiagFile(buf,3);
+ _neutral_success = 1;
+ configFileIO().saveNeutralStatus(_neutral_success, _neutral_bce_pos_mm, _neutral_batt_pos_mm); // saves to tell re-started program neutral has been found
+
}
else {
xbee().printf("\r\nDid not find NEUTRAL_CHECK_PITCH or NEUTRAL_FIRST_PITCH, how did I get here?!\r\n");
_substate = NEUTRAL_EXIT;
+ sprintf(buf, "in RUNneutral: state failed at final fallthrough point line 1462\n\n");
+ mbedLogger().appendDiagFile(buf,3);
}
}
@@ -1328,7 +1546,7 @@
_state = FB_EXIT;
}
}
- if (xbee().readable() && (_state == SIT_IDLE || _state == KEYBOARD || _state == ENDLEG_WAIT)) { //ends at very end of function 1684
+ if (xbee().readable() && (_state == SIT_IDLE || _state == KEYBOARD || _state == ENDLEG_WAIT)) { //ends at very end of function 1740
// then get the key
user_input = xbee().getc();
// and exit from ENDLEG_WAIT - you got a keyboard input