most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown
Dependencies: mbed MODSERIAL FATFileSystem
StateMachine/StateMachine.hpp
- Committer:
- tnhnrl
- Date:
- 2017-12-20
- Revision:
- 34:9b66c5188051
- Parent:
- 32:f2f8ae34aadc
- Child:
- 36:966a86937e17
File content as of revision 34:9b66c5188051:
#ifndef STATEMACHINE_HPP #define STATEMACHINE_HPP #include "mbed.h" #include <vector> #include "DirectoryList.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_AUTO_NEUTRAL_DEPTH, FIND_AUTO_NEUTRAL_PITCH, TRANSMIT_DATA }; // find_neutral finite state machine enumerations enum { 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 }; // test idea //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(); int 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(); float getDepthReading(); float getPitchReading(); float getTimerReading(); 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 void runActiveNeutralStateMachine(); //new neutral substate returns the state (which is used in overall FSM) float * getLoggerArray(); //delete soon void recordData(int input_state); //record log data to file using MbedLogger class void printDirectory(); void recordState(int input_state); void printCurrentSdLog(); //more tricky for SD card, work in progress void createNewFile(); void transmitData(); 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 _depth_command; // user keyboard depth float _pitch_command; // user keyboard depth float _depth_reading; // depth reading (to get the readings at the same time) float _pitch_reading; // pitch reading (to get the readings at the same time) float _timer_reading; // pitch reading (to get the readings at the same time) 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; float _neutral_sink_command_mm; //defaults for neutral finding sub-FSM float _neutral_rise_command_mm; float _neutral_pitch_command_mm; float _max_recorded_auto_neutral_depth; //float _logger_array[1048][5]; //array to hold the input, one log per dive (row/col) bool _is_log_timer_running; float _log_timer; float _data_log[9]; volatile bool _file_closed; }; #endif