File content as of revision 23:434f04ef1fad:

#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
};
 
// 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)
    
    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();
    
    void getDiveSequence(); //used in multi-dive sequence with public variables for now
    
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;  // keep time for sink incremnets
    float _neutral_rise_timer;  // keep time for rise increment
    float _level_timer;         // keep time for finding level increments
    
    bool isTimeoutRunning;
    
    float previousPosition_mm;
    bool isSubStateTimerRunning;
    
    float _neutral_bce_pos_mm;
    float _neutral_batt_pos_mm;
    
    int _state_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];                    //print out the states
    int _state_array_counter;
};
 
#endif