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

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