Diff: StateMachine/StateMachine.cpp

Revision:

102:0f430de62447

Parent:

97:2b4f78a54227

--- a/StateMachine/StateMachine.cpp	Fri Jun 28 15:07:41 2019 +0000
+++ b/StateMachine/StateMachine.cpp	Fri Sep 13 16:51:48 2019 +0000
@@ -21,8 +21,12 @@
     _start_swim_entry = SIT_IDLE;
     _neutral_entry_state = SIT_IDLE;
     //new commands
-    _BCE_dive_offset = 0.0;     //starting at the limits
+    _BCE_dive_offset = 0.0;     //starting at no movement
     _BMM_dive_offset = 0.0;
+    bmm_dive_mm = 10.0;
+    bce_dive_mm = 20.0;
+    bce_rise_mm = 21.0;
+    bmm_rise_mm = 11.0;     // non zero values but just guesses
     //new commands
     
     _neutral_timer        = 0;                  //timer used in FIND_NEUTRAL sub-FSM
@@ -287,6 +291,20 @@
             _substate_array[_substate_array_counter] = EMERGENCY_CLIMB;  //save to state array
             _substate_array_counter++;
         }
+        else if( (depthLoop().getPosition() > configFileIO().swimConstants.altim_blank_m)    //  ending on altimeter abort - bad, endleg, etc
+                     && (fabs(imu().getRoll()) < 45) && (altimLoop().getPosition() < configFileIO().swimConstants.altim_abort_m)) {
+                //just go to leg_position_rise unless it is the second time
+                       _state = EMERGENCY_CLIMB;
+                       finish_leg =1;
+                       sprintf(buf, "FIND_NEUTRAL : ALTIMETER ABORT : depth=%f altimeter=%f - go to emergency climb\n\n\r",
+                                      depthLoop().getPosition(), altimLoop().getPosition() );
+                    mbedLogger().appendDiagFile(buf,3);
+                       configFileIO().swimConstants.endleg_reason = 7;  // find_neutral fail
+                       _fsm_timer.reset();
+                       _isTimeoutRunning = false;
+                       
+                }
+
         
         
         //what is active? (neutral finding sub-function runs until completion)        
@@ -356,8 +374,13 @@
             _state = RISE;
             _fsm_timer.reset();
             _isTimeoutRunning = false;
-        }
- 
+        }  // add else if(depth > altimeter_blank && altimeter_depth < altimeter_abort  && not_inverted)  then exit with message
+        else if( (depthLoop().getPosition() > configFileIO().swimConstants.altim_blank_m)    //  ending on altimieter abort - bad, endleg, etc
+                     && (fabs(imu().getRoll()) < 45) && (altimLoop().getPosition() < configFileIO().swimConstants.altim_abort_m)) {
+                _state = RISE;
+                _fsm_timer.reset();
+            _isTimeoutRunning = false;
+                }
         // 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 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(),
@@ -472,6 +495,8 @@
                    mbedLogger().appendDiagFile(buf,3);
             configFileIO().report_still_inverted( fabs(imu().getRoll()), _yo_time);  // tells how long you waited, puts in file "inverted.txt"
             _state = FLOAT_BROADCAST;
+            finish_leg = 1;    // failure mode
+            configFileIO().swimConstants.endleg_reason = 5;  //still inverted
             _fsm_timer.reset(); _yotimer.reset();
             _isTimeoutRunning = false;
         }
@@ -483,8 +508,21 @@
             // print message ?
             _fsm_timer.reset(); _yotimer.reset();
             _isTimeoutRunning = false;
