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@63:6cb0405fc6e6, 2018-06-18 (annotated)
- Committer:
- tnhnrl
- Date:
- Mon Jun 18 21:02:55 2018 +0000
- Revision:
- 63:6cb0405fc6e6
- Parent:
- 58:94b7fd55185e
- Child:
- 67:c86a4b464682
Version with code updates on multi-dive (still testing) and PID inputs and fixed data transmissions so it's working very quickly now on USB.
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 | 20:8987a9ae2bc7 | 3 | |
tnhnrl | 16:3363b9f14913 | 4 | #include "mbed.h" |
tnhnrl | 32:f2f8ae34aadc | 5 | #include <vector> |
tnhnrl | 20:8987a9ae2bc7 | 6 | |
tnhnrl | 16:3363b9f14913 | 7 | extern "C" void mbed_reset(); // utilized to reset the mbed |
tnhnrl | 20:8987a9ae2bc7 | 8 | |
tnhnrl | 20:8987a9ae2bc7 | 9 | // main finite state enumerations |
tnhnrl | 16:3363b9f14913 | 10 | enum { |
tnhnrl | 17:7c16b5671d0e | 11 | SIT_IDLE, // stops both motors, exits after a keyboard input |
tnhnrl | 49:47ffa4feb6db | 12 | CHECK_TUNING, // runs the system to the positions specified in the files |
tnhnrl | 17:7c16b5671d0e | 13 | FIND_NEUTRAL, // dives to depth at zero pitch, exits when stable |
tnhnrl | 17:7c16b5671d0e | 14 | DIVE, // dives to depth at negative pitch, exits when crossing a defined depth |
tnhnrl | 17:7c16b5671d0e | 15 | RISE, // rises to surface at positive pitch, exits when near surface |
tnhnrl | 58:94b7fd55185e | 16 | POSITION_DIVE, // NEW POSITION ONLY COMMANDS (inner loop) |
tnhnrl | 58:94b7fd55185e | 17 | POSITION_RISE, // NEW POSITION ONLY COMMANDS (inner loop) |
tnhnrl | 17:7c16b5671d0e | 18 | FLOAT_LEVEL, // bce position to float, pitch loop active at zero, exits when stable near zero pitch |
tnhnrl | 17:7c16b5671d0e | 19 | FLOAT_BROADCAST, // bce position to float, batt position forward to hold tail up, exits when actuators done |
tnhnrl | 17:7c16b5671d0e | 20 | EMERGENCY_CLIMB, // bce position to full rise, batt position to full aft, exits when at surface |
danstrider | 22:a10ee088403b | 21 | MULTI_DIVE, // multi-dive sequence |
tnhnrl | 28:16c83a2fdefa | 22 | MULTI_RISE, // multi-rise sequence |
tnhnrl | 32:f2f8ae34aadc | 23 | KEYBOARD, // "state" for tracking only |
tnhnrl | 63:6cb0405fc6e6 | 24 | TRANSMIT_MBED_LOG, |
tnhnrl | 45:16b8162188ca | 25 | RECEIVE_SEQUENCE |
tnhnrl | 20:8987a9ae2bc7 | 26 | }; |
tnhnrl | 20:8987a9ae2bc7 | 27 | |
tnhnrl | 20:8987a9ae2bc7 | 28 | // find_neutral finite state machine enumerations |
tnhnrl | 20:8987a9ae2bc7 | 29 | enum { |
danstrider | 22:a10ee088403b | 30 | NEUTRAL_SINKING, // increment the bce until really start sinking |
danstrider | 22:a10ee088403b | 31 | NEUTRAL_SLOWLY_RISE, // once sinking, arrest the sink |
danstrider | 22:a10ee088403b | 32 | NEUTRAL_CHECK_PITCH, // find level again, then save the data and exit |
tnhnrl | 32:f2f8ae34aadc | 33 | NEUTRAL_EXIT, // sub-FSM has completed all checks |
tnhnrl | 17:7c16b5671d0e | 34 | }; |
tnhnrl | 32:f2f8ae34aadc | 35 | |
tnhnrl | 32:f2f8ae34aadc | 36 | // test idea |
tnhnrl | 20:8987a9ae2bc7 | 37 | |
tnhnrl | 17:7c16b5671d0e | 38 | //struct for saving the data |
tnhnrl | 17:7c16b5671d0e | 39 | struct currentSequenceStruct { |
tnhnrl | 17:7c16b5671d0e | 40 | int state; //for the current StateMachine, states are ID-ed with enumeration |
tnhnrl | 17:7c16b5671d0e | 41 | float timeout; |
tnhnrl | 17:7c16b5671d0e | 42 | float depth; |
tnhnrl | 17:7c16b5671d0e | 43 | float pitch; |
tnhnrl | 16:3363b9f14913 | 44 | }; |
tnhnrl | 20:8987a9ae2bc7 | 45 | |
tnhnrl | 16:3363b9f14913 | 46 | class StateMachine { |
tnhnrl | 16:3363b9f14913 | 47 | public: |
tnhnrl | 16:3363b9f14913 | 48 | StateMachine(); |
tnhnrl | 16:3363b9f14913 | 49 | |
tnhnrl | 34:9b66c5188051 | 50 | int runStateMachine(); |
tnhnrl | 16:3363b9f14913 | 51 | |
tnhnrl | 57:ec69651c8c21 | 52 | void showSimpleMenu(); // simple menu |
tnhnrl | 57:ec69651c8c21 | 53 | void showDebugMenu(); // debug menu |
tnhnrl | 16:3363b9f14913 | 54 | |
tnhnrl | 17:7c16b5671d0e | 55 | void keyboard(); |
tnhnrl | 16:3363b9f14913 | 56 | |
tnhnrl | 49:47ffa4feb6db | 57 | void keyboard_menu_MANUAL_TUNING(); |
tnhnrl | 52:f207567d3ea4 | 58 | void keyboard_menu_STREAM_STATUS(); |
tnhnrl | 52:f207567d3ea4 | 59 | void keyboard_menu_CHANNEL_READINGS(); |
tnhnrl | 55:f4ec445c42fe | 60 | void keyboard_menu_POSITION_READINGS(); |
tnhnrl | 52:f207567d3ea4 | 61 | void keyboard_menu_RUDDER_SERVO_settings(); |
tnhnrl | 58:94b7fd55185e | 62 | void keyboard_menu_HEADING_PID_settings(); |
tnhnrl | 58:94b7fd55185e | 63 | void keyboard_menu_COUNTS_STATUS(); //remove? |
tnhnrl | 49:47ffa4feb6db | 64 | |
tnhnrl | 16:3363b9f14913 | 65 | void keyboard_menu_BCE_PID_settings(); |
tnhnrl | 16:3363b9f14913 | 66 | void keyboard_menu_BATT_PID_settings(); |
tnhnrl | 16:3363b9f14913 | 67 | void keyboard_menu_DEPTH_PID_settings(); |
tnhnrl | 16:3363b9f14913 | 68 | void keyboard_menu_PITCH_PID_settings(); |
tnhnrl | 16:3363b9f14913 | 69 | |
tnhnrl | 16:3363b9f14913 | 70 | float getDepthCommand(); |
tnhnrl | 16:3363b9f14913 | 71 | float getPitchCommand(); |
tnhnrl | 32:f2f8ae34aadc | 72 | float getDepthReading(); |
tnhnrl | 32:f2f8ae34aadc | 73 | float getPitchReading(); |
tnhnrl | 32:f2f8ae34aadc | 74 | float getTimerReading(); |
tnhnrl | 16:3363b9f14913 | 75 | |
tnhnrl | 32:f2f8ae34aadc | 76 | int runNeutralStateMachine(); //substate returns the state (which is used in overall FSM) |
tnhnrl | 17:7c16b5671d0e | 77 | |
tnhnrl | 28:16c83a2fdefa | 78 | int getState(); |
tnhnrl | 17:7c16b5671d0e | 79 | void setState(int input_state); |
tnhnrl | 28:16c83a2fdefa | 80 | |
tnhnrl | 17:7c16b5671d0e | 81 | void setTimeout(float input_timeout); |
tnhnrl | 17:7c16b5671d0e | 82 | void setDepthCommand(float input_depth_command); |
tnhnrl | 17:7c16b5671d0e | 83 | void setPitchCommand(float input_pitch_command); |
tnhnrl | 17:7c16b5671d0e | 84 | |
tnhnrl | 17:7c16b5671d0e | 85 | void setNeutralPositions(float batt_pos_mm, float bce_pos_mm); |
tnhnrl | 17:7c16b5671d0e | 86 | |
tnhnrl | 17:7c16b5671d0e | 87 | int timeoutRunning(); |
tnhnrl | 17:7c16b5671d0e | 88 | |
tnhnrl | 21:38c8544db6f4 | 89 | void getDiveSequence(); //used in multi-dive sequence with public variables for now |
tnhnrl | 17:7c16b5671d0e | 90 | |
tnhnrl | 32:f2f8ae34aadc | 91 | void runActiveNeutralStateMachine(); //new neutral substate returns the state (which is used in overall FSM) |
tnhnrl | 32:f2f8ae34aadc | 92 | |
tnhnrl | 34:9b66c5188051 | 93 | float * getLoggerArray(); //delete soon |
tnhnrl | 32:f2f8ae34aadc | 94 | |
tnhnrl | 34:9b66c5188051 | 95 | void recordData(int input_state); //record log data to file using MbedLogger class |
tnhnrl | 32:f2f8ae34aadc | 96 | void printDirectory(); |
tnhnrl | 32:f2f8ae34aadc | 97 | void recordState(int input_state); |
tnhnrl | 34:9b66c5188051 | 98 | void printCurrentSdLog(); //more tricky for SD card, work in progress |
tnhnrl | 32:f2f8ae34aadc | 99 | |
tnhnrl | 32:f2f8ae34aadc | 100 | void createNewFile(); |
tnhnrl | 32:f2f8ae34aadc | 101 | |
tnhnrl | 32:f2f8ae34aadc | 102 | void transmitData(); |
tnhnrl | 32:f2f8ae34aadc | 103 | |
tnhnrl | 36:966a86937e17 | 104 | float * dataArray(); |
tnhnrl | 36:966a86937e17 | 105 | |
tnhnrl | 16:3363b9f14913 | 106 | private: |
tnhnrl | 57:ec69651c8c21 | 107 | bool _debug_menu_on; // default is false to show simple menu, debug allows more tuning and has a lot of keyboard commands |
tnhnrl | 57:ec69651c8c21 | 108 | |
tnhnrl | 17:7c16b5671d0e | 109 | int _timeout; // generic timeout for every state, seconds |
tnhnrl | 28:16c83a2fdefa | 110 | float _pitchTolerance; // pitch angle tolerance for neutral finding exit criteria |
tnhnrl | 28:16c83a2fdefa | 111 | float _bceFloatPosition; // bce position for "float" states |
tnhnrl | 28:16c83a2fdefa | 112 | float _battFloatPosition; // batt position for "broadcast" state |
tnhnrl | 16:3363b9f14913 | 113 | |
tnhnrl | 32:f2f8ae34aadc | 114 | float _depth_command; // user keyboard depth |
tnhnrl | 58:94b7fd55185e | 115 | float _pitch_command; // user keyboard pitch |
tnhnrl | 58:94b7fd55185e | 116 | float _heading_command; // user keyboard heading |
tnhnrl | 58:94b7fd55185e | 117 | |
tnhnrl | 32:f2f8ae34aadc | 118 | float _depth_reading; // depth reading (to get the readings at the same time) |
tnhnrl | 32:f2f8ae34aadc | 119 | float _pitch_reading; // pitch reading (to get the readings at the same time) |
tnhnrl | 32:f2f8ae34aadc | 120 | float _timer_reading; // pitch reading (to get the readings at the same time) |
tnhnrl | 16:3363b9f14913 | 121 | |
tnhnrl | 16:3363b9f14913 | 122 | Timer timer; |
tnhnrl | 17:7c16b5671d0e | 123 | |
danstrider | 22:a10ee088403b | 124 | int _state; // Fine State Machine (FSM) state |
tnhnrl | 28:16c83a2fdefa | 125 | int _previous_state; // record previous state |
danstrider | 22:a10ee088403b | 126 | int _sub_state; // substate on find_neutral function |
danstrider | 22:a10ee088403b | 127 | int _previous_sub_state; // previous substate so that what goes into the sub-state is not being changed as it is processed |
tnhnrl | 24:c7d9b5bf3829 | 128 | float _neutral_timer; // keep time for rise/sink/level timer incremnets |
tnhnrl | 17:7c16b5671d0e | 129 | |
tnhnrl | 28:16c83a2fdefa | 130 | bool _isTimeoutRunning; |
tnhnrl | 17:7c16b5671d0e | 131 | |
tnhnrl | 28:16c83a2fdefa | 132 | bool _isSubStateTimerRunning; |
tnhnrl | 17:7c16b5671d0e | 133 | |
tnhnrl | 21:38c8544db6f4 | 134 | float _neutral_bce_pos_mm; |
tnhnrl | 21:38c8544db6f4 | 135 | float _neutral_batt_pos_mm; |
tnhnrl | 17:7c16b5671d0e | 136 | |
tnhnrl | 24:c7d9b5bf3829 | 137 | int _multi_dive_counter; |
tnhnrl | 28:16c83a2fdefa | 138 | |
tnhnrl | 21:38c8544db6f4 | 139 | currentSequenceStruct currentStateStruct; //type_of_struct struct_name |
tnhnrl | 21:38c8544db6f4 | 140 | |
tnhnrl | 21:38c8544db6f4 | 141 | bool _neutral_sub_state_active; // controls neutral_sub_state |
tnhnrl | 21:38c8544db6f4 | 142 | |
tnhnrl | 21:38c8544db6f4 | 143 | float _depth_KP; |
tnhnrl | 21:38c8544db6f4 | 144 | float _depth_KI; |
tnhnrl | 21:38c8544db6f4 | 145 | float _depth_KD; |
tnhnrl | 21:38c8544db6f4 | 146 | |
tnhnrl | 21:38c8544db6f4 | 147 | float _pitch_KP; |
tnhnrl | 21:38c8544db6f4 | 148 | float _pitch_KI; |
tnhnrl | 21:38c8544db6f4 | 149 | float _pitch_KD; |
tnhnrl | 28:16c83a2fdefa | 150 | |
tnhnrl | 45:16b8162188ca | 151 | int _state_array[256]; //used to print out the states |
tnhnrl | 28:16c83a2fdefa | 152 | int _state_array_counter; //used to iterate through state records |
tnhnrl | 45:16b8162188ca | 153 | int _substate_array[256]; //used to print out the sub-states |
tnhnrl | 28:16c83a2fdefa | 154 | int _substate_array_counter; //used to iterate through sub-state records |
tnhnrl | 23:434f04ef1fad | 155 | |
tnhnrl | 24:c7d9b5bf3829 | 156 | int _substate; |
tnhnrl | 24:c7d9b5bf3829 | 157 | int _previous_substate; |
tnhnrl | 28:16c83a2fdefa | 158 | |
tnhnrl | 28:16c83a2fdefa | 159 | float _max_recorded_depth_neutral; |
tnhnrl | 28:16c83a2fdefa | 160 | float _max_recorded_depth_dive; |
tnhnrl | 32:f2f8ae34aadc | 161 | |
tnhnrl | 32:f2f8ae34aadc | 162 | float _neutral_sink_command_mm; //defaults for neutral finding sub-FSM |
tnhnrl | 32:f2f8ae34aadc | 163 | float _neutral_rise_command_mm; |
tnhnrl | 32:f2f8ae34aadc | 164 | float _neutral_pitch_command_mm; |
tnhnrl | 32:f2f8ae34aadc | 165 | |
tnhnrl | 32:f2f8ae34aadc | 166 | float _max_recorded_auto_neutral_depth; |
tnhnrl | 32:f2f8ae34aadc | 167 | |
tnhnrl | 32:f2f8ae34aadc | 168 | //float _logger_array[1048][5]; //array to hold the input, one log per dive (row/col) |
tnhnrl | 32:f2f8ae34aadc | 169 | bool _is_log_timer_running; |
tnhnrl | 32:f2f8ae34aadc | 170 | float _log_timer; |
tnhnrl | 32:f2f8ae34aadc | 171 | |
tnhnrl | 32:f2f8ae34aadc | 172 | float _data_log[9]; |
tnhnrl | 32:f2f8ae34aadc | 173 | |
tnhnrl | 58:94b7fd55185e | 174 | float _BCE_dive_offset; // NEW COMMANDS FOR POSITION CONTROLLER |
tnhnrl | 58:94b7fd55185e | 175 | float _BMM_dive_offset; |
tnhnrl | 58:94b7fd55185e | 176 | |
tnhnrl | 58:94b7fd55185e | 177 | float getFloatUserInput(); |
tnhnrl | 57:ec69651c8c21 | 178 | |
tnhnrl | 32:f2f8ae34aadc | 179 | volatile bool _file_closed; |
tnhnrl | 16:3363b9f14913 | 180 | }; |
tnhnrl | 20:8987a9ae2bc7 | 181 | |
tnhnrl | 16:3363b9f14913 | 182 | #endif |