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@92:52a91656458a, 2019-05-13 (annotated)

Committer:
joel_ssc
Date:
Mon May 13 19:25:26 2019 +0000
Revision:
92:52a91656458a
Parent:
88:1813f583cee9 
version for first flight test, timeouts not yet set correctly

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tnhnrl 73:f6f378311c8d 1 #ifndef STATEMACHINE_HPP
tnhnrl 73:f6f378311c8d 2 #define STATEMACHINE_HPP
tnhnrl 73:f6f378311c8d 3
tnhnrl 73:f6f378311c8d 4 #include "mbed.h"
tnhnrl 73:f6f378311c8d 5 #include <vector>
tnhnrl 73:f6f378311c8d 6
tnhnrl 73:f6f378311c8d 7 extern "C" void mbed_reset(); // utilized to reset the mbed
tnhnrl 73:f6f378311c8d 8
tnhnrl 73:f6f378311c8d 9 // main finite state enumerations
tnhnrl 73:f6f378311c8d 10 enum {
tnhnrl 73:f6f378311c8d 11 SIT_IDLE, // stops both motors, exits after a keyboard input
tnhnrl 73:f6f378311c8d 12 CHECK_TUNING, // runs the system to the positions specified in the files
tnhnrl 73:f6f378311c8d 13 FIND_NEUTRAL, // dives to depth at zero pitch, exits when stable
tnhnrl 73:f6f378311c8d 14 DIVE, // dives to depth at negative pitch, exits when crossing a defined depth
tnhnrl 73:f6f378311c8d 15 RISE, // rises to surface at positive pitch, exits when near surface
tnhnrl 73:f6f378311c8d 16 POSITION_DIVE, // NEW POSITION ONLY COMMANDS (inner loop)
tnhnrl 73:f6f378311c8d 17 POSITION_RISE, // NEW POSITION ONLY COMMANDS (inner loop)
tnhnrl 73:f6f378311c8d 18 FLOAT_LEVEL, // bce position to float, pitch loop active at zero, exits when stable near zero pitch
tnhnrl 73:f6f378311c8d 19 FLOAT_BROADCAST, // bce position to float, batt position forward to hold tail up, exits when actuators done
tnhnrl 73:f6f378311c8d 20 EMERGENCY_CLIMB, // bce position to full rise, batt position to full aft, exits when at surface
tnhnrl 73:f6f378311c8d 21 MULTI_DIVE, // multi-dive sequence
tnhnrl 73:f6f378311c8d 22 MULTI_RISE, // multi-rise sequence
tnhnrl 73:f6f378311c8d 23 KEYBOARD, // "state" for tracking only
joel_ssc 82:0981b9ada820 24 LEG_POSITION_DIVE, // leg has heading and min,max depths and timeout
joel_ssc 82:0981b9ada820 25 LEG_POSITION_RISE,
tnhnrl 73:f6f378311c8d 26 TX_MBED_LOG,
joel_ssc 82:0981b9ada820 27 RX_SEQUENCE,
joel_ssc 87:6d95f853dab3 28 START_SWIM,
joel_ssc 87:6d95f853dab3 29 FLYING_IDLE,
joel_ssc 85:dd8176285b6e 30 FB_EXIT,
joel_ssc 85:dd8176285b6e 31 ENDLEG_WAIT
tnhnrl 73:f6f378311c8d 32 };
tnhnrl 73:f6f378311c8d 33
tnhnrl 73:f6f378311c8d 34 // find_neutral finite state machine enumerations
tnhnrl 73:f6f378311c8d 35 enum {
tnhnrl 73:f6f378311c8d 36 NEUTRAL_SINKING, // increment the bce until really start sinking
tnhnrl 73:f6f378311c8d 37 NEUTRAL_SLOWLY_RISE, // once sinking, arrest the sink
tnhnrl 73:f6f378311c8d 38 NEUTRAL_CHECK_PITCH, // find level again, then save the data and exit
tnhnrl 73:f6f378311c8d 39 NEUTRAL_EXIT, // sub-FSM has completed all checks
tnhnrl 73:f6f378311c8d 40 };
tnhnrl 73:f6f378311c8d 41
tnhnrl 73:f6f378311c8d 42 // test idea
tnhnrl 73:f6f378311c8d 43
tnhnrl 73:f6f378311c8d 44 //struct for saving the data
tnhnrl 73:f6f378311c8d 45 struct currentSequenceStruct {
tnhnrl 73:f6f378311c8d 46 int state; //for the current StateMachine, states are ID-ed with enumeration
tnhnrl 73:f6f378311c8d 47 float timeout;
tnhnrl 73:f6f378311c8d 48 float depth;
tnhnrl 73:f6f378311c8d 49 float pitch;
tnhnrl 73:f6f378311c8d 50 };
joel_ssc 82:0981b9ada820 51 //new struct for saving the leg data
joel_ssc 82:0981b9ada820 52 struct currentLegStruct {
joel_ssc 82:0981b9ada820 53 int state; //for the current StateMachine, states are ID-ed with enumeration
joel_ssc 82:0981b9ada820 54 float timeout;
joel_ssc 82:0981b9ada820 55 float yo_time;
joel_ssc 82:0981b9ada820 56 float min_depth;
joel_ssc 82:0981b9ada820 57 float max_depth;
joel_ssc 82:0981b9ada820 58 float heading;
joel_ssc 82:0981b9ada820 59 };
tnhnrl 73:f6f378311c8d 60
tnhnrl 73:f6f378311c8d 61 class StateMachine {
tnhnrl 73:f6f378311c8d 62 public:
tnhnrl 73:f6f378311c8d 63 StateMachine();
tnhnrl 73:f6f378311c8d 64
tnhnrl 73:f6f378311c8d 65 int runStateMachine();
tnhnrl 73:f6f378311c8d 66
tnhnrl 73:f6f378311c8d 67 void printSimpleMenu(); // simple menu
tnhnrl 73:f6f378311c8d 68 void printDebugMenu(); // debug menu
tnhnrl 73:f6f378311c8d 69
tnhnrl 73:f6f378311c8d 70 void keyboard();
tnhnrl 73:f6f378311c8d 71
tnhnrl 73:f6f378311c8d 72 void keyboard_menu_MANUAL_TUNING();
tnhnrl 73:f6f378311c8d 73 void keyboard_menu_STREAM_STATUS();
tnhnrl 73:f6f378311c8d 74 void keyboard_menu_CHANNEL_READINGS();
tnhnrl 73:f6f378311c8d 75 void keyboard_menu_POSITION_READINGS();
tnhnrl 73:f6f378311c8d 76 void keyboard_menu_RUDDER_SERVO_settings();
tnhnrl 73:f6f378311c8d 77 void keyboard_menu_HEADING_PID_settings();
tnhnrl 73:f6f378311c8d 78 void keyboard_menu_COUNTS_STATUS(); //remove?
tnhnrl 73:f6f378311c8d 79
tnhnrl 73:f6f378311c8d 80 void keyboard_menu_BCE_PID_settings();
tnhnrl 73:f6f378311c8d 81 void keyboard_menu_BATT_PID_settings();
tnhnrl 73:f6f378311c8d 82 void keyboard_menu_DEPTH_PID_settings();
tnhnrl 73:f6f378311c8d 83 void keyboard_menu_PITCH_PID_settings();
tnhnrl 73:f6f378311c8d 84
tnhnrl 73:f6f378311c8d 85 float getDepthCommand();
tnhnrl 73:f6f378311c8d 86 float getPitchCommand();
tnhnrl 73:f6f378311c8d 87 float getDepthReading();
tnhnrl 73:f6f378311c8d 88 float getPitchReading();
tnhnrl 73:f6f378311c8d 89 float getTimerReading();
tnhnrl 73:f6f378311c8d 90
tnhnrl 73:f6f378311c8d 91 int runNeutralStateMachine(); //substate returns the state (which is used in overall FSM)
tnhnrl 73:f6f378311c8d 92
tnhnrl 73:f6f378311c8d 93 int getState();
tnhnrl 73:f6f378311c8d 94 void setState(int input_state);
joel_ssc 88:1813f583cee9 95 void setstate_frommain(int set_state_val, int new_timeout);
tnhnrl 73:f6f378311c8d 96
tnhnrl 73:f6f378311c8d 97 void setTimeout(float input_timeout);
tnhnrl 73:f6f378311c8d 98 void setDepthCommand(float input_depth_command);
tnhnrl 73:f6f378311c8d 99 void setPitchCommand(float input_pitch_command);
tnhnrl 73:f6f378311c8d 100
tnhnrl 73:f6f378311c8d 101 void setNeutralPositions(float batt_pos_mm, float bce_pos_mm);
tnhnrl 73:f6f378311c8d 102
tnhnrl 73:f6f378311c8d 103 void getDiveSequence(); //used in multi-dive sequence with public variables for now
joel_ssc 82:0981b9ada820 104 void getLegParams(); //used in multi-leg sequence with public variables for now
tnhnrl 73:f6f378311c8d 105
tnhnrl 73:f6f378311c8d 106 void runActiveNeutralStateMachine(); //new neutral substate returns the state (which is used in overall FSM)
tnhnrl 73:f6f378311c8d 107
tnhnrl 73:f6f378311c8d 108 float * getLoggerArray(); //delete soon
tnhnrl 73:f6f378311c8d 109
tnhnrl 73:f6f378311c8d 110 void printDirectory();
tnhnrl 73:f6f378311c8d 111 void printCurrentSdLog(); //more tricky for SD card, work in progress
tnhnrl 73:f6f378311c8d 112
tnhnrl 73:f6f378311c8d 113 void createNewFile();
tnhnrl 73:f6f378311c8d 114
tnhnrl 73:f6f378311c8d 115 void transmitData();
tnhnrl 73:f6f378311c8d 116
tnhnrl 73:f6f378311c8d 117 float * dataArray();
tnhnrl 73:f6f378311c8d 118
tnhnrl 73:f6f378311c8d 119 //GUI UPDATE FUNCTIONS
tnhnrl 73:f6f378311c8d 120 float getTimerValue();
tnhnrl 73:f6f378311c8d 121
tnhnrl 80:4e5d306d695b 122 void logFileMenu(); //instead of immediately erasing log files, NRL Stennis suggests a confirmation
tnhnrl 80:4e5d306d695b 123
tnhnrl 73:f6f378311c8d 124 private:
tnhnrl 73:f6f378311c8d 125 bool _debug_menu_on; // default is false to show simple menu, debug allows more tuning and has a lot of keyboard commands
tnhnrl 73:f6f378311c8d 126
tnhnrl 73:f6f378311c8d 127 int _timeout; // generic timeout for every state, seconds
joel_ssc 82:0981b9ada820 128 int _yo_time; // time allowed for one up or down in a leg of yo-yos
joel_ssc 84:eccd8e837134 129 int _state_transition_time;
tnhnrl 73:f6f378311c8d 130 float _pitchTolerance; // pitch angle tolerance for neutral finding exit criteria
tnhnrl 73:f6f378311c8d 131 float _bceFloatPosition; // bce position for "float" states
tnhnrl 73:f6f378311c8d 132 float _battFloatPosition; // batt position for "broadcast" state
joel_ssc 87:6d95f853dab3 133 float _disconnect_batt_pos_mm; // all the way forward
joel_ssc 87:6d95f853dab3 134 float _batt_flying_pos_mm;
tnhnrl 73:f6f378311c8d 135
joel_ssc 87:6d95f853dab3 136 float _timeout_splashdown; // two minutes??
joel_ssc 87:6d95f853dab3 137 int _motorDisconnect_triggered;
joel_ssc 87:6d95f853dab3 138
joel_ssc 87:6d95f853dab3 139 int _start_swim_entry;
joel_ssc 87:6d95f853dab3 140 int _neutral_entry_state;
tnhnrl 73:f6f378311c8d 141 float _depth_command; // user keyboard depth
tnhnrl 73:f6f378311c8d 142 float _pitch_command; // user keyboard pitch
tnhnrl 73:f6f378311c8d 143 float _heading_command; // user keyboard heading
tnhnrl 73:f6f378311c8d 144
tnhnrl 73:f6f378311c8d 145 float _depth_reading; // depth reading (to get the readings at the same time)
tnhnrl 73:f6f378311c8d 146 float _pitch_reading; // pitch reading (to get the readings at the same time)
tnhnrl 73:f6f378311c8d 147 float _timer_reading; // pitch reading (to get the readings at the same time)
tnhnrl 73:f6f378311c8d 148
joel_ssc 85:dd8176285b6e 149 Timer _fsm_timer; //timing variable used in class how do I know this is seconds?
joel_ssc 82:0981b9ada820 150 Timer _yotimer; // timer for each yo in a yoyo leg
tnhnrl 73:f6f378311c8d 151
tnhnrl 73:f6f378311c8d 152 volatile int _state; // current state of Finite State Machine (FSM)
tnhnrl 73:f6f378311c8d 153 int _previous_state; // record previous state
tnhnrl 73:f6f378311c8d 154 int _sub_state; // substate on find_neutral function
tnhnrl 73:f6f378311c8d 155 int _previous_sub_state; // previous substate so that what goes into the sub-state is not being changed as it is processed
tnhnrl 73:f6f378311c8d 156 float _neutral_timer; // keep time for rise/sink/level timer incremnets
joel_ssc 87:6d95f853dab3 157 int _neutral_success;
tnhnrl 73:f6f378311c8d 158 bool _isTimeoutRunning;
tnhnrl 73:f6f378311c8d 159
tnhnrl 73:f6f378311c8d 160 bool _isSubStateTimerRunning;
tnhnrl 73:f6f378311c8d 161
tnhnrl 73:f6f378311c8d 162 float _neutral_bce_pos_mm;
tnhnrl 73:f6f378311c8d 163 float _neutral_batt_pos_mm;
tnhnrl 73:f6f378311c8d 164
tnhnrl 73:f6f378311c8d 165 int _multi_dive_counter;
joel_ssc 82:0981b9ada820 166 int _multi_leg_counter;
tnhnrl 73:f6f378311c8d 167
tnhnrl 73:f6f378311c8d 168 currentSequenceStruct currentStateStruct; //type_of_struct struct_name
joel_ssc 82:0981b9ada820 169 currentLegStruct currentLegStateStruct; //type_of_struct struct_name
tnhnrl 73:f6f378311c8d 170
tnhnrl 73:f6f378311c8d 171 float _depth_KP;
tnhnrl 73:f6f378311c8d 172 float _depth_KI;
tnhnrl 73:f6f378311c8d 173 float _depth_KD;
tnhnrl 73:f6f378311c8d 174
tnhnrl 73:f6f378311c8d 175 float _pitch_KP;
tnhnrl 73:f6f378311c8d 176 float _pitch_KI;
tnhnrl 73:f6f378311c8d 177 float _pitch_KD;
tnhnrl 73:f6f378311c8d 178
tnhnrl 73:f6f378311c8d 179 int _state_array[256]; //used to print out the states
tnhnrl 73:f6f378311c8d 180 int _state_array_counter; //used to iterate through state records
tnhnrl 73:f6f378311c8d 181 int _substate_array[256]; //used to print out the sub-states
tnhnrl 73:f6f378311c8d 182 int _substate_array_counter; //used to iterate through sub-state records
tnhnrl 73:f6f378311c8d 183
tnhnrl 73:f6f378311c8d 184 int _substate;
tnhnrl 73:f6f378311c8d 185 int _previous_substate;
tnhnrl 73:f6f378311c8d 186
tnhnrl 73:f6f378311c8d 187 float _max_recorded_depth_neutral;
tnhnrl 73:f6f378311c8d 188 float _max_recorded_depth_dive;
tnhnrl 73:f6f378311c8d 189
tnhnrl 73:f6f378311c8d 190 float _neutral_sink_command_mm; //defaults for neutral finding sub-FSM
tnhnrl 73:f6f378311c8d 191 float _neutral_rise_command_mm;
tnhnrl 73:f6f378311c8d 192 float _neutral_pitch_command_mm;
tnhnrl 73:f6f378311c8d 193
tnhnrl 73:f6f378311c8d 194 float _max_recorded_auto_neutral_depth;
tnhnrl 73:f6f378311c8d 195
tnhnrl 73:f6f378311c8d 196 bool _is_log_timer_running;
tnhnrl 73:f6f378311c8d 197 float _log_timer;
tnhnrl 73:f6f378311c8d 198
tnhnrl 73:f6f378311c8d 199 float _BCE_dive_offset; // NEW COMMANDS FOR POSITION CONTROLLER
tnhnrl 73:f6f378311c8d 200 float _BMM_dive_offset;
tnhnrl 73:f6f378311c8d 201
tnhnrl 73:f6f378311c8d 202 float getFloatUserInput();
tnhnrl 73:f6f378311c8d 203
tnhnrl 73:f6f378311c8d 204 //new
tnhnrl 73:f6f378311c8d 205 float _batt_filter_freq;
tnhnrl 73:f6f378311c8d 206 float _bce_filter_freq;
tnhnrl 73:f6f378311c8d 207 float _pitch_filter_freq;
tnhnrl 73:f6f378311c8d 208 float _depth_filter_freq;
tnhnrl 73:f6f378311c8d 209 float _heading_filter_freq;
tnhnrl 73:f6f378311c8d 210
tnhnrl 73:f6f378311c8d 211 float _batt_deadband;
tnhnrl 73:f6f378311c8d 212 float _bce_deadband;
tnhnrl 73:f6f378311c8d 213 float _pitch_deadband;
tnhnrl 73:f6f378311c8d 214 float _depth_deadband;
tnhnrl 73:f6f378311c8d 215 float _heading_deadband;
tnhnrl 73:f6f378311c8d 216 };
tnhnrl 73:f6f378311c8d 217
tnhnrl 73:f6f378311c8d 218 #endif