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@28:16c83a2fdefa, 2017-12-01 (annotated)

Committer:
tnhnrl
Date:
Fri Dec 01 22:43:14 2017 +0000
Revision:
28:16c83a2fdefa
Parent:
27:0a5b90cd65d6
Child:
32:f2f8ae34aadc
Pool tested code from Friday 12/1/17

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 20:8987a9ae2bc7 5
tnhnrl 16:3363b9f14913 6 extern "C" void mbed_reset(); // utilized to reset the mbed
tnhnrl 20:8987a9ae2bc7 7
tnhnrl 20:8987a9ae2bc7 8 // main finite 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 FIND_NEUTRAL, // dives to depth at zero pitch, exits when stable
tnhnrl 17:7c16b5671d0e 12 DIVE, // dives to depth at negative pitch, exits when crossing a defined depth
tnhnrl 17:7c16b5671d0e 13 RISE, // rises to surface at positive pitch, exits when near surface
tnhnrl 17:7c16b5671d0e 14 FLOAT_LEVEL, // bce position to float, pitch loop active at zero, exits when stable near zero pitch
tnhnrl 17:7c16b5671d0e 15 FLOAT_BROADCAST, // bce position to float, batt position forward to hold tail up, exits when actuators done
tnhnrl 17:7c16b5671d0e 16 EMERGENCY_CLIMB, // bce position to full rise, batt position to full aft, exits when at surface
danstrider 22:a10ee088403b 17 MULTI_DIVE, // multi-dive sequence
tnhnrl 28:16c83a2fdefa 18 MULTI_RISE, // multi-rise sequence
tnhnrl 28:16c83a2fdefa 19 KEYBOARD // "state" for tracking only
tnhnrl 20:8987a9ae2bc7 20 };
tnhnrl 20:8987a9ae2bc7 21
tnhnrl 20:8987a9ae2bc7 22 // find_neutral finite state machine enumerations
tnhnrl 20:8987a9ae2bc7 23 enum {
danstrider 22:a10ee088403b 24 NEUTRAL_FIRST_PITCH, // NEW first state: find level by moving the battery
danstrider 22:a10ee088403b 25 NEUTRAL_SINKING, // increment the bce until really start sinking
danstrider 22:a10ee088403b 26 NEUTRAL_SLOWLY_RISE, // once sinking, arrest the sink
danstrider 22:a10ee088403b 27 NEUTRAL_CHECK_PITCH, // find level again, then save the data and exit
danstrider 22:a10ee088403b 28 NEUTRAL_EXIT // sub-FSM has completed all checks
tnhnrl 17:7c16b5671d0e 29 };
tnhnrl 20:8987a9ae2bc7 30
tnhnrl 17:7c16b5671d0e 31 //struct for saving the data
tnhnrl 17:7c16b5671d0e 32 struct currentSequenceStruct {
tnhnrl 17:7c16b5671d0e 33 int state; //for the current StateMachine, states are ID-ed with enumeration
tnhnrl 17:7c16b5671d0e 34 float timeout;
tnhnrl 17:7c16b5671d0e 35 float depth;
tnhnrl 17:7c16b5671d0e 36 float pitch;
tnhnrl 16:3363b9f14913 37 };
tnhnrl 20:8987a9ae2bc7 38
tnhnrl 16:3363b9f14913 39 class StateMachine {
tnhnrl 16:3363b9f14913 40 public:
tnhnrl 16:3363b9f14913 41 StateMachine();
tnhnrl 16:3363b9f14913 42
tnhnrl 16:3363b9f14913 43 void runStateMachine();
tnhnrl 16:3363b9f14913 44
tnhnrl 16:3363b9f14913 45 void showMenu();
tnhnrl 16:3363b9f14913 46
tnhnrl 17:7c16b5671d0e 47 void keyboard();
tnhnrl 16:3363b9f14913 48
tnhnrl 16:3363b9f14913 49 void keyboard_menu_BCE_PID_settings();
tnhnrl 16:3363b9f14913 50 void keyboard_menu_BATT_PID_settings();
tnhnrl 16:3363b9f14913 51 void keyboard_menu_DEPTH_PID_settings();
tnhnrl 16:3363b9f14913 52 void keyboard_menu_PITCH_PID_settings();
tnhnrl 16:3363b9f14913 53
tnhnrl 16:3363b9f14913 54 float getDepthCommand();
tnhnrl 16:3363b9f14913 55 float getPitchCommand();
tnhnrl 16:3363b9f14913 56
tnhnrl 21:38c8544db6f4 57 int runNeutralStateMachine(); //substate returns the state (which is used in overall FSM)
tnhnrl 17:7c16b5671d0e 58
tnhnrl 28:16c83a2fdefa 59 int getState();
tnhnrl 17:7c16b5671d0e 60 void setState(int input_state);
tnhnrl 28:16c83a2fdefa 61
tnhnrl 17:7c16b5671d0e 62 void setTimeout(float input_timeout);
tnhnrl 17:7c16b5671d0e 63 void setDepthCommand(float input_depth_command);
tnhnrl 17:7c16b5671d0e 64 void setPitchCommand(float input_pitch_command);
tnhnrl 17:7c16b5671d0e 65
tnhnrl 17:7c16b5671d0e 66 void setNeutralPositions(float batt_pos_mm, float bce_pos_mm);
tnhnrl 17:7c16b5671d0e 67
tnhnrl 17:7c16b5671d0e 68 int timeoutRunning();
tnhnrl 17:7c16b5671d0e 69
tnhnrl 21:38c8544db6f4 70 void getDiveSequence(); //used in multi-dive sequence with public variables for now
tnhnrl 17:7c16b5671d0e 71
tnhnrl 16:3363b9f14913 72 private:
tnhnrl 17:7c16b5671d0e 73 int _timeout; // generic timeout for every state, seconds
tnhnrl 28:16c83a2fdefa 74 float _pitchTolerance; // pitch angle tolerance for neutral finding exit criteria
tnhnrl 28:16c83a2fdefa 75 float _bceFloatPosition; // bce position for "float" states
tnhnrl 28:16c83a2fdefa 76 float _battFloatPosition; // batt position for "broadcast" state
tnhnrl 16:3363b9f14913 77
tnhnrl 28:16c83a2fdefa 78 float _depthCommand; // user keyboard depth
tnhnrl 28:16c83a2fdefa 79 float _pitchCommand; // user keyboard depth
tnhnrl 16:3363b9f14913 80
tnhnrl 16:3363b9f14913 81 Timer timer;
tnhnrl 17:7c16b5671d0e 82
danstrider 22:a10ee088403b 83 int _state; // Fine State Machine (FSM) state
tnhnrl 28:16c83a2fdefa 84 int _previous_state; // record previous state
danstrider 22:a10ee088403b 85 int _sub_state; // substate on find_neutral function
danstrider 22:a10ee088403b 86 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 87 float _neutral_timer; // keep time for rise/sink/level timer incremnets
tnhnrl 17:7c16b5671d0e 88
tnhnrl 28:16c83a2fdefa 89 bool _isTimeoutRunning;
tnhnrl 17:7c16b5671d0e 90
tnhnrl 28:16c83a2fdefa 91 bool _isSubStateTimerRunning;
tnhnrl 17:7c16b5671d0e 92
tnhnrl 21:38c8544db6f4 93 float _neutral_bce_pos_mm;
tnhnrl 21:38c8544db6f4 94 float _neutral_batt_pos_mm;
tnhnrl 17:7c16b5671d0e 95
tnhnrl 24:c7d9b5bf3829 96 int _multi_dive_counter;
tnhnrl 28:16c83a2fdefa 97
tnhnrl 21:38c8544db6f4 98 currentSequenceStruct currentStateStruct; //type_of_struct struct_name
tnhnrl 21:38c8544db6f4 99
tnhnrl 21:38c8544db6f4 100 bool _neutral_sub_state_active; // controls neutral_sub_state
tnhnrl 21:38c8544db6f4 101
tnhnrl 21:38c8544db6f4 102 float _depth_KP;
tnhnrl 21:38c8544db6f4 103 float _depth_KI;
tnhnrl 21:38c8544db6f4 104 float _depth_KD;
tnhnrl 21:38c8544db6f4 105
tnhnrl 21:38c8544db6f4 106 float _pitch_KP;
tnhnrl 21:38c8544db6f4 107 float _pitch_KI;
tnhnrl 21:38c8544db6f4 108 float _pitch_KD;
tnhnrl 28:16c83a2fdefa 109
tnhnrl 28:16c83a2fdefa 110 int _state_array[1024]; //used to print out the states
tnhnrl 28:16c83a2fdefa 111 int _state_array_counter; //used to iterate through state records
tnhnrl 28:16c83a2fdefa 112 int _substate_array[1024]; //used to print out the sub-states
tnhnrl 28:16c83a2fdefa 113 int _substate_array_counter; //used to iterate through sub-state records
tnhnrl 23:434f04ef1fad 114
tnhnrl 24:c7d9b5bf3829 115 int _substate;
tnhnrl 24:c7d9b5bf3829 116 int _previous_substate;
tnhnrl 28:16c83a2fdefa 117
tnhnrl 28:16c83a2fdefa 118 float _max_recorded_depth_neutral;
tnhnrl 28:16c83a2fdefa 119 float _max_recorded_depth_dive;
tnhnrl 16:3363b9f14913 120 };
tnhnrl 20:8987a9ae2bc7 121
tnhnrl 16:3363b9f14913 122 #endif