File content as of revision 17:7c16b5671d0e:

#ifndef STATEMACHINE_HPP
#define STATEMACHINE_HPP

#include "mbed.h"

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
    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 {
public:
    StateMachine();
    
    void runStateMachine();
    
    void showMenu();
    
    void keyboard();
    
    void keyboard_menu_BCE_PID_settings();
    void keyboard_menu_BATT_PID_settings();
    void keyboard_menu_DEPTH_PID_settings();
    void keyboard_menu_PITCH_PID_settings();
    
    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
    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
    float battFloatPosition;    // batt position for "broadcast" state
    
    float depthCommand;         // user keyboard depth
    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