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

Revision:
87:6d95f853dab3
Parent:
85:dd8176285b6e
Child:
88:1813f583cee9
--- a/StateMachine/StateMachine.hpp	Thu May 02 20:34:16 2019 +0000
+++ b/StateMachine/StateMachine.hpp	Wed May 08 13:24:04 2019 +0000
@@ -25,6 +25,8 @@
     LEG_POSITION_RISE,
     TX_MBED_LOG,
     RX_SEQUENCE,
+    START_SWIM,
+    FLYING_IDLE,
     FB_EXIT,
     ENDLEG_WAIT
 };
@@ -127,7 +129,14 @@
     float _pitchTolerance;       // pitch angle tolerance for neutral finding exit criteria
     float _bceFloatPosition;     // bce position for "float" states
     float _battFloatPosition;    // batt position for "broadcast" state
+    float _disconnect_batt_pos_mm;  // all the way forward
+    float _batt_flying_pos_mm;
     
+    float _timeout_splashdown;  // two minutes??
+    int _motorDisconnect_triggered;
+    
+    int _start_swim_entry;
+    int _neutral_entry_state;
     float _depth_command;       // user keyboard depth
     float _pitch_command;       // user keyboard pitch
     float _heading_command;     // user keyboard heading
@@ -144,7 +153,7 @@
     int _sub_state;             // substate on find_neutral function
     int _previous_sub_state;    // previous substate so that what goes into the sub-state is not being changed as it is processed
     float _neutral_timer;  // keep time for rise/sink/level timer incremnets
-    
+    int _neutral_success;
     bool _isTimeoutRunning;
     
     bool _isSubStateTimerRunning;