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@17:7c16b5671d0e, 2017-11-21 (annotated)
- Committer:
- tnhnrl
- Date:
- Tue Nov 21 22:03:26 2017 +0000
- Revision:
- 17:7c16b5671d0e
- Parent:
- 16:3363b9f14913
- Child:
- 20:8987a9ae2bc7
FSG code commit 11/21
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tnhnrl | 16:3363b9f14913 | 1 | #ifndef STATEMACHINE_HPP |
tnhnrl | 16:3363b9f14913 | 2 | #define STATEMACHINE_HPP |
tnhnrl | 16:3363b9f14913 | 3 | |
tnhnrl | 16:3363b9f14913 | 4 | #include "mbed.h" |
tnhnrl | 16:3363b9f14913 | 5 | |
tnhnrl | 16:3363b9f14913 | 6 | extern "C" void mbed_reset(); // utilized to reset the mbed |
tnhnrl | 16:3363b9f14913 | 7 | |
tnhnrl | 16:3363b9f14913 | 8 | // state enumerations |
tnhnrl | 16:3363b9f14913 | 9 | enum { |
tnhnrl | 17:7c16b5671d0e | 10 | SIT_IDLE, // stops both motors, exits after a keyboard input |
tnhnrl | 17:7c16b5671d0e | 11 | KEYBOARD, // handles an individual keypress, exits to state by a keyboard menu |
tnhnrl | 17:7c16b5671d0e | 12 | FIND_NEUTRAL, // dives to depth at zero pitch, exits when stable |
tnhnrl | 17:7c16b5671d0e | 13 | DIVE, // dives to depth at negative pitch, exits when crossing a defined depth |
tnhnrl | 17:7c16b5671d0e | 14 | RISE, // rises to surface at positive pitch, exits when near surface |
tnhnrl | 17:7c16b5671d0e | 15 | FLOAT_LEVEL, // bce position to float, pitch loop active at zero, exits when stable near zero pitch |
tnhnrl | 17:7c16b5671d0e | 16 | FLOAT_BROADCAST, // bce position to float, batt position forward to hold tail up, exits when actuators done |
tnhnrl | 17:7c16b5671d0e | 17 | EMERGENCY_CLIMB, // bce position to full rise, batt position to full aft, exits when at surface |
tnhnrl | 17:7c16b5671d0e | 18 | NEUTRAL_SINKING, // NEW: FIND_NEUTRAL immediately goes into "sinking" |
tnhnrl | 17:7c16b5671d0e | 19 | NEUTRAL_SLOWLY_RISE, // NEW: Once sinking times out, goes into "check pitch" |
tnhnrl | 17:7c16b5671d0e | 20 | NEUTRAL_CHECK_PITCH, // NEW: Once pitch rate confirmed, saves the data and exits to RISE state |
tnhnrl | 17:7c16b5671d0e | 21 | NEUTRAL_EXIT, // NEW: sub-FSM has completed all checks |
tnhnrl | 17:7c16b5671d0e | 22 | MULTI_DIVE, // NEW: multi-dive sequence |
tnhnrl | 17:7c16b5671d0e | 23 | MULTI_RISE // NEW: multi-rise sequence |
tnhnrl | 17:7c16b5671d0e | 24 | }; |
tnhnrl | 17:7c16b5671d0e | 25 | |
tnhnrl | 17:7c16b5671d0e | 26 | //struct for saving the data |
tnhnrl | 17:7c16b5671d0e | 27 | struct currentSequenceStruct { |
tnhnrl | 17:7c16b5671d0e | 28 | int state; //for the current StateMachine, states are ID-ed with enumeration |
tnhnrl | 17:7c16b5671d0e | 29 | float timeout; |
tnhnrl | 17:7c16b5671d0e | 30 | float depth; |
tnhnrl | 17:7c16b5671d0e | 31 | float pitch; |
tnhnrl | 16:3363b9f14913 | 32 | }; |
tnhnrl | 16:3363b9f14913 | 33 | |
tnhnrl | 16:3363b9f14913 | 34 | class StateMachine { |
tnhnrl | 16:3363b9f14913 | 35 | public: |
tnhnrl | 16:3363b9f14913 | 36 | StateMachine(); |
tnhnrl | 16:3363b9f14913 | 37 | |
tnhnrl | 16:3363b9f14913 | 38 | void runStateMachine(); |
tnhnrl | 16:3363b9f14913 | 39 | |
tnhnrl | 16:3363b9f14913 | 40 | void showMenu(); |
tnhnrl | 16:3363b9f14913 | 41 | |
tnhnrl | 17:7c16b5671d0e | 42 | void keyboard(); |
tnhnrl | 16:3363b9f14913 | 43 | |
tnhnrl | 16:3363b9f14913 | 44 | void keyboard_menu_BCE_PID_settings(); |
tnhnrl | 16:3363b9f14913 | 45 | void keyboard_menu_BATT_PID_settings(); |
tnhnrl | 16:3363b9f14913 | 46 | void keyboard_menu_DEPTH_PID_settings(); |
tnhnrl | 16:3363b9f14913 | 47 | void keyboard_menu_PITCH_PID_settings(); |
tnhnrl | 16:3363b9f14913 | 48 | |
tnhnrl | 16:3363b9f14913 | 49 | float getDepthCommand(); |
tnhnrl | 16:3363b9f14913 | 50 | float getPitchCommand(); |
tnhnrl | 16:3363b9f14913 | 51 | |
tnhnrl | 17:7c16b5671d0e | 52 | int findNeutralSubState(int input_state); //substate returns the state (which is used in overall FSM) |
tnhnrl | 17:7c16b5671d0e | 53 | |
tnhnrl | 17:7c16b5671d0e | 54 | void setState(int input_state); |
tnhnrl | 17:7c16b5671d0e | 55 | int getState(); |
tnhnrl | 17:7c16b5671d0e | 56 | void setTimeout(float input_timeout); |
tnhnrl | 17:7c16b5671d0e | 57 | void setDepthCommand(float input_depth_command); |
tnhnrl | 17:7c16b5671d0e | 58 | void setPitchCommand(float input_pitch_command); |
tnhnrl | 17:7c16b5671d0e | 59 | |
tnhnrl | 17:7c16b5671d0e | 60 | void setNeutralPositions(float batt_pos_mm, float bce_pos_mm); |
tnhnrl | 17:7c16b5671d0e | 61 | |
tnhnrl | 17:7c16b5671d0e | 62 | int timeoutRunning(); |
tnhnrl | 17:7c16b5671d0e | 63 | |
tnhnrl | 17:7c16b5671d0e | 64 | //new (quick and dirty version using public variables) |
tnhnrl | 17:7c16b5671d0e | 65 | void getDiveSequence(); |
tnhnrl | 17:7c16b5671d0e | 66 | |
tnhnrl | 16:3363b9f14913 | 67 | private: |
tnhnrl | 17:7c16b5671d0e | 68 | int _timeout; // generic timeout for every state, seconds |
tnhnrl | 16:3363b9f14913 | 69 | float depthTolerance; // depth tolerance for neutral finding exit critera |
tnhnrl | 16:3363b9f14913 | 70 | float pitchTolerance; // pitch angle tolerance for neutral finding exit criteria |
tnhnrl | 16:3363b9f14913 | 71 | float bceFloatPosition; // bce position for "float" states |
tnhnrl | 16:3363b9f14913 | 72 | float battFloatPosition; // batt position for "broadcast" state |
tnhnrl | 16:3363b9f14913 | 73 | |
tnhnrl | 16:3363b9f14913 | 74 | float depthCommand; // user keyboard depth |
tnhnrl | 16:3363b9f14913 | 75 | float pitchCommand; // user keyboard depth |
tnhnrl | 16:3363b9f14913 | 76 | |
tnhnrl | 16:3363b9f14913 | 77 | Timer timer; |
tnhnrl | 17:7c16b5671d0e | 78 | |
tnhnrl | 17:7c16b5671d0e | 79 | int _state; //Fine State Machine (FSM) state |
tnhnrl | 17:7c16b5671d0e | 80 | int _sub_state; //substate on find_neutral function |
tnhnrl | 17:7c16b5671d0e | 81 | int _previous_sub_state; //previous substate so that what goes into the sub-state is not being changed as it is processed |
tnhnrl | 17:7c16b5671d0e | 82 | float _neutral_sink_timer; |
tnhnrl | 17:7c16b5671d0e | 83 | float _neutral_rise_timer; |
tnhnrl | 17:7c16b5671d0e | 84 | |
tnhnrl | 17:7c16b5671d0e | 85 | //volatile bool isTimeoutRunning; |
tnhnrl | 17:7c16b5671d0e | 86 | bool isTimeoutRunning; |
tnhnrl | 17:7c16b5671d0e | 87 | |
tnhnrl | 17:7c16b5671d0e | 88 | float previousPosition_mm; |
tnhnrl | 17:7c16b5671d0e | 89 | bool isSubStateTimeoutRunning; |
tnhnrl | 17:7c16b5671d0e | 90 | |
tnhnrl | 17:7c16b5671d0e | 91 | float _neutral_buoyancy_bce_pos_mm; |
tnhnrl | 17:7c16b5671d0e | 92 | float _neutral_buoyancy_batt_pos_mm; |
tnhnrl | 17:7c16b5671d0e | 93 | |
tnhnrl | 17:7c16b5671d0e | 94 | int _keyboard_state; |
tnhnrl | 17:7c16b5671d0e | 95 | int _next_state; //next state is used to prevent states from changing as the FSM executes |
tnhnrl | 17:7c16b5671d0e | 96 | |
tnhnrl | 17:7c16b5671d0e | 97 | int _state_counter; |
tnhnrl | 17:7c16b5671d0e | 98 | currentSequenceStruct currentStateStruct; //type_of_struct struct_name |
tnhnrl | 16:3363b9f14913 | 99 | }; |
tnhnrl | 16:3363b9f14913 | 100 | |
tnhnrl | 16:3363b9f14913 | 101 | #endif |