update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
Diff: StateMachine/StateMachine.cpp
- Revision:
- 102:0f430de62447
- Parent:
- 97:2b4f78a54227
--- a/StateMachine/StateMachine.cpp Fri Jun 28 15:07:41 2019 +0000
+++ b/StateMachine/StateMachine.cpp Fri Sep 13 16:51:48 2019 +0000
@@ -21,8 +21,12 @@
_start_swim_entry = SIT_IDLE;
_neutral_entry_state = SIT_IDLE;
//new commands
- _BCE_dive_offset = 0.0; //starting at the limits
+ _BCE_dive_offset = 0.0; //starting at no movement
_BMM_dive_offset = 0.0;
+ bmm_dive_mm = 10.0;
+ bce_dive_mm = 20.0;
+ bce_rise_mm = 21.0;
+ bmm_rise_mm = 11.0; // non zero values but just guesses
//new commands
_neutral_timer = 0; //timer used in FIND_NEUTRAL sub-FSM
@@ -287,6 +291,20 @@
_substate_array[_substate_array_counter] = EMERGENCY_CLIMB; //save to state array
_substate_array_counter++;
}
+ else if( (depthLoop().getPosition() > configFileIO().swimConstants.altim_blank_m) // ending on altimeter abort - bad, endleg, etc
+ && (fabs(imu().getRoll()) < 45) && (altimLoop().getPosition() < configFileIO().swimConstants.altim_abort_m)) {
+ //just go to leg_position_rise unless it is the second time
+ _state = EMERGENCY_CLIMB;
+ finish_leg =1;
+ sprintf(buf, "FIND_NEUTRAL : ALTIMETER ABORT : depth=%f altimeter=%f - go to emergency climb\n\n\r",
+ depthLoop().getPosition(), altimLoop().getPosition() );
+ mbedLogger().appendDiagFile(buf,3);
+ configFileIO().swimConstants.endleg_reason = 7; // find_neutral fail
+ _fsm_timer.reset();
+ _isTimeoutRunning = false;
+
+ }
+
//what is active? (neutral finding sub-function runs until completion)
@@ -356,8 +374,13 @@
_state = RISE;
_fsm_timer.reset();
_isTimeoutRunning = false;
- }
-
+ } // add else if(depth > altimeter_blank && altimeter_depth < altimeter_abort && not_inverted) then exit with message
+ else if( (depthLoop().getPosition() > configFileIO().swimConstants.altim_blank_m) // ending on altimieter abort - bad, endleg, etc
+ && (fabs(imu().getRoll()) < 45) && (altimLoop().getPosition() < configFileIO().swimConstants.altim_abort_m)) {
+ _state = RISE;
+ _fsm_timer.reset();
+ _isTimeoutRunning = false;
+ }
// WHAT IS ACTIVE?
xbee().printf("DIVE: 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(),
@@ -472,6 +495,8 @@
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;
+ finish_leg = 1; // failure mode
+ configFileIO().swimConstants.endleg_reason = 5; //still inverted
_fsm_timer.reset(); _yotimer.reset();
_isTimeoutRunning = false;
}
@@ -483,8 +508,21 @@
// print message ?
_fsm_timer.reset(); _yotimer.reset();
_isTimeoutRunning = false;
- }
-
+ } // add else if(depth > altimeter_blank && altimeter_depth < altimeter_abort && not_inverted) then exit with message
+ else if( (depthLoop().getPosition() > configFileIO().swimConstants.altim_blank_m) // ending on altimieter abort - bad, endleg, etc
+ && (fabs(imu().getRoll()) < 45) && (altimLoop().getPosition() < configFileIO().swimConstants.altim_abort_m)) {
+ //just go to emergency climb
+ _state = EMERGENCY_CLIMB;
+ finish_leg =1;
+ sprintf(buf, "START_SWIM: ALTIMETER ABORT - : depth=%f altimeter=%f - go to emergency climb\n\n\r",
+ depthLoop().getPosition(), altimLoop().getPosition() );
+ mbedLogger().appendDiagFile(buf,3);
+ configFileIO().swimConstants.endleg_reason = 7; // altimeter abort + no_find_neutral
+ _fsm_timer.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",
@@ -493,7 +531,8 @@
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
-
+ // ACTIVE RUDDER CONTROL
+ rudder().setPosition_deg(headingLoop().getOutput());
break;
case RISE :
@@ -567,9 +606,13 @@
// 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);
- bce().setPosition_mm(_neutral_bce_pos_mm - _BCE_dive_offset);
+ bce_dive_mm = configFileIO().swimConstants.bce_dive_mm;
+ bmm_dive_mm = configFileIO().swimConstants.bmm_dive_mm;
+ // old way batt().setPosition_mm(_neutral_batt_pos_mm + _BMM_dive_offset);
+ // old way bce().setPosition_mm(_neutral_bce_pos_mm - _BCE_dive_offset);
+ batt().setPosition_mm(_neutral_batt_pos_mm + bmm_dive_mm);
+ bce().setPosition_mm(_neutral_bce_pos_mm - bce_dive_mm);
//DEPTH COMMAND
depthLoop().setCommand(_depth_command);
@@ -598,8 +641,21 @@
_state = POSITION_RISE;
_fsm_timer.reset();
_isTimeoutRunning = false;
- }
+ }// add else if(depth > altimeter_blank && altimeter_depth < altimeter_abort && not_inverted) then exit with message
+ else if( (depthLoop().getPosition() > configFileIO().swimConstants.altim_blank_m) // ending on altimieter abort - bad, endleg, etc
+ && (fabs(imu().getRoll()) < 45) && (altimLoop().getPosition() < configFileIO().swimConstants.altim_abort_m)) {
+ //just go to leg_position_rise unless it is the second time
+ _state = POSITION_RISE;
+ sprintf(buf, "POSITION_DIVE: ALTIMETER ABORT - !: depth=%f altimeter=%f - go to POSITION_RISE\n\n\r",
+ depthLoop().getPosition(), altimLoop().getPosition() );
+ mbedLogger().appendDiagFile(buf,3);
+ configFileIO().swimConstants.endleg_reason = 3; // altimeter abort
+ _fsm_timer.reset();
+ _isTimeoutRunning = false;
+
+ }
+
// what is active?
xbee().printf("POS DIVE: 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, heading_imu:%6.1f deg>>[%0.2f sec] \r",
bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_deg(),
@@ -633,9 +689,13 @@
// 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);
- bce().setPosition_mm(_neutral_bce_pos_mm - _BCE_dive_offset);
+ // old way batt().setPosition_mm(_neutral_batt_pos_mm + _BMM_dive_offset);
+ // old way bce().setPosition_mm(_neutral_bce_pos_mm - _BCE_dive_offset);
+ bce_dive_mm = configFileIO().swimConstants.bce_dive_mm;
+ bmm_dive_mm = configFileIO().swimConstants.bmm_dive_mm;
+ batt().setPosition_mm(_neutral_batt_pos_mm + bmm_dive_mm);
+ bce().setPosition_mm(_neutral_bce_pos_mm - bce_dive_mm);
//retrieve commands from structs (loaded from legfile.txt file)
stateMachine().getLegParams();
@@ -675,8 +735,14 @@
// 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);
- bce().setPosition_mm(_neutral_bce_pos_mm - _BCE_dive_offset);
+
+ bce_dive_mm = configFileIO().swimConstants.bce_dive_mm;
+ bmm_dive_mm = configFileIO().swimConstants.bmm_dive_mm;
+ // old way batt().setPosition_mm(_neutral_batt_pos_mm + _BMM_dive_offset);
+ // old way bce().setPosition_mm(_neutral_bce_pos_mm - _BCE_dive_offset);
+
+ batt().setPosition_mm(_neutral_batt_pos_mm + bmm_dive_mm);
+ bce().setPosition_mm(_neutral_bce_pos_mm - bce_dive_mm);
//retrieve commands from structs (loaded from missionleg.cfg file)
leg_max_depth = currentLegStateStruct.max_depth;
leg_heading = currentLegStateStruct.heading;
@@ -705,6 +771,7 @@
xbee().printf("LEG POSITION DIVE timed out for overall leg time\r\n\n");
_state = LEG_POSITION_RISE; // now start a timer on yo_timer for rise
finish_leg =1;
+ configFileIO().swimConstants.endleg_reason = 1; // normal timeout
lpr_oneshots=0;
sprintf(buf, "go to LEG POSITION DIVE on overall leg timeout, set finish_leg=1\n\n\r");
mbedLogger().appendDiagFile(buf,0);
@@ -721,25 +788,53 @@
// _fsm_timer.reset(); // reset time if still inside legg long timeout?
// _isTimeoutRunning = false;
} else if(_yotimer.read() > _yo_time ) {
- xbee().printf("LEG POS DIVE: yo_time timed out - go to LEG_POSITION_RISE actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
+ xbee().printf("LEG POS DIVE: yo_time timed out - go to LEG_POSITION_RISE actual depth: %3.1f (cmd: %3.1f)\r\n",
+ depthLoop().getPosition(), depthLoop().getCommand());
sprintf(buf, "LEG POS DIVE: yo_time timed out - go to LEG_POSITION_RISE\n\n\r");
- mbedLogger().appendDiagFile(buf,3);
+ mbedLogger().appendDiagFile(buf,3);
_state = LEG_POSITION_RISE;
finish_leg =1;
+ configFileIO().swimConstants.endleg_reason = 2; //yo_timeout
//_yotimer.reset();
//_yotimer.start(); // restart the yo timer for next yo in rise mode -- test if depth is small fraction of max_depth and exit to EC if small enough??
lpr_oneshots=0;
if (depthLoop().getPosition() < 0.7* depthLoop().getCommand()) { //too slow - exit
_state = EMERGENCY_CLIMB; // overridden here for testing
finish_leg = 1;
+ configFileIO().swimConstants.endleg_reason = 6; // 6 is too slow descent
xbee().printf("LEG POS DIVE:descent too slow (<0.7*maxdepth) - go to EMERGENCY_CLIMB actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
sprintf(buf, "LEG POS DIVE: descent too slow yo_time timed out - go to emergency climb but not really for testing\n\n\r");
mbedLogger().appendDiagFile(buf,3);
_state = LEG_POSITION_RISE;
// _fsm_timer.reset();
// _isTimeoutRunning = false;
+ } // add else if(depth > altimeter_blank && altimeter_depth < altimeter_abort && not_inverted) then exit with message
+ }
+ else if( (depthLoop().getPosition() > configFileIO().swimConstants.altim_blank_m) // ending on altimieter abort - bad, endleg, etc
+ && (fabs(imu().getRoll()) < 45) && (altimLoop().getPosition() < configFileIO().swimConstants.altim_abort_m)) {
+ //just go to leg_position_rise unless it is the second time
+ if(configFileIO().swimConstants.altim_abort_count >= 2) { // more than one altimeter abort
+ _state = EMERGENCY_CLIMB;
+ finish_leg =1;
+ sprintf(buf, "LEG POS DIVE: ALTIMETER ABORT - more than one!: depth=%f altimeter=%f - go to emergency climb\n\n\r",
+ depthLoop().getPosition(), altimLoop().getPosition() );
+ mbedLogger().appendDiagFile(buf,3);
+ configFileIO().swimConstants.endleg_reason = 3; // altimeter abort
+ _fsm_timer.reset();
+ _isTimeoutRunning = false;
+
+ }
+ // just go to leg_position_rise unless it is the second time
+ if(configFileIO().swimConstants.altim_abort_count < 2) {
+ _state = LEG_POSITION_RISE;
+ _fsm_timer.reset();
+ printf(buf, "LEG POS DIVE: ALTIMETER ABORT first one: ONLY go to leg_postiion_rise -depth=%f altimeter=%f - go to emergency climb\n\n\r",
+ depthLoop().getPosition(), altimLoop().getPosition() );
+ mbedLogger().appendDiagFile(buf,3);
+ _isTimeoutRunning = false;
+ configFileIO().swimConstants.altim_abort_count += 1;
+ }
}
- }
// what is active?
xbee().printf("LEG POS DIVE: 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, heading_imu:%6.1f deg>>[%0.2f sec] \r",
@@ -831,8 +926,14 @@
stateMachine().getLegParams();
// what are the commands? (using inner loops except for heading outer loop)
- batt().setPosition_mm(_neutral_batt_pos_mm - _BMM_dive_offset); //reversing the BCE and BATT positions
- bce().setPosition_mm(_neutral_bce_pos_mm + _BCE_dive_offset); //reversing the BCE and BATT positions
+ bce_rise_mm = configFileIO().swimConstants.bce_rise_mm;
+ bmm_rise_mm = configFileIO().swimConstants.bmm_rise_mm;
+
+ batt().setPosition_mm(_neutral_batt_pos_mm - bmm_rise_mm); // lighter nose
+ bce().setPosition_mm(_neutral_bce_pos_mm + bce_rise_mm); // more buoyant
+
+ // old way batt().setPosition_mm(_neutral_batt_pos_mm - _BMM_dive_offset); //reversing the BCE and BATT positions
+ // old way bce().setPosition_mm(_neutral_bce_pos_mm + _BCE_dive_offset); //reversing the BCE and BATT positions
//retrieve commands from structs (loaded from missionleg.cfg file)
headingLoop().setCommand(leg_heading); //ACTIVE HEADING (mimic of dive and rise code)
@@ -857,8 +958,15 @@
batt().unpause();
// what are the commands? (using inner loops except for heading outer loop)
- batt().setPosition_mm(_neutral_batt_pos_mm - _BMM_dive_offset); //reversing the BCE and BATT positions
- bce().setPosition_mm(_neutral_bce_pos_mm + _BCE_dive_offset); //reversing the BCE and BATT positions
+ // what are the commands? (using inner loops except for heading outer loop)
+ bce_rise_mm = configFileIO().swimConstants.bce_rise_mm;
+ bmm_rise_mm = configFileIO().swimConstants.bmm_rise_mm;
+
+ batt().setPosition_mm(_neutral_batt_pos_mm - bmm_rise_mm); // lighter nose
+ bce().setPosition_mm(_neutral_bce_pos_mm + bce_rise_mm); // more buoyant
+
+ // old way batt().setPosition_mm(_neutral_batt_pos_mm - _BMM_dive_offset); //reversing the BCE and BATT positions
+ // old way bce().setPosition_mm(_neutral_bce_pos_mm + _BCE_dive_offset); //reversing the BCE and BATT positions
//retrieve commands from structs (loaded from missionleg.cfg file)
headingLoop().setCommand(leg_heading); //ACTIVE HEADING (mimic of dive and rise code)
@@ -877,6 +985,7 @@
// _fsm_timer.reset();
sprintf(buf, "LEG POSITION DIVE Ended via overall timeout\n\n\r");
mbedLogger().appendDiagFile(buf,0);
+ configFileIO().swimConstants.endleg_reason = 1; // normal timeout
// _isTimeoutRunning = false;
}
else if (depthLoop().getPosition() < leg_min_depth - 0.5 ) { // add in check for finish_leg. Don't flip flop between states in that case
@@ -901,7 +1010,8 @@
finish_leg = 1;
_fsm_timer.reset(); _yotimer.reset();
_isTimeoutRunning = false;
- sprintf(buf, "LEG POSITION DIVE ... ENDS on yo_time too long, exit to emergnecy climb + finish_leg=1\n\n\r");
+ configFileIO().swimConstants.endleg_reason = 2; // yo_timeout
+ sprintf(buf, "LEG POSITION DIVE ... ENDS on yo_time too long, exit to emergency climb + finish_leg=1\n\n\r");
mbedLogger().appendDiagFile(buf,0);
}
@@ -992,6 +1102,7 @@
_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);
+ if(_neutral_success == -1) {configFileIO().swimConstants.endleg_reason = 4; }// find_neutral failed
} // allows wait at surface for xbee messaging to not close program
}
@@ -1055,8 +1166,17 @@
_state = MULTI_RISE;
_fsm_timer.reset();
_isTimeoutRunning = false;
- }
-
+ }// add else if(depth > altimeter_blank && altimeter_depth < altimeter_abort && not_inverted) then exit with message
+ else if( (depthLoop().getPosition() > configFileIO().swimConstants.altim_blank_m) // ending on altimieter abort - bad, endleg, etc
+ && (fabs(imu().getRoll()) < 45) && (altimLoop().getPosition() < configFileIO().swimConstants.altim_abort_m)) {
+ //just go to leg_position_rise unless it is the second time
+ _state = MULTI_RISE;
+ _fsm_timer.reset();
+ _isTimeoutRunning = false;
+
+ }
+
+
// WHAT IS ACTIVE?
xbee().printf("MD: 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());
@@ -1135,7 +1255,9 @@
}
// WHAT IS ACTIVE?
- xbee().printf("MR: 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());
+ xbee().printf("MR: 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());
@@ -1311,7 +1433,8 @@
if (!_isSubStateTimerRunning) {
_neutral_timer = _fsm_timer.read() + 5; //record the time when this block is first entered and add 5 seconds
- xbee().printf("\r\n\nNEUTRAL_SINKING: Next retraction at %0.1f sec [current time: %0.1f] (pitch: %0.1f) (BCE getSetPosition: %0.1f)\r\n", _neutral_timer, _fsm_timer.read(), pitchLoop().getPosition(), bce().getSetPosition_mm());
+ xbee().printf("\r\n\nNEUTRAL_SINKING: Next retraction at %0.1f sec [current time: %0.1f] (pitch: %0.1f) (BCE getSetPosition: %0.1f)\r\n", _neutral_timer, _fsm_timer.read(),
+ pitchLoop().getPosition(), bce().getSetPosition_mm());
// what are the commands? (BCE linear actuator active, no BMM or pitch movement)
bce().setPosition_mm(bce().getSetPosition_mm() - 2.5);