File content as of revision 28:16c83a2fdefa:

#ifndef STATEMACHINE_HPP
#define STATEMACHINE_HPP
 
#include "mbed.h"
 
extern "C" void mbed_reset();           // utilized to reset the mbed
 
// main finite state enumerations
enum {
    SIT_IDLE,               // stops both motors, exits after a keyboard input
    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
    MULTI_DIVE,             // multi-dive sequence
    MULTI_RISE,             // multi-rise sequence
    KEYBOARD                // "state" for tracking only
};
 
// find_neutral finite state machine enumerations
enum {
    NEUTRAL_FIRST_PITCH,    // NEW first state: find level by moving the battery
    NEUTRAL_SINKING,        // increment the bce until really start sinking
    NEUTRAL_SLOWLY_RISE,    // once sinking, arrest the sink
    NEUTRAL_CHECK_PITCH,    // find level again, then save the data and exit
    NEUTRAL_EXIT            // sub-FSM has completed all checks
};
 
//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 runNeutralStateMachine();   //substate returns the state (which is used in overall FSM)
    
    int getState();
    void setState(int input_state);
    
    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();
    
    void getDiveSequence(); //used in multi-dive sequence with public variables for now
    
private:
    int _timeout;                // generic timeout for every state, seconds
    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 _previous_state;        // record previous 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_timer;  // keep time for rise/sink/level timer incremnets
    
    bool _isTimeoutRunning;
    
    bool _isSubStateTimerRunning;
    
    float _neutral_bce_pos_mm;
    float _neutral_batt_pos_mm;
    
    int _multi_dive_counter;
    
    currentSequenceStruct currentStateStruct;   //type_of_struct struct_name
    
    bool _neutral_sub_state_active;             // controls neutral_sub_state
    
    float _depth_KP;
    float _depth_KI;
    float _depth_KD;
    
    float _pitch_KP;
    float _pitch_KI;
    float _pitch_KD;

    int _state_array[1024];                         //used to print out the states
    int _state_array_counter;                       //used to iterate through state records    
    int _substate_array[1024];                      //used to print out the sub-states
    int _substate_array_counter;                    //used to iterate through sub-state records
    
    int _substate;
    int _previous_substate;
    
    float _max_recorded_depth_neutral;
    float _max_recorded_depth_dive;
};
 
#endif