-        }
-        
+        }  // add else if(depth > altimeter_blank && altimeter_depth < altimeter_abort  && not_inverted)  then exit with message
+        else if( (depthLoop().getPosition() > configFileIO().swimConstants.altim_blank_m)    //  ending on altimieter abort - bad, endleg, etc
+                     && (fabs(imu().getRoll()) < 45) && (altimLoop().getPosition() < configFileIO().swimConstants.altim_abort_m)) {
+                //just go to emergency climb
+                       _state = EMERGENCY_CLIMB;
+                       finish_leg =1;
+                       sprintf(buf, "START_SWIM: ALTIMETER ABORT - : depth=%f altimeter=%f - go to emergency climb\n\n\r",
+                                      depthLoop().getPosition(), altimLoop().getPosition() );
+                    mbedLogger().appendDiagFile(buf,3);
+                       configFileIO().swimConstants.endleg_reason = 7;  // altimeter abort + no_find_neutral
+                       _fsm_timer.reset();
+                       _isTimeoutRunning = false;
+                       
+                }
+
         //WHAT IS ACTIVE?
         //print status to screen continuously
         xbee().printf("START_SWIM: 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", 
@@ -493,7 +531,8 @@
         
         bce().setPosition_mm(depthLoop().getOutput());  //constantly checking the Outer Loop output to move the motors
         batt().setPosition_mm(pitchLoop().getOutput());  //these together should hold a depth, rather than turn around and rise
-        
+        // ACTIVE RUDDER CONTROL
+        rudder().setPosition_deg(headingLoop().getOutput());
         break;
    
     case RISE :
@@ -567,9 +606,13 @@
  
             // 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);
+            bce_dive_mm = configFileIO().swimConstants.bce_dive_mm;
+            bmm_dive_mm = configFileIO().swimConstants.bmm_dive_mm;
+            //   old way                  batt().setPosition_mm(_neutral_batt_pos_mm + _BMM_dive_offset);
+            //   old way                  bce().setPosition_mm(_neutral_bce_pos_mm - _BCE_dive_offset);
             
+            batt().setPosition_mm(_neutral_batt_pos_mm + bmm_dive_mm);
+            bce().setPosition_mm(_neutral_bce_pos_mm - bce_dive_mm);
             //DEPTH COMMAND
             depthLoop().setCommand(_depth_command);
                         
@@ -598,8 +641,21 @@
             _state = POSITION_RISE;
             _fsm_timer.reset();
             _isTimeoutRunning = false;
-        }
+        }// add else if(depth > altimeter_blank && altimeter_depth < altimeter_abort  && not_inverted)  then exit with message
+        else if( (depthLoop().getPosition() > configFileIO().swimConstants.altim_blank_m)    //  ending on altimieter abort - bad, endleg, etc
+                     && (fabs(imu().getRoll()) < 45) && (altimLoop().getPosition() < configFileIO().swimConstants.altim_abort_m)) {
+                //just go to leg_position_rise unless it is the second time
+                       _state = POSITION_RISE;
+                       sprintf(buf, "POSITION_DIVE: ALTIMETER ABORT - !: depth=%f altimeter=%f - go to POSITION_RISE\n\n\r",
+                                      depthLoop().getPosition(), altimLoop().getPosition() );
+                    mbedLogger().appendDiagFile(buf,3);
+                       configFileIO().swimConstants.endleg_reason = 3;  // altimeter abort
+                       _fsm_timer.reset();
+                       _isTimeoutRunning = false;
+                       
+                }
  
+
         // 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 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(),
@@ -633,9 +689,13 @@
  
             // 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);
+            //   old way              batt().setPosition_mm(_neutral_batt_pos_mm + _BMM_dive_offset);
+            //   old way              bce().setPosition_mm(_neutral_bce_pos_mm - _BCE_dive_offset);
+            bce_dive_mm = configFileIO().swimConstants.bce_dive_mm;
+            bmm_dive_mm = configFileIO().swimConstants.bmm_dive_mm;
             
+            batt().setPosition_mm(_neutral_batt_pos_mm + bmm_dive_mm);
+            bce().setPosition_mm(_neutral_bce_pos_mm - bce_dive_mm);
             
             //retrieve commands from structs (loaded from legfile.txt file)
             stateMachine().getLegParams();
