update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
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 float _timeout_inversion; // two minutes?? 00138 int _motorDisconnect_triggered; 00139 00140 int _start_swim_entry; 00141 int _neutral_entry_state; 00142 float _depth_command; // user keyboard depth 00143 float _pitch_command; // user keyboard pitch 00144 float _heading_command; // user keyboard heading 00145 00146 float _depth_reading; // depth reading (to get the readings at the same time) 00147 float _pitch_reading; // pitch reading (to get the readings at the same time) 00148 float _timer_reading; // pitch reading (to get the readings at the same time) 00149 00150 Timer _fsm_timer; //timing variable used in class how do I know this is seconds? 00151 Timer _yotimer; // timer for each yo in a yoyo leg 00152 00153 volatile int _state; // current state of Finite State Machine (FSM) 00154 int _previous_state; // record previous state 00155 int _sub_state; // substate on find_neutral function 00156 int _previous_sub_state; // previous substate so that what goes into the sub-state is not being changed as it is processed 00157 float _neutral_timer; // keep time for rise/sink/level timer incremnets 00158 int _neutral_success; 00159 bool _isTimeoutRunning; 00160 00161 bool _isSubStateTimerRunning; 00162 00163 float _neutral_bce_pos_mm; 00164 float _neutral_batt_pos_mm; 00165 00166 int _multi_dive_counter; 00167 int _multi_leg_counter; 00168 00169 currentSequenceStruct currentStateStruct; //type_of_struct struct_name 00170 currentLegStruct currentLegStateStruct; //type_of_struct struct_name 00171 00172 float _depth_KP; 00173 float _depth_KI; 00174 float _depth_KD; 00175 00176 float _pitch_KP; 00177 float _pitch_KI; 00178 float _pitch_KD; 00179 00180 int _state_array[256]; //used to print out the states 00181 int _state_array_counter; //used to iterate through state records 00182 int _substate_array[256]; //used to print out the sub-states 00183 int _substate_array_counter; //used to iterate through sub-state records 00184 00185 int _substate; 00186 int _previous_substate; 00187 00188 float _max_recorded_depth_neutral; 00189 float _max_recorded_depth_dive; 00190 00191 float _neutral_sink_command_mm; //defaults for neutral finding sub-FSM 00192 float _neutral_rise_command_mm; 00193 float _neutral_pitch_command_mm; 00194 00195 float _max_recorded_auto_neutral_depth; 00196 00197 bool _is_log_timer_running; 00198 float _log_timer; 00199 00200 float _BCE_dive_offset; // NEW COMMANDS FOR POSITION CONTROLLER 00201 float _BMM_dive_offset; 00202 float bce_dive_mm, bce_rise_mm, bmm_dive_mm, bmm_rise_mm; 00203 00204 00205 float getFloatUserInput(); 00206 00207 //new 00208 float _batt_filter_freq; 00209 float _bce_filter_freq; 00210 float _pitch_filter_freq; 00211 float _depth_filter_freq; 00212 float _heading_filter_freq; 00213 00214 float _batt_deadband; 00215 float _bce_deadband; 00216 float _pitch_deadband; 00217 float _depth_deadband; 00218 float _heading_deadband; 00219 }; 00220 00221 #endif
Generated on Wed Jul 13 2022 15:28:17 by 1.7.2