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:
- 85:dd8176285b6e
- Parent:
- 84:eccd8e837134
- Child:
- 87:6d95f853dab3
--- a/StateMachine/StateMachine.cpp Tue Feb 19 20:11:43 2019 +0000
+++ b/StateMachine/StateMachine.cpp Mon Feb 25 21:26:34 2019 +0000
@@ -3,8 +3,8 @@
StateMachine::StateMachine() {
_timeout = 20; // generic timeout for every state, seconds
- _yo_time = 400; // timeout for a dive or rise yo, not set for other ops
- _state_transition_time = 60; // time to allow motors to come to rest in float_broadcast
+ _yo_time = 40; // previously= 400 ; timeout for a dive or rise yo, not set for other ops
+ _state_transition_time = 20; // previously =60; time to allow motors to come to rest in float_broadcast
_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)
@@ -63,7 +63,7 @@
}
//Finite State Machine (FSM)
-int StateMachine::runStateMachine() { // ends about line 956
+int StateMachine::runStateMachine() { // ends about line 1022
// finite state machine ... each state has at least one exit criteria
static int lpd_oneshots=0;
static int lpr_oneshots=0;
@@ -73,11 +73,18 @@
static float leg_heading = 90; //go east!
char buf[256];
switch (_state) {
- case SIT_IDLE :
- case FB_EXIT :
+ case ENDLEG_WAIT:
+ if(!_isTimeoutRunning) {
+ _yotimer.reset(); _yotimer.start(); _fsm_timer.reset(); _fsm_timer.start();
+ _isTimeoutRunning = 1;
+ }
+ keyboard(); //keyboard function now needs to know about this state and its timeout
+ 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 :
- // there actually is no timeout for SIT_IDLE, but this enables some one-shot actions
- if (!_isTimeoutRunning) {
+ // there actually is no timeout test for SIT_IDLE, but this enables some one-shot actions
+ if (!_isTimeoutRunning) { // presumably the first time back in this block, whatever ended stopped the timeout_running.
//tare pressure sensor
depth().tare(); // tares to ambient (do on surface)
@@ -134,7 +141,9 @@
// the inner loop position controls are maintaining the positions of the linear actuators
//print status to screen continuously
- xbee().printf("CHECK_TUNING: BCE_position: %0.1f, BATT_position: %0.1f (BCE_cmd: %0.1f, BATT_cmd: %0.1f)(depth: %0.1f ft,pitch: %0.1f deg,heading: %0.1f) [%0.1f sec]\r",bce().getPosition_mm(),batt().getPosition_mm(),bce().getSetPosition_mm(),batt().getSetPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition(),imu().getHeading(),_fsm_timer.read());
+ xbee().printf("CHECK_TUNING: BCE_position: %0.1f, BATT_position: %0.1f (BCE_cmd: %0.1f, BATT_cmd: %0.1f)(depth: %0.1f m,pitch: %0.1f deg,heading: %0.1f) [%0.1f sec]\r",
+ bce().getPosition_mm(),batt().getPosition_mm(),bce().getSetPosition_mm(),batt().getSetPosition_mm(),depthLoop().getPosition(),
+ pitchLoop().getPosition(),imu().getHeading(),_fsm_timer.read());
break;
@@ -272,7 +281,9 @@
}
// 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 ft [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("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(),
+ 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());
@@ -327,7 +338,9 @@
}
// WHAT IS ACTIVE?
- xbee().printf("RISE: BcePos (cmd):%6.1f mm(%0.1f), BattPos:%6.1f mm(%0.1f), RUD_deg_cmd: %5.1f <<current depth:%6.1f ft [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("RISE: 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());
@@ -386,7 +399,9 @@
}
// 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 ft [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(),depthLoop().getPosition(),depthLoop().getCommand(),pitchLoop().getPosition(),imu().getHeading(),_fsm_timer.read());
+ 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(),
+ depthLoop().getPosition(),depthLoop().getCommand(),pitchLoop().getPosition(),imu().getHeading(),_fsm_timer.read());
if (depthLoop().getPosition() > _max_recorded_depth_dive) {
_max_recorded_depth_dive = depthLoop().getPosition(); //new max depth recorded when it is larger than previous values
@@ -418,6 +433,8 @@
// 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);
+
+
//retrieve commands from structs (loaded from legfile.txt file)
stateMachine().getLegParams();
@@ -523,7 +540,7 @@
}
// 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 ft [cmd:%6.1f]), pitch:%6.1f deg, heading_imu:%6.1f deg>>[%0.2f sec] \r",
+ 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",
bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_deg(),
depthLoop().getPosition(),depthLoop().getCommand(),pitchLoop().getPosition(),imu().getHeading(),_fsm_timer.read());
@@ -575,7 +592,7 @@
}
// what is active?
- xbee().printf("POS RISE: BcePos (cmd):%6.1f mm(%0.1f), BattPos:%6.1f mm(%0.1f), RUD_deg_cmd: %5.1f <<current depth:%6.1f ft [cmd:%6.1f]), pitch:%6.1f deg, heading_imu:%6.1f deg>>[%0.2f sec] \r",
+ xbee().printf("POS RISE: 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(),depthLoop().getPosition(),depthLoop().getCommand(),pitchLoop().getPosition(),imu().getHeading(),_fsm_timer.read());
@@ -593,6 +610,13 @@
xbee().printf("\r\n\nstate: LEG POSITION RISE\r\n");
sprintf(buf, "state: LEG POSITION RISE entered with timeout NOT running should not happen\n\n\r");
mbedLogger().appendDiagFile(buf,3);
+ // go and read the legparams just in case
+
+ //retrieve commands from structs (loaded from legfile.txt file)
+ stateMachine().getLegParams();
+ leg_min_depth = currentLegStateStruct.min_depth;
+ leg_heading = currentLegStateStruct.heading;
+
//_fsm_timer.reset(); // timer goes back to zero --no
_fsm_timer.start(); // background timer starts running
_isTimeoutRunning = true;
@@ -680,7 +704,7 @@
}
// what is active?
- xbee().printf("LEG POS RISE: BcePos (cmd):%6.1f mm(%0.1f), BattPos:%6.1f mm(%0.1f), RUD_deg_cmd: %5.1f <<current depth:%6.1f ft [cmd:%6.1f]), pitch:%6.1f deg, heading_imu:%6.1f deg>>[%0.2f sec] \r",
+ xbee().printf("LEG POS RISE: 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(),depthLoop().getPosition(),depthLoop().getCommand(),pitchLoop().getPosition(),imu().getHeading(),_fsm_timer.read());
@@ -760,7 +784,7 @@
//mbedLogger().closeFile();
_isTimeoutRunning = false;
- if(finish_leg == 1) { _state = FB_EXIT; } // allows exit if finish_leg set by leg_POS_dive, or LEG_POS_RISE timeout or too slow yo
+ if(finish_leg == 1) { _state = ENDLEG_WAIT; } // allows wait at surface for xbee messaging to not close program
}
//fix on float_broadcast to account for BCE stopping early in current hardware
@@ -769,19 +793,19 @@
xbee().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;
_fsm_timer.reset();
+ _isTimeoutRunning = false;
//stop recording data
//mbedLogger().closeFile();
- if(finish_leg == 1) { _state = FB_EXIT; } // allows exit if finish_leg set by leg_POS_dive, or LEG_POS_RISE timeout or too slow yo
- _isTimeoutRunning = false;
- }
+ if(finish_leg == 1) { _state = ENDLEG_WAIT; } // allows exit via wait at surface first
+ }
// what is active?
- xbee().printf("FB: BcePos (cmd):%6.1f mm(%0.1f), BattPos:%6.1f mm(%0.1f), RUD_deg_cmd: %5.1f <<current depth:%6.1f ft [cmd:%6.1f]), pitch:%6.1f deg, heading_imu:%6.1f deg>>[%0.2f sec] \r",
+ xbee().printf("FB: 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(),
depthLoop().getPosition(),depthLoop().getCommand(),pitchLoop().getPosition(),imu().getHeading(),_fsm_timer.read());
- if (finish_leg == 1) { _state = FB_EXIT;}
+ // if (finish_leg == 1) { _state = FB_EXIT;}
break;
case MULTI_DIVE :
@@ -806,7 +830,7 @@
headingLoop().setCommand(_heading_command); //ACTIVE HEADING (mimic of dive and rise code)
- xbee().printf("MULTI-DIVE: depth cmd: %3.1f ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
+ xbee().printf("MULTI-DIVE: depth cmd: %3.1f m, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
//no max depth recording right now
}
@@ -826,7 +850,7 @@
}
// 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 ft [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("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());
batt().setPosition_mm(pitchLoop().getOutput());
@@ -859,7 +883,7 @@
pitchLoop().setCommand(-sequence_pitch_command);
headingLoop().setCommand(_heading_command); //ACTIVE HEADING (mimic of dive and rise code)
- xbee().printf("MULTI-RISE: depth cmd: 0.0 ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
+ xbee().printf("MULTI-RISE: depth cmd: 0.0 m, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
}
// how exit?
@@ -903,7 +927,7 @@
}
// 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 ft [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());
@@ -1020,7 +1044,7 @@
xbee().printf("Q to TYPE in the BMM offset: %0.1f (BMM Dive POS: %0.1f, Rise POS: %0.1f) (positive input offset = pitch down)\r\n",_BMM_dive_offset, _neutral_batt_pos_mm + _BMM_dive_offset, _neutral_batt_pos_mm - _BMM_dive_offset);
xbee().printf("A to TYPE in the BCE offset: %0.1f (BCE Dive POS: %0.1f, Rise POS: %0.1f) (positive input offset = dive)\r\n",_BCE_dive_offset, _neutral_bce_pos_mm - _BCE_dive_offset, _neutral_bce_pos_mm + _BCE_dive_offset);
xbee().printf("W to TYPE in the heading command: %0.1f deg (imu heading: %0.1f)\r\n",_heading_command,imu().getHeading());
- xbee().printf("S to TYPE in depth setpoint: %0.1f (Current depth: %0.1f ft)\r\n",_depth_command, depthLoop().getPosition());
+ xbee().printf("S to TYPE in depth setpoint: %0.1f (Current depth: %0.1f m)\r\n",_depth_command, depthLoop().getPosition());
xbee().printf("T to TYPE in the timeout (default is 60 seconds): %d s\r\n",_timeout);
xbee().printf(" C See sensor readings (and max recorded depth of dive & neutral sequences)\r\n");
@@ -1050,7 +1074,7 @@
xbee().printf("; or : to TYPE in the BCE neutral position: %0.1f\r\n", _neutral_bce_pos_mm);
xbee().printf("[ or { to TYPE in the BMM neutral position: %0.1f\r\n", _neutral_batt_pos_mm);
xbee().printf("Q to TYPE in pitch setpoint: %0.1f (Current IMU pitch: %0.1f deg)\r\n",_pitch_command,imu().getPitch());
- xbee().printf("A to TYPE in depth setpoint: %0.1f (Current depth: %0.1f ft)\r\n",_depth_command, depthLoop().getPosition());
+ xbee().printf("A to TYPE in depth setpoint: %0.1f (Current depth: %0.1f m)\r\n",_depth_command, depthLoop().getPosition());
xbee().printf("W to TYPE in the heading command: %0.1f deg (imu heading: %0.1f)\r\n",_heading_command,imu().getHeading());
xbee().printf("T to enter in the timeout (default is 60 seconds): %d s\r\n",_timeout);
@@ -1111,7 +1135,7 @@
}
// what is active? (only the buoyancy engine moved every 5 seconds at start)
- xbee().printf("BCE current pos: %0.1f mm (BCE setpoint: %0.1f mm) (current depth: %0.1f ft)\r", bce().getPosition_mm(),bce().getSetPosition_mm(),depthLoop().getPosition()); //debug
+ xbee().printf("BCE current pos: %0.1f mm (BCE setpoint: %0.1f mm) (current depth: %0.1f m)\r", bce().getPosition_mm(),bce().getSetPosition_mm(),depthLoop().getPosition()); //debug
//the BCE moves every 5 seconds. No BMM or rudder movement.
@@ -1145,8 +1169,8 @@
}
//Troy: Depth rate will go negative as the pressure vessel starts rising
//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
- xbee().printf("\r\n\nNEUTRAL_SLOWLY_RISE: Sink Rate < 0 ft/s [time: %0.1f]\r\n", _fsm_timer.read());
+ else if (depthLoop().getVelocity() < 0) { //less than zero ft/s should now be m/s
+ 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
}
@@ -1282,16 +1306,35 @@
void StateMachine::keyboard() {
char user_input;
+ char buf[256];
// check keyboard and make settings changes as requested
// states can be changed only at the start of a sequence (when the system is in SIT_IDLE)
//TEST
int _keyboard_state = 0; //made this a local variable because it was retaining the last keyboard state
-
- if (xbee().readable() && (_state == SIT_IDLE || _state == KEYBOARD)) {
- // get the key
- user_input = xbee().getc();
+ static int yocount = -3;
+ static float htime = 0.0;
+ if(_state == ENDLEG_WAIT) {
+ yocount = int(_yotimer.read());
+ if ( ( (yocount % 5) == 0 ) && ( _yotimer.read() > 2.5+htime) ) { // send out a heartbeat message, need to cast as an int from float
+ xbee().printf("FSG on surface at end of leg, hit c to wake up\r\n");
+ sprintf(buf, "ENDLEG WAIT heartbeat message sent yotimer=%f \n\n\r", _yotimer.read());
+ mbedLogger().appendDiagFile(buf,0);
+ htime = _yotimer.read();
+ }
+ if( _yotimer > _state_transition_time) {
+ _keyboard_state = FB_EXIT;
+ _state = FB_EXIT;
+ }
+ }
+ if (xbee().readable() && (_state == SIT_IDLE || _state == KEYBOARD || _state == ENDLEG_WAIT)) { //ends at very end of function 1684
+ // then get the key
+ user_input = xbee().getc();
+ // and exit from ENDLEG_WAIT - you got a keyboard input
+ if(_state == ENDLEG_WAIT) {
+ _keyboard_state = SIT_IDLE; // this is installed in _state at end of function
+ }
//record that the keyboard was used
_state_array[_state_array_counter] = KEYBOARD;
@@ -1388,7 +1431,7 @@
xbee().printf("calc vessel pressure: %f (counts: %d) \r\n",sensors().getInternalPressurePSI(),adc().readCh5());
// End of ADC Test
- xbee().printf("depth: %3.1f ft\r\n",depthLoop().getPosition());
+ xbee().printf("depth: %3.1f m\r\n",depthLoop().getPosition());
xbee().printf("pitch: %3.1f deg\r\n",imu().getPitch());
xbee().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm());
xbee().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm());
@@ -1414,7 +1457,7 @@
/***************************** COMMON COMMANDS *****************************/
/***************************** DEBUG MENU *****************************/
- if (_debug_menu_on) {
+ if (_debug_menu_on) { // ends around +175 lines
if (user_input == 'D') {
_keyboard_state = DIVE;
}
@@ -1500,7 +1543,15 @@
else if (_state_array[i] == MULTI_RISE)
string_state = "MULTI_RISE";
else if (_state_array[i] == KEYBOARD)
- string_state = "KEYBOARD";
+ string_state = "KEYBOARD";
+ else if (_state_array[i] == LEG_POSITION_DIVE)
+ string_state = "LEG_POS_DIVE";
+ else if (_state_array[i] == LEG_POSITION_RISE)
+ string_state = "LEG_POS_RISE";
+ else if (_state_array[i] == FB_EXIT)
+ string_state = "FB_EXIT";
+ else if (_state_array[i] == ENDLEG_WAIT)
+ string_state = "ENDLEG_WAIT";
xbee().printf("State #%d: %d (%s)\r\n", i, _state_array[i], string_state.c_str());
}
@@ -1549,7 +1600,7 @@
_pitch_command = getFloatUserInput();
}
else if (user_input == 'A') {
- xbee().printf(">> Please enter the desired DEPTH (ft).\r\n");
+ xbee().printf(">> Please enter the desired DEPTH (m).\r\n");
_depth_command = getFloatUserInput();
}
@@ -1589,7 +1640,7 @@
wait(1);
_debug_menu_on = false;
}
- } //end of debug menu
+ } //end of debug menu matches line 1445 approx -180
/***************************** DEBUG MENU *****************************/
/***************************** SIMPLE MENU *****************************/
@@ -1628,7 +1679,7 @@
}
else if (user_input == 'S') {
- xbee().printf(">> Please enter the desired DEPTH (ft).\r\n");
+ xbee().printf(">> Please enter the desired DEPTH (m).\r\n");
_depth_command = getFloatUserInput();
}
//POSITION DIVE COMMANDS
@@ -1639,12 +1690,12 @@
wait(1);
}
}
-/***************************** SIMPLE MENU *****************************/
+/***************************** END SIMPLE MENU *****************************/
//when you read the keyboard successfully, change the state
_state = _keyboard_state; //set state at the end of this function
//xbee().printf("\r\n\n ********* KEYBOARD STATE: %d *********\r\n\n", _state);
- }
+ } //matches very top of function - first if()
}
void StateMachine::keyboard_menu_STREAM_STATUS() {
@@ -1663,8 +1714,13 @@
wait(1);
-// xbee().printf("BCE POS (CMD): %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS_psi: %0.2f [depth_ft: %0.2f], PITCH: %0.2f, HEADING: %0.2f, rudder_servo_pwm: %0.1f \n[FILT/RAW 0(%d,%d),1(%d),2(%d),6(%d),4(%d),5(%d),6(%d),7(%d)] Switch: BCE(%d) BMM(%d)\r",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),depth().getPsi(),depthLoop().getPosition(),imu().getPitch(),imu().getHeading(),rudder().getSetPosition_pwm(),adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),adc().readCh5(),adc().readCh6(),adc().readCh7(), bce().getHardwareSwitchStatus(),batt().getHardwareSwitchStatus());
- xbee().printf("BCE POS: %0.1f (cmd %0.1f) BATT POS: %0.1f (cmd %0.1f) PRESS_psi: %0.2f [depth_ft: %0.2f], PITCH: %0.2f, HEADING: %0.2f, rdr_pwm: %0.1f [FILT/RAW 0(%d,%d),1(%d,%d),2(%d,%d),3(%d,%d),4(%d,%d),5(%d,%d),6(%d,%d),7(%d,%d)] Switch: BCE(%d) BMM(%d)\r",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),depth().getPsi(),depthLoop().getPosition(),imu().getPitch(),imu().getHeading(),rudder().getSetPosition_pwm(),adc().readCh0(),adc().readRawCh0(),adc().readCh1(),adc().readRawCh1(),adc().readCh2(),adc().readRawCh2(),adc().readCh3(),adc().readRawCh3(),adc().readCh4(),adc().readRawCh4(),adc().readCh5(),adc().readRawCh5(),adc().readCh6(),adc().readRawCh6(),adc().readCh7(),adc().readRawCh7(),bce().getHardwareSwitchStatus(),batt().getHardwareSwitchStatus());
+// xbee().printf("BCE POS (CMD): %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS_psi: %0.2f [depth_m: %0.2f], PITCH: %0.2f, HEADING: %0.2f, rudder_servo_pwm: %0.1f \n[FILT/RAW 0(%d,%d),1(%d),2(%d),6(%d),4(%d),5(%d),6(%d),7(%d)] Switch: BCE(%d) BMM(%d)\r",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),depth().getPsi(),depthLoop().getPosition(),imu().getPitch(),imu().getHeading(),rudder().getSetPosition_pwm(),adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),adc().readCh5(),adc().readCh6(),adc().readCh7(), bce().getHardwareSwitchStatus(),batt().getHardwareSwitchStatus());
+ xbee().printf("BCE POS: %0.1f (cmd %0.1f) BATT POS: %0.1f (cmd %0.1f) PRESS_psi: %0.2f [depth_m: %0.2f], PITCH: %0.2f, HEADING: %0.2f, rdr_pwm: %0.1f [FILT/RAW 0(%d,%d),1(%d,%d),2(%d,%d),3(%d,%d),4(%d,%d),5(%d,%d),6(%d,%d),7(%d,%d)] Switch: BCE(%d) BMM(%d)\r",
+ bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(),depth().getPsi(),
+ depthLoop().getPosition(),imu().getPitch(),imu().getHeading(),rudder().getSetPosition_pwm(),adc().readCh0(),adc().readRawCh0(),
+ adc().readCh1(),adc().readRawCh1(),adc().readCh2(),adc().readRawCh2(),adc().readCh3(),adc().readRawCh3(),adc().readCh4(),
+ adc().readRawCh4(),adc().readCh5(),adc().readRawCh5(),adc().readCh6(),adc().readRawCh6(),adc().readCh7(),adc().readRawCh7(),
+ bce().getHardwareSwitchStatus(),batt().getHardwareSwitchStatus());
continue; // didn't get a user input, so keep waiting for it
}
@@ -1776,7 +1832,7 @@
// show the menu
xbee().printf("\r\n7: MANUAL TUNING MENU (EXIT WITH 'X' !) (Pause and Unpause rudder ticker with P and U\n");
xbee().printf("\r\n(Adjust BCE and BATT positions in real-time. Timeout NOT running! (decrease/increase BCE with A/S, BATT with Q/W, RUDDER with E/R)\r\n");
- xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
+ xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f m, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
// what needs to be started?
bce().unpause(); //this is now active
@@ -1812,34 +1868,34 @@
else if (TUNING_key == 'A' or TUNING_key == 'a') {
_tuning_bce_pos_mm = _tuning_bce_pos_mm - 1.0;
bce().setPosition_mm(_tuning_bce_pos_mm); //this variable is loaded from the file at initialization
- xbee().printf("\r\nMANUAL_TUNING: (BCE CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",bce().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
- xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
+ xbee().printf("\r\nMANUAL_TUNING: (BCE CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f m, pitch: %0.1f deg)\r",bce().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
+ xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f m, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
}
else if (TUNING_key == 'S' or TUNING_key == 's') {
_tuning_bce_pos_mm = _tuning_bce_pos_mm + 1.0;
bce().setPosition_mm(_tuning_bce_pos_mm); //this variable is loaded from the file at initialization
- xbee().printf("\r\nMANUAL_TUNING: (BCE CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",bce().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
- xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
+ xbee().printf("\r\nMANUAL_TUNING: (BCE CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f m, pitch: %0.1f deg)\r",bce().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
+ xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f m, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
}
//BATTERY
else if (TUNING_key == 'Q' or TUNING_key == 'q') {
_tuning_batt_pos_mm = _tuning_batt_pos_mm - 1.0;
batt().setPosition_mm(_tuning_batt_pos_mm); //this variable is loaded from the file at initialization
- xbee().printf("\r\nMANUAL_TUNING: (BATT CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",batt().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
- xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
+ xbee().printf("\r\nMANUAL_TUNING: (BATT CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f m, pitch: %0.1f deg)\r",batt().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
+ xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f m, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
}
else if (TUNING_key == 'W' or TUNING_key == 'w') {
_tuning_batt_pos_mm = _tuning_batt_pos_mm + 1.0;
batt().setPosition_mm(_tuning_batt_pos_mm); //this variable is loaded from the file at initialization
- xbee().printf("\r\nMANUAL_TUNING: (BATT CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",batt().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
- xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
+ xbee().printf("\r\nMANUAL_TUNING: (BATT CHANGE: %0.1f)\r\n BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f m, pitch: %0.1f deg)\r",batt().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
+ xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f m, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
}
else if (TUNING_key == 'c' or TUNING_key == 'C') {
- xbee().printf("\r\nMT: (CURRENT POSITIONS) BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
+ xbee().printf("\r\nMT: (CURRENT POSITIONS) BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f m, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
}
//RUDER
@@ -1847,28 +1903,28 @@
_tuning_rudder_pos_deg = _tuning_rudder_pos_deg - 0.5;
rudder().setPosition_deg(_tuning_rudder_pos_deg);
xbee().printf("MT: RUDDER CHANGE %0.1f deg [servo pwm: %f, %0.1f deg] (headingLoop heading: % 0.1f deg, IMU heading: %0.1f deg)\r\n", _tuning_rudder_pos_deg, rudder().getSetPosition_pwm(), rudder().getSetPosition_deg(), headingLoop().getPosition(), imu().getHeading());
- xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
+ xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f m, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
}
else if (TUNING_key == 'E' or TUNING_key == 'e') {
_tuning_rudder_pos_deg = _tuning_rudder_pos_deg + 0.5;
rudder().setPosition_deg(_tuning_rudder_pos_deg);
xbee().printf("MT: RUDDER CHANGE %0.1f deg [servo pwm: %f, %0.1f deg] (headingLoop heading: % 0.1f deg, IMU heading: %0.1f deg)\r\n", _tuning_rudder_pos_deg, rudder().getSetPosition_pwm(), rudder().getSetPosition_deg(), headingLoop().getPosition(), imu().getHeading());
- xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
+ xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f m, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
}
else if (TUNING_key == '-' or TUNING_key == '_') {
_tuning_rudder_pwm -= 10.0;
rudder().setPWM(_tuning_rudder_pwm);
xbee().printf("MT: (-) RUDDER CHANGE %0.1f pwm\n\r", rudder().getSetPosition_pwm());
- xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
+ xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f m, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
}
else if (TUNING_key == '=' or TUNING_key == '+') {
_tuning_rudder_pwm += 10.0;
rudder().setPWM(_tuning_rudder_pwm);
xbee().printf("MT: (+) RUDDER CHANGE %0.1f pwm\n\r", rudder().getSetPosition_pwm());
- xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f ft, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
+ xbee().printf("\r\nMT: BCE pos:%5.1f (cmd:%5.1f), BMM:%5.1f (cmd:%5.1f), RUDDER:%6.1f (%6.1f deg) (depth: %0.1f m, pitch: %0.1f deg, headingLoop heading: %0.1f deg, IMU heading: %0.1f deg) \n\n\r",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm(),rudder().getSetPosition_pwm(),rudder().getSetPosition_deg(),depthLoop().getPosition(),pitchLoop().getPosition(), headingLoop().getPosition(),imu().getHeading());
}
else {
@@ -2354,6 +2410,7 @@
_timeout = currentLegStateStruct.timeout; //set timeout before exiting this function
_yo_time = currentLegStateStruct.yo_time;
_state = currentLegStateStruct.state;
+ // _multi_leg_counter += 1; // this is wrong, because this subroutine is called multiple times on the same leg, I need indicator of 'next_leg', first.
}
void StateMachine::printCurrentSdLog() {