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@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?

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 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