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() {