@@ -675,8 +735,14 @@
 
                 // 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);
+                
+                bce_dive_mm = configFileIO().swimConstants.bce_dive_mm;
+                bmm_dive_mm = configFileIO().swimConstants.bmm_dive_mm;
+                //   old way                  batt().setPosition_mm(_neutral_batt_pos_mm + _BMM_dive_offset);
+                //   old way                  bce().setPosition_mm(_neutral_bce_pos_mm - _BCE_dive_offset);
+            
+                batt().setPosition_mm(_neutral_batt_pos_mm + bmm_dive_mm);
+                bce().setPosition_mm(_neutral_bce_pos_mm - bce_dive_mm);
                 //retrieve commands from structs (loaded from missionleg.cfg file)
                 leg_max_depth = currentLegStateStruct.max_depth;
                 leg_heading = currentLegStateStruct.heading;
@@ -705,6 +771,7 @@
                 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;
+                configFileIO().swimConstants.endleg_reason = 1;  // normal timeout
                 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);
@@ -721,25 +788,53 @@
                 // _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;
+                configFileIO().swimConstants.endleg_reason = 2;   //yo_timeout
                 //_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;  // overridden here for testing
                     finish_leg = 1;
+                    configFileIO().swimConstants.endleg_reason = 6;  // 6 is too slow descent
                     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;
+                } // add else if(depth > altimeter_blank && altimeter_depth < altimeter_abort  && not_inverted)  then exit with message
+            }
+            else if( (depthLoop().getPosition() > configFileIO().swimConstants.altim_blank_m)    //  ending on altimieter abort - bad, endleg, etc
+                     && (fabs(imu().getRoll()) < 45) && (altimLoop().getPosition() < configFileIO().swimConstants.altim_abort_m)) {
+                //just go to leg_position_rise unless it is the second time
+                    if(configFileIO().swimConstants.altim_abort_count  >= 2) {  // more than one altimeter abort
+                       _state = EMERGENCY_CLIMB;
+                       finish_leg =1;
+                       sprintf(buf, "LEG POS DIVE: ALTIMETER ABORT - more than one!: depth=%f altimeter=%f - go to emergency climb\n\n\r",
+                                      depthLoop().getPosition(), altimLoop().getPosition() );
+                    mbedLogger().appendDiagFile(buf,3);
+                       configFileIO().swimConstants.endleg_reason = 3;  // altimeter abort
+                       _fsm_timer.reset();
+                       _isTimeoutRunning = false;
+                       
+                    }
+                // just go to leg_position_rise unless it is the second time
+                    if(configFileIO().swimConstants.altim_abort_count  < 2) {
+                       _state = LEG_POSITION_RISE;
+                       _fsm_timer.reset();
+                       printf(buf, "LEG POS DIVE: ALTIMETER ABORT first one:  ONLY go to leg_postiion_rise -depth=%f altimeter=%f - go to emergency climb\n\n\r",
+                                      depthLoop().getPosition(), altimLoop().getPosition() );
+                       mbedLogger().appendDiagFile(buf,3);
+                       _isTimeoutRunning = false;
+                       configFileIO().swimConstants.altim_abort_count += 1;
+                    }
                 }
-            }
 
             // 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]                                         \r",
@@ -831,8 +926,14 @@
             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
+                bce_rise_mm = configFileIO().swimConstants.bce_rise_mm;
+                bmm_rise_mm = configFileIO().swimConstants.bmm_rise_mm;
+                
+                batt().setPosition_mm(_neutral_batt_pos_mm - bmm_rise_mm);  // lighter nose
+                bce().setPosition_mm(_neutral_bce_pos_mm + bce_rise_mm);  // more buoyant
+                
+            //   old way  batt().setPosition_mm(_neutral_batt_pos_mm - _BMM_dive_offset);          //reversing the BCE and BATT positions
+            //   old way  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)
@@ -857,8 +958,15 @@
             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
