StateMachine.hpp

00001 #ifndef STATEMACHINE_HPP
00002 #define STATEMACHINE_HPP
00003  
00004 #include "mbed.h"
00005 #include <vector>
00006  
00007 extern "C" void mbed_reset();           // utilized to reset the mbed
00008  
00009 // main finite state enumerations
00010 enum {
00011     SIT_IDLE,               // stops both motors, exits after a keyboard input
00012     CHECK_TUNING,           // runs the system to the positions specified in the files
00013     FIND_NEUTRAL,           // dives to depth at zero pitch, exits when stable
00014     DIVE,                   // dives to depth at negative pitch, exits when crossing a defined depth
00015     RISE,                   // rises to surface at positive pitch, exits when near surface
00016     POSITION_DIVE,          // NEW POSITION ONLY COMMANDS (inner loop)
00017     POSITION_RISE,          // NEW POSITION ONLY COMMANDS (inner loop)
00018     FLOAT_LEVEL,            // bce position to float, pitch loop active at zero, exits when stable near zero pitch
00019     FLOAT_BROADCAST,        // bce position to float, batt position forward to hold tail up, exits when actuators done
00020     EMERGENCY_CLIMB,        // bce position to full rise, batt position to full aft, exits when at surface
00021     MULTI_DIVE,             // multi-dive sequence
00022     MULTI_RISE,             // multi-rise sequence
00023     KEYBOARD,               // "state" for tracking only
00024     LEG_POSITION_DIVE,      // leg has heading and min,max depths and timeout
00025     LEG_POSITION_RISE,
00026     TX_MBED_LOG,
00027     RX_SEQUENCE,
00028     START_SWIM,
00029     FLYING_IDLE,
00030     FB_EXIT,
00031     ENDLEG_WAIT
00032 };
00033  
00034 // find_neutral finite state machine enumerations
00035 enum {
00036     NEUTRAL_SINKING,        // increment the bce until really start sinking
00037     NEUTRAL_SLOWLY_RISE,    // once sinking, arrest the sink
00038     NEUTRAL_CHECK_PITCH,    // find level again, then save the data and exit
00039     NEUTRAL_EXIT,            // sub-FSM has completed all checks
00040 };
00041 
00042 // test idea
00043  
00044 //struct for saving the data
00045 struct currentSequenceStruct {
00046     int state;      //for the current StateMachine, states are ID-ed with enumeration
00047     float timeout;
00048     float depth;
00049     float pitch;
00050 };
00051 //new struct for saving the leg data
00052 struct currentLegStruct {
00053     int state;      //for the current StateMachine, states are ID-ed with enumeration
00054     float timeout;
00055     float yo_time;
00056     float min_depth;
00057     float max_depth;
00058     float heading;
00059 };
00060  
00061 class StateMachine {
00062 public:
00063     StateMachine();
00064     
00065     int runStateMachine();
00066     
00067     void printSimpleMenu();  // simple menu
00068     void printDebugMenu();   // debug menu
00069     
00070     void keyboard();
00071     
00072     void keyboard_menu_MANUAL_TUNING();
00073     void keyboard_menu_STREAM_STATUS();
00074     void keyboard_menu_CHANNEL_READINGS();
00075     void keyboard_menu_POSITION_READINGS();
00076     void keyboard_menu_RUDDER_SERVO_settings();
00077     void keyboard_menu_HEADING_PID_settings();
00078     void keyboard_menu_COUNTS_STATUS();             //remove?
00079     
00080     void keyboard_menu_BCE_PID_settings();
00081     void keyboard_menu_BATT_PID_settings();
00082     void keyboard_menu_DEPTH_PID_settings();
00083     void keyboard_menu_PITCH_PID_settings();
00084     
00085     float getDepthCommand();
00086     float getPitchCommand();
00087     float getDepthReading();
00088     float getPitchReading();
00089     float getTimerReading();
00090     
00091     int runNeutralStateMachine();           //substate returns the state (which is used in overall FSM)
00092     
00093     int getState();
00094     void setState(int input_state);
00095     void setstate_frommain(int set_state_val, int new_timeout);
00096     
00097     void setTimeout(float input_timeout);    
00098     void setDepthCommand(float input_depth_command);    
00099     void setPitchCommand(float input_pitch_command);
00100     
00101     void setNeutralPositions(float batt_pos_mm, float bce_pos_mm);
00102     
00103     void getDiveSequence(); //used in multi-dive sequence with public variables for now
00104     void getLegParams(); //used in multi-leg sequence with public variables for now
00105     
00106     void runActiveNeutralStateMachine();    //new neutral substate returns the state (which is used in overall FSM)
00107     
00108     float * getLoggerArray();               //delete soon
00109     
00110     void printDirectory();
00111     void printCurrentSdLog();               //more tricky for SD card, work in progress
00112     
00113     void createNewFile();
00114     
00115     void transmitData();
00116     
00117     float * dataArray();
00118     
00119 //GUI UPDATE FUNCTIONS
00120     float getTimerValue();
00121     
00122     void logFileMenu();         //instead of immediately erasing log files, NRL Stennis suggests a confirmation 
00123     
00124 private:
00125     bool _debug_menu_on;         // default is false to show simple menu, debug allows more tuning and has a lot of keyboard commands
00126 
00127     int _timeout;                // generic timeout for every state, seconds
00128     int _yo_time;                //  time allowed for one up or down in a leg of yo-yos
00129     int _state_transition_time;
00130     float _pitchTolerance;       // pitch angle tolerance for neutral finding exit criteria
00131     float _bceFloatPosition;     // bce position for "float" states
00132     float _battFloatPosition;    // batt position for "broadcast" state
00133     float _disconnect_batt_pos_mm;  // all the way forward
00134     float _batt_flying_pos_mm;
00135     
00136     float _timeout_splashdown;  // two minutes??
00137     int _motorDisconnect_triggered;
00138     
00139     int _start_swim_entry;
00140     int _neutral_entry_state;
00141     float _depth_command;       // user keyboard depth
00142     float _pitch_command;       // user keyboard pitch
00143     float _heading_command;     // user keyboard heading
00144     
00145     float _depth_reading;       // depth reading (to get the readings at the same time)
00146     float _pitch_reading;       // pitch reading (to get the readings at the same time)   
00147     float _timer_reading;       // pitch reading (to get the readings at the same time)
00148     
00149     Timer _fsm_timer;               //timing variable used in class  how do I know this is seconds?
00150     Timer _yotimer;                // timer for each yo in a yoyo leg
00151     
00152     volatile int _state;                 // current state of Finite State Machine (FSM)
00153     int _previous_state;        // record previous state
00154     int _sub_state;             // substate on find_neutral function
00155     int _previous_sub_state;    // previous substate so that what goes into the sub-state is not being changed as it is processed
00156     float _neutral_timer;  // keep time for rise/sink/level timer incremnets
00157     int _neutral_success;
00158     bool _isTimeoutRunning;
00159     
00160     bool _isSubStateTimerRunning;
00161     
00162     float _neutral_bce_pos_mm;
00163     float _neutral_batt_pos_mm;
00164     
00165     int _multi_dive_counter;
00166     int _multi_leg_counter;
00167     
00168     currentSequenceStruct currentStateStruct;   //type_of_struct struct_name
00169     currentLegStruct      currentLegStateStruct;   //type_of_struct struct_name
00170     
00171     float _depth_KP;
00172     float _depth_KI;
00173     float _depth_KD;
00174     
00175     float _pitch_KP;
00176     float _pitch_KI;
00177     float _pitch_KD;
00178 
00179     int _state_array[256];                         //used to print out the states
00180     int _state_array_counter;                       //used to iterate through state records    
00181     int _substate_array[256];                      //used to print out the sub-states
00182     int _substate_array_counter;                    //used to iterate through sub-state records
00183     
00184     int _substate;
00185     int _previous_substate;
00186     
00187     float _max_recorded_depth_neutral;
00188     float _max_recorded_depth_dive;
00189     
00190     float _neutral_sink_command_mm;                 //defaults for neutral finding sub-FSM
00191     float _neutral_rise_command_mm;
00192     float _neutral_pitch_command_mm;
00193     
00194     float _max_recorded_auto_neutral_depth;
00195     
00196     bool _is_log_timer_running;
00197     float _log_timer;
00198     
00199     float _BCE_dive_offset;                         // NEW COMMANDS FOR POSITION CONTROLLER
00200     float _BMM_dive_offset;
00201     
00202     float getFloatUserInput();
00203     
00204     //new
00205     float _batt_filter_freq;
00206     float _bce_filter_freq;
00207     float _pitch_filter_freq;
00208     float _depth_filter_freq;
00209     float _heading_filter_freq;
00210     
00211     float _batt_deadband;
00212     float _bce_deadband;
00213     float _pitch_deadband;
00214     float _depth_deadband;
00215     float _heading_deadband;
00216 };
00217  
00218 #endif