Dependencies: mbed MODSERIAL FATFileSystem
Diff: StateMachine/StateMachine.cpp
- Revision:
- 100:a21bb2e4493d
- Parent:
- 99:9d0849f5fcd7
--- a/StateMachine/StateMachine.cpp Fri Jun 28 13:59:11 2019 +0000
+++ b/StateMachine/StateMachine.cpp Fri Jun 28 15:04:49 2019 +0000
@@ -80,7 +80,7 @@
static int finish_leg=0; // allow a rise to complete ( if it takes less than a yo_time, then exit to FB)
static float leg_max_depth = 0;
static float leg_min_depth =0;
- static float _leg_heading = 90; //go east!
+ static float leg_heading = 90; //go east!
char buf[256];
switch (_state) {
case ENDLEG_WAIT:
@@ -622,53 +622,53 @@
break;
- case LEG_POSITION_DIVE :
- // start local state timer and init any other one-shot actions
- if (!_isTimeoutRunning) {
- xbee().printf("\r\n\nstate: LEG POSITION DIVE first time - start timer\r\n");
- sprintf(buf, "LEG POSITION DIVE start first dive start timer\n\n\r");
- mbedLogger().appendDiagFile(buf,0);
- _fsm_timer.reset(); // timer goes back to zero I am not sure about this reset jcw
- _fsm_timer.start(); // background timer starts running
- _isTimeoutRunning = true;
- _yotimer.reset();
- _yotimer.start(); //sets the yo_timer running on the dive
-
- // what needs to be started?
- bce().unpause();
- batt().unpause();
- rudder().unpause();
-
- //retrieve commands from structs (loaded from legfile.txt file) // only needs to be called once
- stateMachine().getLegParams();
-
- //leg_max_depth = currentLegStateStruct.max_depth;
- //_leg_heading = currentLegStateStruct.heading;
-
-
-
- // 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 + _leg_BMM_dive_offset); //changed to _leg_BMM_dive_offset
- bce().setPosition_mm(_neutral_bce_pos_mm - _leg_BCE_dive_offset); //changed to _leg_BCE_dive_offset
-
- //DEPTH COMMAND
- //depthLoop().setCommand(_depth_command); // I want this to be the max_depth in the legStruct
- depthLoop().setCommand(_leg_max_depth); // I want this to be the max_depth in the legStruct
-
- //headingLoop().setCommand(_heading_command); //ACTIVE HEADING (mimic of dive and rise code)
- headingLoop().setCommand(_leg_heading); //ACTIVE HEADING (mimic of dive and rise code)
-
- xbee().printf("LEG POS DIVE: BATT cmd: %3.1f\r\n",batt().getSetPosition_mm()); //get the actual commanded position
- xbee().printf("LEG POS DIVE: BCE cmd: %3.1f\r\n",bce().getSetPosition_mm()); //get the actual commanded position
- xbee().printf("LEG POS DIVE: heading cmd: %3.1f\r\n",headingLoop().getCommand());
-
- //reset max dive depth
- _max_recorded_depth_dive = -99; //float to record max depth
- lpd_oneshots = 1;
- }
-
- if (!lpd_oneshots) {
+ case LEG_POSITION_DIVE :
+ // start local state timer and init any other one-shot actions
+ if (!_isTimeoutRunning) {
+ xbee().printf("\r\n\nstate: LEG POSITION DIVE first time - start timer\r\n");
+ sprintf(buf, "LEG POSITION DIVE start first dive start timer\n\n\r");
+ mbedLogger().appendDiagFile(buf,0);
+ _fsm_timer.reset(); // timer goes back to zero I am not sure about this reset jcw
+ _fsm_timer.start(); // background timer starts running
+ _isTimeoutRunning = true;
+ _yotimer.reset();
+ _yotimer.start(); //sets the yo_timer running on the dive
+
+ // what needs to be started?
+ bce().unpause();
+ batt().unpause();
+ rudder().unpause();
+
+ // 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);
+
+
+ //retrieve commands from structs (loaded from legfile.txt file)
+ stateMachine().getLegParams();
+
+ leg_max_depth = currentLegStateStruct.max_depth;
+ leg_heading = currentLegStateStruct.heading;
+
+
+ //DEPTH COMMAND
+ //depthLoop().setCommand(_depth_command); // I want this to be the max_depth in the legStruct
+ depthLoop().setCommand(leg_max_depth); // I want this to be the max_depth in the legStruct
+
+ //headingLoop().setCommand(_heading_command); //ACTIVE HEADING (mimic of dive and rise code)
+ headingLoop().setCommand(leg_heading); //ACTIVE HEADING (mimic of dive and rise code)
+
+ xbee().printf("LEG POS DIVE: BATT cmd: %3.1f\r\n",batt().getSetPosition_mm()); //get the actual commanded position
+ xbee().printf("LEG POS DIVE: BCE cmd: %3.1f\r\n",bce().getSetPosition_mm()); //get the actual commanded position
+ xbee().printf("LEG POS DIVE: heading cmd: %3.1f\r\n",headingLoop().getCommand());
+
+ //reset max dive depth
+ _max_recorded_depth_dive = -99; //float to record max depth
+ lpd_oneshots = 1;
+ }
+
+ if (!lpd_oneshots) {
xbee().printf("\r\n\nstate: LEG POSITION DIVE oneshots\r\n");
// _fsm_timer.reset(); // timer goes back to zero I am not sure about this reset jcw
// sm_timer.start(); // background timer starts running
@@ -683,20 +683,20 @@
// 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 + _leg_BMM_dive_offset);
- bce().setPosition_mm(_neutral_bce_pos_mm - _leg_BCE_dive_offset);
+ 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 missionleg.cfg file)
- //leg_max_depth = currentLegStateStruct.max_depth;
- //_leg_heading = currentLegStateStruct.heading;
+ leg_max_depth = currentLegStateStruct.max_depth;
+ leg_heading = currentLegStateStruct.heading;
sprintf(buf, "LEG POSITION DIVE entered via Leg_pos_rise - lpd_oneshots, only restart _yotimer\n\n\r");
- mbedLogger().appendDiagFile(buf,0);
+ mbedLogger().appendDiagFile(buf,0);
//DEPTH COMMAND
//depthLoop().setCommand(_depth_command); // I want this to be the max_depth in the legStruct
depthLoop().setCommand(leg_max_depth); // I want this to be the max_depth in the legStruct
//headingLoop().setCommand(_heading_command); //ACTIVE HEADING (mimic of dive and rise code)
- headingLoop().setCommand(_leg_heading); //ACTIVE HEADING (mimic of dive and rise code)
+ headingLoop().setCommand(leg_heading); //ACTIVE HEADING (mimic of dive and rise code)
xbee().printf("LEG POS DIVE: BATT cmd: %3.1f\r\n",batt().getSetPosition_mm()); //get the actual commanded position
xbee().printf("LEG POS DIVE: BCE cmd: %3.1f\r\n",bce().getSetPosition_mm()); //get the actual commanded position
@@ -705,7 +705,7 @@
lpd_oneshots = 1;
//reset max dive depth
//_max_recorded_depth_dive = -99; //float to record max depth
- } // lpd_oneshots if timer is already running
+ } // lpd_oneshots if timer is already running
// how exit?
// timer runs out goes to LEG_POSITION_RISE but finish_leg flag is turned on.
@@ -715,7 +715,7 @@
finish_leg =1;
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);
+ mbedLogger().appendDiagFile(buf,0);
// _fsm_timer.reset();
// _isTimeoutRunning = false;
}
@@ -725,13 +725,13 @@
xbee().printf("LEG POS DIVE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
_state = LEG_POSITION_RISE;
lpr_oneshots = 0;
-
+
// _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;
//_yotimer.reset();
@@ -743,19 +743,19 @@
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;
+ _state = LEG_POSITION_RISE;
+ // _fsm_timer.reset();
+ // _isTimeoutRunning = false;
}
}
// 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] \n\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());
if (depthLoop().getPosition() > _max_recorded_depth_dive) {
- _max_recorded_depth_dive = depthLoop().getPosition(); //new max depth recorded when it is larger than previous values
+ _max_recorded_depth_dive = depthLoop().getPosition(); //new max depth recorded when it is larger than previous values
}
// ACTIVE RUDDER CONTROL
@@ -811,117 +811,116 @@
break; //end of POS RISE
case LEG_POSITION_RISE :
- // start local state timer and init any other one-shot actions
- //leg_min_depth = currentLegStateStruct.min_depth; // edit by CAM || don't need this every leg
- //_leg_heading = currentLegStateStruct.heading; // edit by CAM
-
- if (!_isTimeoutRunning) { // this should never occur. the only entry into this case is from LEG_POSITION_DIVE exit
- 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;
- _yotimer.reset(); //reset yo_timer;
- _yotimer.start(); //and start it
-
- // what needs to be started?
- bce().unpause();
- batt().unpause();
- stateMachine().getLegParams();
-
- // what are the commands? (using inner loops except for heading outer loop)
- batt().setPosition_mm(_neutral_batt_pos_mm - _leg_BMM_rise_offset); //reversing the BCE and BATT positions
- bce().setPosition_mm(_neutral_bce_pos_mm + _leg_BCE_rise_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)
-
- xbee().printf("LEG POS RISE: BATT cmd: %3.1f\r\n",batt().getSetPosition_mm()); //get the actual commanded position
- xbee().printf("LEG POS RISE: BCE cmd: %3.1f\r\n",bce().getSetPosition_mm()); //get the actual commanded position
- xbee().printf("LEG POS RISE: heading cmd: %3.1f\r\n",headingLoop().getCommand());
- lpr_oneshots = 1;
+ // start local state timer and init any other one-shot actions
+ leg_min_depth = currentLegStateStruct.min_depth;
+ leg_heading = currentLegStateStruct.heading;
+
+ if (!_isTimeoutRunning) { // this should never occur. the only entry into this case is from LEG_POSITION_DIVE exit
+ 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;
+ _yotimer.reset(); //reset yo_timer;
+ _yotimer.start(); //and start it
+
+ // what needs to be started?
+ bce().unpause();
+ batt().unpause();
+ 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
+ //retrieve commands from structs (loaded from missionleg.cfg file)
+
+ headingLoop().setCommand(leg_heading); //ACTIVE HEADING (mimic of dive and rise code)
+
+ xbee().printf("LEG POS RISE: BATT cmd: %3.1f\r\n",batt().getSetPosition_mm()); //get the actual commanded position
+ xbee().printf("LEG POS RISE: BCE cmd: %3.1f\r\n",bce().getSetPosition_mm()); //get the actual commanded position
+ xbee().printf("LEG POS RISE: heading cmd: %3.1f\r\n",headingLoop().getCommand());
+ lpr_oneshots = 1;
+ }
+ if (!lpr_oneshots) {
+ xbee().printf("\r\n\nstate: LEG POSITION RISE entered via LEG_POSiTION_DIVE finish\r\n");
+ sprintf(buf, "state: LEG POSITION RISE entered via LEG_POSITION_DIVE\n\n\r");
+ mbedLogger().appendDiagFile(buf,3);
+ //_fsm_timer.reset(); // timer goes back to zero --no
+ _yotimer.reset(); //reset yo_timer;
+ _yotimer.start(); //and start it
+ //_fsm_timer.start(); timer is already running // background timer starts running
+ // _isTimeoutRunning = true;
+
+ // what needs to be started?
+ bce().unpause();
+ 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
+ //retrieve commands from structs (loaded from missionleg.cfg file)
+
+ headingLoop().setCommand(leg_heading); //ACTIVE HEADING (mimic of dive and rise code)
+
+ xbee().printf("LEG POS RISE: BATT cmd: %3.1f\r\n",batt().getSetPosition_mm()); //get the actual commanded position
+ xbee().printf("LEG POS RISE: BCE cmd: %3.1f\r\n",bce().getSetPosition_mm()); //get the actual commanded position
+ xbee().printf("LEG POS RISE: heading cmd: %3.1f\r\n",headingLoop().getCommand());
+ lpr_oneshots = 1;
+ } // end if(!lpr_oneshots)
+
+ // how exit?
+ if (_fsm_timer.read() > _timeout) {
+ xbee().printf("LEG POS RISE: timed out on overall timeout\r\n");
+ // _state = EMERGENCY_CLIMB;
+ finish_leg = 1; // not going immediately to Emergency_climb and not resetting timers means it will take 1 yo-time of rising first
+ // _fsm_timer.reset();
+ sprintf(buf, "LEG POSITION DIVE Ended via overall timeout\n\n\r");
+ mbedLogger().appendDiagFile(buf,0);
+ // _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
+ xbee().printf("LEG POS RISE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
+ _state = LEG_POSITION_DIVE;
+ sprintf(buf, "LEG POSITION RISE flip-flops to LEG_POSITION_DIVE on near surface pressure \n\n\r");
+ mbedLogger().appendDiagFile(buf,0);
+ //_fsm_timer.reset();
+ //_isTimeoutRunning = false;
+ lpd_oneshots=0;
+ if(finish_leg ==1) {
+ _state = EMERGENCY_CLIMB;
+ _fsm_timer.reset(); _yotimer.reset();
+ _isTimeoutRunning = false;
+ sprintf(buf, "LEG POSITION RISE ... ENDS, and since finish_leg=1, go to Emergency climb\n\n\r");
+ mbedLogger().appendDiagFile(buf,0);
+ }
+ }
+ else if(_yotimer.read() > _yo_time ) {
+ xbee().printf("LEG POS RISE: yo_time timed out - go to Emergency_CLIMB. Actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
+ _state = EMERGENCY_CLIMB; // add diagnostics message
+ 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");
+ mbedLogger().appendDiagFile(buf,0);
}
- if (!lpr_oneshots) {
- xbee().printf("\r\n\nstate: LEG POSITION RISE entered via LEG_POSiTION_DIVE finish\r\n");
- sprintf(buf, "state: LEG POSITION RISE entered via LEG_POSITION_DIVE\n\n\r");
- mbedLogger().appendDiagFile(buf,3);
- //_fsm_timer.reset(); // timer goes back to zero --no
- _yotimer.reset(); //reset yo_timer;
- _yotimer.start(); //and start it
- //_fsm_timer.start(); timer is already running // background timer starts running
- // _isTimeoutRunning = true;
-
- // what needs to be started?
- bce().unpause();
- batt().unpause();
-
- // what are the commands? (using inner loops except for heading outer loop)
- batt().setPosition_mm(_neutral_batt_pos_mm - _leg_BMM_rise_offset); //reversing the BCE and BATT positions
- bce().setPosition_mm(_neutral_bce_pos_mm + _leg_BCE_rise_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)
-
- xbee().printf("LEG POS RISE: BATT cmd: %3.1f\r\n",batt().getSetPosition_mm()); //get the actual commanded position
- xbee().printf("LEG POS RISE: BCE cmd: %3.1f\r\n",bce().getSetPosition_mm()); //get the actual commanded position
- xbee().printf("LEG POS RISE: heading cmd: %3.1f\r\n",headingLoop().getCommand());
- lpr_oneshots = 1;
- } // end if(!lpr_oneshots)
-
- // how exit?
- if (_fsm_timer.read() > _timeout) {
- xbee().printf("LEG POS RISE: timed out on overall timeout\r\n");
- // _state = EMERGENCY_CLIMB;
- finish_leg = 1; // not going immediately to Emergency_climb and not resetting timers means it will take 1 yo-time of rising first
- // _fsm_timer.reset();
- sprintf(buf, "LEG POSITION DIVE Ended via overall timeout\n\n\r");
- mbedLogger().appendDiagFile(buf,0);
- // _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
- xbee().printf("LEG POS RISE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
- _state = LEG_POSITION_DIVE;
- sprintf(buf, "LEG POSITION RISE flip-flops to LEG_POSITION_DIVE on near surface pressure \n\n\r");
- mbedLogger().appendDiagFile(buf,0);
- //_fsm_timer.reset();
- //_isTimeoutRunning = false;
- lpd_oneshots=0;
- if(finish_leg ==1) {
- _state = EMERGENCY_CLIMB;
- _fsm_timer.reset();
- _yotimer.reset();
- _isTimeoutRunning = false;
- sprintf(buf, "LEG POSITION RISE ... ENDS, and since finish_leg=1, go to Emergency climb\n\n\r");
- mbedLogger().appendDiagFile(buf,0);
- }
- } else if(_yotimer.read() > _yo_time ) {
- xbee().printf("LEG POS RISE: yo_time timed out - go to Emergency_CLIMB. Actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
- _state = EMERGENCY_CLIMB; // add diagnostics message
- 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");
- mbedLogger().appendDiagFile(buf,0);
- }
-
- // 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 m [cmd:%6.1f]), pitch:%6.1f deg, heading_imu:%6.1f deg>>[%0.2f sec] \n\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());
-
- // ACTIVE RUDDER CONTROL
- rudder().setPosition_deg(headingLoop().getOutput());
-
- break; // end LEG POS RISE
+
+ // 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 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());
+
+ // ACTIVE RUDDER CONTROL
+ rudder().setPosition_deg(headingLoop().getOutput());
+
+ break; // end LEG POS RISE
// NEW DIVE AND RISE SEQUENCES
case FLOAT_LEVEL :
@@ -2601,8 +2600,7 @@
_timeout = currentStateStruct.timeout; //set timeout before exiting this function
}
//process one state at a time
-void StateMachine::getLegParams()
-{
+void StateMachine::getLegParams() {
//iterate through this sequence using the FSM
currentLegStateStruct.state = legController().legStructLoaded[_multi_leg_counter].state;
currentLegStateStruct.timeout = legController().legStructLoaded[_multi_leg_counter].timeout;
@@ -2610,27 +2608,10 @@
currentLegStateStruct.min_depth = legController().legStructLoaded[_multi_leg_counter].min_depth;
currentLegStateStruct.max_depth = legController().legStructLoaded[_multi_leg_counter].max_depth;
currentLegStateStruct.heading = legController().legStructLoaded[_multi_leg_counter].heading;
- currentLegStateStruct.BCE_dive_offset = legController().legStructLoaded[_multi_leg_counter].BCE_dive_offset;
- currentLegStateStruct.BMM_dive_offset = legController().legStructLoaded[_multi_leg_counter].BMM_dive_offset;
- currentLegStateStruct.BCE_rise_offset = legController().legStructLoaded[_multi_leg_counter].BCE_rise_offset;
- currentLegStateStruct.BMM_rise_offset = legController().legStructLoaded[_multi_leg_counter].BMM_rise_offset;
-
_timeout = currentLegStateStruct.timeout; //set timeout before exiting this function
_yo_time = currentLegStateStruct.yo_time;
_state = currentLegStateStruct.state;
- _heading = currentLegStateStruct.heading;
- _leg_max_depth = currentLegStateStruct.max_depth;
- _leg_min_depth = currentLegStateStruct.min_depth;
- _leg_BCE_dive_offset = currentLegStateStruct.BCE_dive_offset;
- _leg_BMM_dive_offset = currentLegStateStruct.BMM_dive_offset;
- _leg_BCE_rise_offset = currentLegStateStruct.BCE_rise_offset;
- _leg_BMM_rise_offset = currentLegStateStruct.BMM_rise_offset;
-
- _state = currentLegStateStruct.state;
-
- xbee().printf("BCE_DIVE = %f || BMM_DIVE = %f || BCE_RISE = %f || BMM_RISE = %f\n\r", _leg_BCE_dive_offset, _leg_BMM_dive_offset, _leg_BCE_rise_offset, _leg_BMM_rise_offset);
-
// _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::setstate_frommain(int set_state_val, int new_timeout) // used for setting state from main, when find_neutral has not been run