Diff: StateMachine/StateMachine.hpp

Revision:

17:7c16b5671d0e

Parent:

16:3363b9f14913

Child:

20:8987a9ae2bc7

--- a/StateMachine/StateMachine.hpp	Mon Nov 06 22:57:56 2017 +0000
+++ b/StateMachine/StateMachine.hpp	Tue Nov 21 22:03:26 2017 +0000
@@ -2,20 +2,33 @@
 #define STATEMACHINE_HPP
 
 #include "mbed.h"
-//#include "StaticDefs.hpp"
 
 extern "C" void mbed_reset();           // utilized to reset the mbed
 
 // state enumerations
 enum {
-    SIT_IDLE,           // stops both motors, exits after a keyboard input
-    KEYBOARD,           // handles an individual keypress, exits to state by a keyboard menu
-    FIND_NEUTRAL,       // dives to depth at zero pitch, exits when stable
-    DIVE,               // dives to depth at negative pitch, exits when crossing a defined depth
-    RISE,               // rises to surface at positive pitch, exits when near surface
-    FLOAT_LEVEL,        // bce position to float, pitch loop active at zero, exits when stable near zero pitch
-    FLOAT_BROADCAST,    // bce position to float, batt position forward to hold tail up, exits when actuators done
-    EMERGENCY_CLIMB     // bce position to full rise, batt position to full aft, exits when at surface
+    SIT_IDLE,               // stops both motors, exits after a keyboard input
+    KEYBOARD,               // handles an individual keypress, exits to state by a keyboard menu
+    FIND_NEUTRAL,           // dives to depth at zero pitch, exits when stable
+    DIVE,                   // dives to depth at negative pitch, exits when crossing a defined depth
+    RISE,                   // rises to surface at positive pitch, exits when near surface
+    FLOAT_LEVEL,            // bce position to float, pitch loop active at zero, exits when stable near zero pitch
+    FLOAT_BROADCAST,        // bce position to float, batt position forward to hold tail up, exits when actuators done
+    EMERGENCY_CLIMB,        // bce position to full rise, batt position to full aft, exits when at surface
+    NEUTRAL_SINKING,        // NEW: FIND_NEUTRAL immediately goes into "sinking" 
+    NEUTRAL_SLOWLY_RISE,    // NEW: Once sinking times out, goes into "check pitch"
+    NEUTRAL_CHECK_PITCH,    // NEW: Once pitch rate confirmed, saves the data and exits to RISE state
+    NEUTRAL_EXIT,           // NEW: sub-FSM has completed all checks
+    MULTI_DIVE,             // NEW: multi-dive sequence
+    MULTI_RISE              // NEW: multi-rise sequence
+};
+
+//struct for saving the data
+struct currentSequenceStruct {
+    int state;      //for the current StateMachine, states are ID-ed with enumeration
+    float timeout;
+    float depth;
+    float pitch;
 };
 
 class StateMachine {
@@ -26,7 +39,7 @@
     
     void showMenu();
     
-    int keyboard();
+    void keyboard();
     
     void keyboard_menu_BCE_PID_settings();
     void keyboard_menu_BATT_PID_settings();
@@ -36,8 +49,23 @@
     float getDepthCommand();
     float getPitchCommand();
     
+    int findNeutralSubState(int input_state);   //substate returns the state (which is used in overall FSM)
+    
+    void setState(int input_state);
+    int getState();
+    void setTimeout(float input_timeout);    
+    void setDepthCommand(float input_depth_command);    
+    void setPitchCommand(float input_pitch_command);
+    
+    void setNeutralPositions(float batt_pos_mm, float bce_pos_mm);
+    
+    int timeoutRunning();
+    
+    //new (quick and dirty version using public variables)
+    void getDiveSequence();
+    
 private:
-    int timeout;                // generic timeout for every state, seconds
+    int _timeout;                // generic timeout for every state, seconds
     float depthTolerance;       // depth tolerance for neutral finding exit critera
     float pitchTolerance;       // pitch angle tolerance for neutral finding exit criteria
     float bceFloatPosition;     // bce position for "float" states
@@ -47,6 +75,27 @@
     float pitchCommand;         // user keyboard depth
     
     Timer timer;
+    
+    int _state;                         //Fine State Machine (FSM) state
+    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_sink_timer;
+    float _neutral_rise_timer;
+    
+    //volatile bool isTimeoutRunning;
+    bool isTimeoutRunning;
+    
+    float previousPosition_mm;
+    bool isSubStateTimeoutRunning;
+    
+    float _neutral_buoyancy_bce_pos_mm;
+    float _neutral_buoyancy_batt_pos_mm;
+    
+    int _keyboard_state;
+    int _next_state;    //next state is used to prevent states from changing as the FSM executes
+    
+    int _state_counter;
+    currentSequenceStruct currentStateStruct; //type_of_struct struct_name
 };
 
 #endif
\ No newline at end of file