+            // what are the commands? (using inner loops except for heading outer loop)            
+                bce_rise_mm = configFileIO().swimConstants.bce_rise_mm;
+                bmm_rise_mm = configFileIO().swimConstants.bmm_rise_mm;
+                
+                batt().setPosition_mm(_neutral_batt_pos_mm - bmm_rise_mm);  // lighter nose
+                bce().setPosition_mm(_neutral_bce_pos_mm + bce_rise_mm);  // more buoyant
+            
+            // old way    batt().setPosition_mm(_neutral_batt_pos_mm - _BMM_dive_offset);          //reversing the BCE and BATT positions
+            //  old way   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)
@@ -877,6 +985,7 @@
             // _fsm_timer.reset();
             sprintf(buf, "LEG POSITION DIVE  Ended via overall timeout\n\n\r");
             mbedLogger().appendDiagFile(buf,0);
+            configFileIO().swimConstants.endleg_reason = 1;  // normal timeout
             // _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
@@ -901,7 +1010,8 @@
             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");
+            configFileIO().swimConstants.endleg_reason = 2;  // yo_timeout
+            sprintf(buf, "LEG POSITION DIVE ... ENDS on yo_time too long, exit to emergency climb + finish_leg=1\n\n\r");
             mbedLogger().appendDiagFile(buf,0);
             }
  
@@ -992,6 +1102,7 @@
                 _state = ENDLEG_WAIT;
                 sprintf(buf, "in FLOAT_BROADCAST still timed out .... but also finish_leg==1 so go to ENDLEG_WAIT\n\n");
                 mbedLogger().appendDiagFile(buf,3); 
+                if(_neutral_success == -1) {configFileIO().swimConstants.endleg_reason = 4;  }// find_neutral failed
             }  // allows wait at surface for xbee messaging to not close program
         }
         
@@ -1055,8 +1166,17 @@
             _state = MULTI_RISE;
             _fsm_timer.reset();
             _isTimeoutRunning = false;
-        }
-        
+        }// add else if(depth > altimeter_blank && altimeter_depth < altimeter_abort  && not_inverted)  then exit with message
+        else if( (depthLoop().getPosition() > configFileIO().swimConstants.altim_blank_m)    //  ending on altimieter abort - bad, endleg, etc
+                     && (fabs(imu().getRoll()) < 45) && (altimLoop().getPosition() < configFileIO().swimConstants.altim_abort_m)) {
+                //just go to leg_position_rise unless it is the second time
+                       _state = MULTI_RISE;                     
+                       _fsm_timer.reset();
+                       _isTimeoutRunning = false;
+                       
+                    }
+                
+
         // 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 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());
@@ -1135,7 +1255,9 @@
         }
         
         // 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 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());
+        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()); 
         
@@ -1311,7 +1433,8 @@
             if (!_isSubStateTimerRunning) {                
                 _neutral_timer = _fsm_timer.read() + 5; //record the time when this block is first entered and add 5 seconds
                 
-                xbee().printf("\r\n\nNEUTRAL_SINKING: Next retraction at %0.1f sec [current time: %0.1f] (pitch: %0.1f) (BCE getSetPosition: %0.1f)\r\n", _neutral_timer, _fsm_timer.read(), pitchLoop().getPosition(), bce().getSetPosition_mm());
+                xbee().printf("\r\n\nNEUTRAL_SINKING: Next retraction at %0.1f sec [current time: %0.1f] (pitch: %0.1f) (BCE getSetPosition: %0.1f)\r\n", _neutral_timer, _fsm_timer.read(), 
+                pitchLoop().getPosition(), bce().getSetPosition_mm());
                 
                 // what are the commands? (BCE linear actuator active, no BMM or pitch movement)
                 bce().setPosition_mm(bce().getSetPosition_mm() - 2.5);