Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

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@71:939d179478c4, 2018-06-29 (annotated)

Committer:
tnhnrl
Date:
Fri Jun 29 21:52:31 2018 +0000
Revision:
71:939d179478c4
Parent:
69:919ac8d7e023 
recheck

Who changed what in which revision?

UserRevisionLine numberNew 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 71:939d179478c4 24 TX_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 69:919ac8d7e023 52 void printSimpleMenu(); // simple menu
tnhnrl 69:919ac8d7e023 53 void printDebugMenu(); // 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 34:9b66c5188051 97 void printCurrentSdLog(); //more tricky for SD card, work in progress
tnhnrl 32:f2f8ae34aadc 98
tnhnrl 32:f2f8ae34aadc 99 void createNewFile();
tnhnrl 32:f2f8ae34aadc 100
tnhnrl 32:f2f8ae34aadc 101 void transmitData();
tnhnrl 32:f2f8ae34aadc 102
tnhnrl 36:966a86937e17 103 float * dataArray();
tnhnrl 36:966a86937e17 104
tnhnrl 67:c86a4b464682 105 void checkMotorPositions(); //checks motor positions as a safe-guard against broken string pot or bad channel readings
tnhnrl 67:c86a4b464682 106
tnhnrl 16:3363b9f14913 107 private:
tnhnrl 57:ec69651c8c21 108 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 109
tnhnrl 17:7c16b5671d0e 110 int _timeout; // generic timeout for every state, seconds
tnhnrl 28:16c83a2fdefa 111 float _pitchTolerance; // pitch angle tolerance for neutral finding exit criteria
tnhnrl 28:16c83a2fdefa 112 float _bceFloatPosition; // bce position for "float" states
tnhnrl 28:16c83a2fdefa 113 float _battFloatPosition; // batt position for "broadcast" state
tnhnrl 16:3363b9f14913 114
tnhnrl 32:f2f8ae34aadc 115 float _depth_command; // user keyboard depth
tnhnrl 58:94b7fd55185e 116 float _pitch_command; // user keyboard pitch
tnhnrl 58:94b7fd55185e 117 float _heading_command; // user keyboard heading
tnhnrl 58:94b7fd55185e 118
tnhnrl 32:f2f8ae34aadc 119 float _depth_reading; // depth reading (to get the readings at the same time)
tnhnrl 32:f2f8ae34aadc 120 float _pitch_reading; // pitch reading (to get the readings at the same time)
tnhnrl 32:f2f8ae34aadc 121 float _timer_reading; // pitch reading (to get the readings at the same time)
tnhnrl 16:3363b9f14913 122
tnhnrl 16:3363b9f14913 123 Timer timer;
tnhnrl 17:7c16b5671d0e 124
danstrider 22:a10ee088403b 125 int _state; // Fine State Machine (FSM) state
tnhnrl 28:16c83a2fdefa 126 int _previous_state; // record previous state
danstrider 22:a10ee088403b 127 int _sub_state; // substate on find_neutral function
danstrider 22:a10ee088403b 128 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 129 float _neutral_timer; // keep time for rise/sink/level timer incremnets
tnhnrl 17:7c16b5671d0e 130
tnhnrl 28:16c83a2fdefa 131 bool _isTimeoutRunning;
tnhnrl 17:7c16b5671d0e 132
tnhnrl 28:16c83a2fdefa 133 bool _isSubStateTimerRunning;
tnhnrl 17:7c16b5671d0e 134
tnhnrl 21:38c8544db6f4 135 float _neutral_bce_pos_mm;
tnhnrl 21:38c8544db6f4 136 float _neutral_batt_pos_mm;
tnhnrl 17:7c16b5671d0e 137
tnhnrl 24:c7d9b5bf3829 138 int _multi_dive_counter;
tnhnrl 28:16c83a2fdefa 139
tnhnrl 21:38c8544db6f4 140 currentSequenceStruct currentStateStruct; //type_of_struct struct_name
tnhnrl 21:38c8544db6f4 141
tnhnrl 21:38c8544db6f4 142 bool _neutral_sub_state_active; // controls neutral_sub_state
tnhnrl 21:38c8544db6f4 143
tnhnrl 21:38c8544db6f4 144 float _depth_KP;
tnhnrl 21:38c8544db6f4 145 float _depth_KI;
tnhnrl 21:38c8544db6f4 146 float _depth_KD;
tnhnrl 21:38c8544db6f4 147
tnhnrl 21:38c8544db6f4 148 float _pitch_KP;
tnhnrl 21:38c8544db6f4 149 float _pitch_KI;
tnhnrl 21:38c8544db6f4 150 float _pitch_KD;
tnhnrl 28:16c83a2fdefa 151
tnhnrl 45:16b8162188ca 152 int _state_array[256]; //used to print out the states
tnhnrl 28:16c83a2fdefa 153 int _state_array_counter; //used to iterate through state records
tnhnrl 45:16b8162188ca 154 int _substate_array[256]; //used to print out the sub-states
tnhnrl 28:16c83a2fdefa 155 int _substate_array_counter; //used to iterate through sub-state records
tnhnrl 23:434f04ef1fad 156
tnhnrl 24:c7d9b5bf3829 157 int _substate;
tnhnrl 24:c7d9b5bf3829 158 int _previous_substate;
tnhnrl 28:16c83a2fdefa 159
tnhnrl 28:16c83a2fdefa 160 float _max_recorded_depth_neutral;
tnhnrl 28:16c83a2fdefa 161 float _max_recorded_depth_dive;
tnhnrl 32:f2f8ae34aadc 162
tnhnrl 32:f2f8ae34aadc 163 float _neutral_sink_command_mm; //defaults for neutral finding sub-FSM
tnhnrl 32:f2f8ae34aadc 164 float _neutral_rise_command_mm;
tnhnrl 32:f2f8ae34aadc 165 float _neutral_pitch_command_mm;
tnhnrl 32:f2f8ae34aadc 166
tnhnrl 32:f2f8ae34aadc 167 float _max_recorded_auto_neutral_depth;
tnhnrl 32:f2f8ae34aadc 168
tnhnrl 32:f2f8ae34aadc 169 //float _logger_array[1048][5]; //array to hold the input, one log per dive (row/col)
tnhnrl 32:f2f8ae34aadc 170 bool _is_log_timer_running;
tnhnrl 32:f2f8ae34aadc 171 float _log_timer;
tnhnrl 32:f2f8ae34aadc 172
tnhnrl 32:f2f8ae34aadc 173 float _data_log[9];
tnhnrl 32:f2f8ae34aadc 174
tnhnrl 58:94b7fd55185e 175 float _BCE_dive_offset; // NEW COMMANDS FOR POSITION CONTROLLER
tnhnrl 58:94b7fd55185e 176 float _BMM_dive_offset;
tnhnrl 58:94b7fd55185e 177
tnhnrl 58:94b7fd55185e 178 float getFloatUserInput();
tnhnrl 57:ec69651c8c21 179
tnhnrl 32:f2f8ae34aadc 180 volatile bool _file_closed;
tnhnrl 16:3363b9f14913 181 };
tnhnrl 20:8987a9ae2bc7 182
tnhnrl 16:3363b9f14913 183 #endif