Diff: StateMachine/StateMachine.cpp

Revision:

82:0981b9ada820

Parent:

80:4e5d306d695b

Child:

84:eccd8e837134

--- a/StateMachine/StateMachine.cpp	Thu Nov 08 22:30:32 2018 +0000
+++ b/StateMachine/StateMachine.cpp	Fri Feb 15 16:00:17 2019 +0000
@@ -3,6 +3,7 @@
  
 StateMachine::StateMachine() {
     _timeout = 20;            // generic timeout for every state, seconds
+    _yo_time = 1.0e7;         // timeout for a dive or rise yo, not set for other ops
     
     _pitchTolerance = 5.0;     // pitch angle tolerance for FLOAT_LEVEL state
  
@@ -25,7 +26,7 @@
     _isSubStateTimerRunning = false;            // default timer to not running
 /////////////////////////////    
     _multi_dive_counter = 0;
-    
+    _multi_leg_counter = 0;
     _depth_KP = depthLoop().getControllerP();  // load current depth value
     _depth_KI = depthLoop().getControllerI();  // load current depth value
     _depth_KD = depthLoop().getControllerD();  // load current depth value
@@ -62,10 +63,18 @@
 }
  
 //Finite State Machine (FSM)
-int StateMachine::runStateMachine() {
+int StateMachine::runStateMachine() {   // ends about line 956
     // finite state machine ... each state has at least one exit criteria
+    static int lpd_oneshots=0;
+    static int lpr_oneshots=0;
+    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!
+    char buf[256];
     switch (_state) {
     case SIT_IDLE :
+    case FB_EXIT :  
     case KEYBOARD :
         // there actually is no timeout for SIT_IDLE, but this enables some one-shot actions
         if (!_isTimeoutRunning) {
@@ -147,10 +156,10 @@
         }
         
         // how exit?
-        if (_fsm_timer > _timeout) {
+        if ((_fsm_timer > _timeout ) || (_yotimer > _yo_time)) {
             xbee().printf("EC: timed out\r\n");
             _state = FLOAT_BROADCAST;
-            _fsm_timer.reset();
+            _fsm_timer.reset(); _yotimer.reset();
             _isTimeoutRunning = false;
         }
         else if (depthLoop().getPosition() < 2.0) { //if the depth is greater than 0.2 feet, go to float broadcast
@@ -385,6 +394,139 @@
         rudder().setPosition_deg(headingLoop().getOutput());
         
         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();
+ 
+            // 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
+                _yotimer.reset();   //sets the yo_timer running  on the NEW dive
+                _yotimer.start();
+                //_isTimeoutRunning = true;
+
+                // 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 missionleg.cfg file)
+                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());
+
+                lpd_oneshots = 1;
+                //reset max dive depth
+                //_max_recorded_depth_dive = -99;            //float to record max depth
+          }  // lpd_oneshots  if timer is already running
+
+            // how exit?
+            // timer runs out goes to LEG_POSITION_RISE but finish_leg flag is turned on.
+            if (_fsm_timer.read() > _timeout) {
+                xbee().printf("LEG POSITION DIVE timed out for overall leg time\r\n\n");
+                _state = LEG_POSITION_RISE; //  now start a timer on yo_timer for rise
+                finish_leg =1;
+                lpr_oneshots=0;
+                // _fsm_timer.reset();
+                // _isTimeoutRunning = false;
+            }
+
+            // when you reach the dive threshold, surface
+            else if (depthLoop().getPosition() > depthLoop().getCommand() - 0.5) { // including offset for low momentum approaches
+                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());
+                sprintf(buf, "LEG POS DIVE: yo_time timed out - go to LEG_POSITION_RISE\n\n\r");
+    mbedLogger().appendDiagFile(buf,3);
+                _state = LEG_POSITION_RISE;
+                //_yotimer.reset();
+                //_yotimer.start();    // restart the yo timer for next yo in rise mode    -- test if depth is small fraction of max_depth and exit to EC if small enough??
+                lpr_oneshots=0;
+                if (depthLoop().getPosition() < 0.7* depthLoop().getCommand()) {     //too slow - exit
+                    _state = EMERGENCY_CLIMB;
+                    finish_leg = 1;
+                    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\n\n\r");
+                    mbedLogger().appendDiagFile(buf,3);
+                _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 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());
+
+            if (depthLoop().getPosition() > _max_recorded_depth_dive) {
+            _max_recorded_depth_dive = depthLoop().getPosition();    //new max depth recorded when it is larger than previous values
+            }
+
+            // ACTIVE RUDDER CONTROL
+            rudder().setPosition_deg(headingLoop().getOutput());
+
+            break;  // end LEG POSITION DIVE
     
     case POSITION_RISE :
         // start local state timer and init any other one-shot actions
@@ -425,12 +567,106 @@
         }
  
         // 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", 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 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",
+            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;
+        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;
+           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);
+            //_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_POSTION_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;
+        }
+ 
+        // 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;
+            _fsm_timer.reset();
+            _isTimeoutRunning = false;
+        }
+        else if (depthLoop().getPosition() < leg_min_depth - 0.5) {
+            xbee().printf("LEG POS RISE: actual depth: %3.1f (cmd: %3.1f)\r\n", depthLoop().getPosition(), depthLoop().getCommand());
+            _state = LEG_POSITION_DIVE;
+            //_fsm_timer.reset();
+            //_isTimeoutRunning = false;
+            lpd_oneshots=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;
+            finish_leg = 1;
+            _fsm_timer.reset();
+            _isTimeoutRunning = false;
+            }
+ 
+        // 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", 
+        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 :
@@ -491,7 +727,8 @@
         }
         
         // how exit?
-        if (_fsm_timer > _timeout) {
+        
+        if (_fsm_timer > _timeout ) {
             xbee().printf("FB: timed out\r\n");
             _state = SIT_IDLE;
             _fsm_timer.reset();
@@ -500,6 +737,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
         }
         
         //fix on float_broadcast to account for BCE stopping early in current hardware
@@ -516,8 +754,11 @@
         }
         
         // 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", 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("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", 
+        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;}
         break;
         
     case MULTI_DIVE :
@@ -2077,6 +2318,20 @@
     
     _timeout = currentStateStruct.timeout;  //set timeout before exiting this function
 }
+//process one state at a time
+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;
+    currentLegStateStruct.yo_time = legController().legStructLoaded[_multi_leg_counter].yo_time;  //timer for each up/down segment
+    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;
+    
+    _timeout = currentLegStateStruct.timeout;  //set timeout before exiting this function
+    _yo_time = currentLegStateStruct.yo_time;
+    _state   = currentLegStateStruct.state;
+}
 
 void StateMachine::printCurrentSdLog() {
     xbee().printf("SD card log work in progress\r\n");