Diff: StateMachine/StateMachine.cpp

Revision:

84:eccd8e837134

Parent:

82:0981b9ada820

Child:

85:dd8176285b6e

--- a/StateMachine/StateMachine.cpp	Fri Feb 15 16:14:22 2019 +0000
+++ b/StateMachine/StateMachine.cpp	Tue Feb 19 20:11:43 2019 +0000
@@ -3,8 +3,8 @@
  
 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
-    
+    _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
     _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)
@@ -144,6 +144,8 @@
             xbee().printf("\r\n\nstate: EMERGENCY_CLIMB\r\n");
             _fsm_timer.reset(); // timer goes back to zero
             _fsm_timer.start(); // background timer starts running
+            _yotimer.reset();
+            _yotimer.start();
             _isTimeoutRunning = true; 
             
             // what needs to be started?
@@ -164,7 +166,7 @@
         }
         else if (depthLoop().getPosition() < 2.0) { //if the depth is greater than 0.2 feet, go to float broadcast
             _state = FLOAT_BROADCAST;
-            _fsm_timer.reset();
+            _fsm_timer.reset(); _yotimer.reset();
             _isTimeoutRunning = false;
         }
         
@@ -400,7 +402,7 @@
         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);
+            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; 
@@ -459,7 +461,8 @@
                 //retrieve commands from structs (loaded from missionleg.cfg file)
                 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);
 
                 //DEPTH COMMAND
                 //depthLoop().setCommand(_depth_command);  // I want this to be the max_depth in the legStruct
@@ -484,6 +487,8 @@
                 _state = LEG_POSITION_RISE; //  now start a timer on yo_timer for rise
                 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);
                 // _fsm_timer.reset();
                 // _isTimeoutRunning = false;
             }
@@ -493,6 +498,7 @@
                 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 ) {
@@ -500,17 +506,19 @@
                 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;
+                finish_leg =1;
                 //_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;
+                    _state = EMERGENCY_CLIMB;  // overridden here for testing
                     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");
+                    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);
-                _fsm_timer.reset();
-                    _isTimeoutRunning = false;
+                   _state = LEG_POSITION_RISE;
+              //      _fsm_timer.reset();
+              //      _isTimeoutRunning = false;
                 }
             }
 
@@ -610,8 +618,8 @@
         }
         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);
+            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
@@ -633,29 +641,42 @@
             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;
-            _fsm_timer.reset();
-            _isTimeoutRunning = false;
+            //  _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) {
+        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;
+            _state = EMERGENCY_CLIMB; // add diagnostics message
             finish_leg = 1;
-            _fsm_timer.reset();
+            _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?
@@ -712,6 +733,8 @@
             xbee().printf("\r\n\nstate: FLOAT_BROADCAST\r\n");
             _fsm_timer.reset(); // timer goes back to zero
             _fsm_timer.start(); // background timer starts running
+            _yotimer.reset();
+            _yotimer.start();
             _isTimeoutRunning = true; 
             
             // what needs to be started?
@@ -728,7 +751,7 @@
         
         // how exit?
         
-        if (_fsm_timer > _timeout ) {
+        if (_fsm_timer > _timeout || ( _yotimer > _state_transition_time)) {
             xbee().printf("FB: timed out\r\n");
             _state = SIT_IDLE;
             _fsm_timer.reset();
@@ -749,7 +772,7 @@
             
             //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;
         }