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@36:966a86937e17, 2017-12-20 (annotated)

Committer:
tnhnrl
Date:
Wed Dec 20 23:21:15 2017 +0000
Revision:
36:966a86937e17
Parent:
34:9b66c5188051
Child:
38:83d06c294807
Working version for pool testing

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 32:f2f8ae34aadc 6 #include "DirectoryList.h"
tnhnrl 20:8987a9ae2bc7 7
tnhnrl 16:3363b9f14913 8 extern "C" void mbed_reset(); // utilized to reset the mbed
tnhnrl 20:8987a9ae2bc7 9
tnhnrl 20:8987a9ae2bc7 10 // main finite state enumerations
tnhnrl 16:3363b9f14913 11 enum {
tnhnrl 17:7c16b5671d0e 12 SIT_IDLE, // stops both motors, exits after a keyboard input
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 17:7c16b5671d0e 16 FLOAT_LEVEL, // bce position to float, pitch loop active at zero, exits when stable near zero pitch
tnhnrl 17:7c16b5671d0e 17 FLOAT_BROADCAST, // bce position to float, batt position forward to hold tail up, exits when actuators done
tnhnrl 17:7c16b5671d0e 18 EMERGENCY_CLIMB, // bce position to full rise, batt position to full aft, exits when at surface
danstrider 22:a10ee088403b 19 MULTI_DIVE, // multi-dive sequence
tnhnrl 28:16c83a2fdefa 20 MULTI_RISE, // multi-rise sequence
tnhnrl 32:f2f8ae34aadc 21 KEYBOARD, // "state" for tracking only
tnhnrl 32:f2f8ae34aadc 22 FIND_AUTO_NEUTRAL_DEPTH,
tnhnrl 34:9b66c5188051 23 FIND_AUTO_NEUTRAL_PITCH,
tnhnrl 34:9b66c5188051 24 TRANSMIT_DATA
tnhnrl 20:8987a9ae2bc7 25 };
tnhnrl 20:8987a9ae2bc7 26
tnhnrl 20:8987a9ae2bc7 27 // find_neutral finite state machine enumerations
tnhnrl 20:8987a9ae2bc7 28 enum {
danstrider 22:a10ee088403b 29 NEUTRAL_SINKING, // increment the bce until really start sinking
danstrider 22:a10ee088403b 30 NEUTRAL_SLOWLY_RISE, // once sinking, arrest the sink
danstrider 22:a10ee088403b 31 NEUTRAL_CHECK_PITCH, // find level again, then save the data and exit
tnhnrl 32:f2f8ae34aadc 32 NEUTRAL_EXIT, // sub-FSM has completed all checks
tnhnrl 17:7c16b5671d0e 33 };
tnhnrl 32:f2f8ae34aadc 34
tnhnrl 32:f2f8ae34aadc 35 // test idea
tnhnrl 20:8987a9ae2bc7 36
tnhnrl 17:7c16b5671d0e 37 //struct for saving the data
tnhnrl 17:7c16b5671d0e 38 struct currentSequenceStruct {
tnhnrl 17:7c16b5671d0e 39 int state; //for the current StateMachine, states are ID-ed with enumeration
tnhnrl 17:7c16b5671d0e 40 float timeout;
tnhnrl 17:7c16b5671d0e 41 float depth;
tnhnrl 17:7c16b5671d0e 42 float pitch;
tnhnrl 16:3363b9f14913 43 };
tnhnrl 20:8987a9ae2bc7 44
tnhnrl 16:3363b9f14913 45 class StateMachine {
tnhnrl 16:3363b9f14913 46 public:
tnhnrl 16:3363b9f14913 47 StateMachine();
tnhnrl 16:3363b9f14913 48
tnhnrl 34:9b66c5188051 49 int runStateMachine();
tnhnrl 16:3363b9f14913 50
tnhnrl 16:3363b9f14913 51 void showMenu();
tnhnrl 16:3363b9f14913 52
tnhnrl 17:7c16b5671d0e 53 void keyboard();
tnhnrl 16:3363b9f14913 54
tnhnrl 16:3363b9f14913 55 void keyboard_menu_BCE_PID_settings();
tnhnrl 16:3363b9f14913 56 void keyboard_menu_BATT_PID_settings();
tnhnrl 16:3363b9f14913 57 void keyboard_menu_DEPTH_PID_settings();
tnhnrl 16:3363b9f14913 58 void keyboard_menu_PITCH_PID_settings();
tnhnrl 16:3363b9f14913 59
tnhnrl 16:3363b9f14913 60 float getDepthCommand();
tnhnrl 16:3363b9f14913 61 float getPitchCommand();
tnhnrl 32:f2f8ae34aadc 62 float getDepthReading();
tnhnrl 32:f2f8ae34aadc 63 float getPitchReading();
tnhnrl 32:f2f8ae34aadc 64 float getTimerReading();
tnhnrl 16:3363b9f14913 65
tnhnrl 32:f2f8ae34aadc 66 int runNeutralStateMachine(); //substate returns the state (which is used in overall FSM)
tnhnrl 17:7c16b5671d0e 67
tnhnrl 28:16c83a2fdefa 68 int getState();
tnhnrl 17:7c16b5671d0e 69 void setState(int input_state);
tnhnrl 28:16c83a2fdefa 70
tnhnrl 17:7c16b5671d0e 71 void setTimeout(float input_timeout);
tnhnrl 17:7c16b5671d0e 72 void setDepthCommand(float input_depth_command);
tnhnrl 17:7c16b5671d0e 73 void setPitchCommand(float input_pitch_command);
tnhnrl 17:7c16b5671d0e 74
tnhnrl 17:7c16b5671d0e 75 void setNeutralPositions(float batt_pos_mm, float bce_pos_mm);
tnhnrl 17:7c16b5671d0e 76
tnhnrl 17:7c16b5671d0e 77 int timeoutRunning();
tnhnrl 17:7c16b5671d0e 78
tnhnrl 21:38c8544db6f4 79 void getDiveSequence(); //used in multi-dive sequence with public variables for now
tnhnrl 17:7c16b5671d0e 80
tnhnrl 32:f2f8ae34aadc 81 void runActiveNeutralStateMachine(); //new neutral substate returns the state (which is used in overall FSM)
tnhnrl 32:f2f8ae34aadc 82
tnhnrl 34:9b66c5188051 83 float * getLoggerArray(); //delete soon
tnhnrl 32:f2f8ae34aadc 84
tnhnrl 34:9b66c5188051 85 void recordData(int input_state); //record log data to file using MbedLogger class
tnhnrl 32:f2f8ae34aadc 86 void printDirectory();
tnhnrl 32:f2f8ae34aadc 87 void recordState(int input_state);
tnhnrl 34:9b66c5188051 88 void printCurrentSdLog(); //more tricky for SD card, work in progress
tnhnrl 32:f2f8ae34aadc 89
tnhnrl 32:f2f8ae34aadc 90 void createNewFile();
tnhnrl 32:f2f8ae34aadc 91
tnhnrl 32:f2f8ae34aadc 92 void transmitData();
tnhnrl 32:f2f8ae34aadc 93
tnhnrl 36:966a86937e17 94 float * dataArray();
tnhnrl 36:966a86937e17 95
tnhnrl 16:3363b9f14913 96 private:
tnhnrl 17:7c16b5671d0e 97 int _timeout; // generic timeout for every state, seconds
tnhnrl 28:16c83a2fdefa 98 float _pitchTolerance; // pitch angle tolerance for neutral finding exit criteria
tnhnrl 28:16c83a2fdefa 99 float _bceFloatPosition; // bce position for "float" states
tnhnrl 28:16c83a2fdefa 100 float _battFloatPosition; // batt position for "broadcast" state
tnhnrl 16:3363b9f14913 101
tnhnrl 32:f2f8ae34aadc 102 float _depth_command; // user keyboard depth
tnhnrl 32:f2f8ae34aadc 103 float _pitch_command; // user keyboard depth
tnhnrl 32:f2f8ae34aadc 104 float _depth_reading; // depth reading (to get the readings at the same time)
tnhnrl 32:f2f8ae34aadc 105 float _pitch_reading; // pitch reading (to get the readings at the same time)
tnhnrl 32:f2f8ae34aadc 106 float _timer_reading; // pitch reading (to get the readings at the same time)
tnhnrl 16:3363b9f14913 107
tnhnrl 16:3363b9f14913 108 Timer timer;
tnhnrl 17:7c16b5671d0e 109
danstrider 22:a10ee088403b 110 int _state; // Fine State Machine (FSM) state
tnhnrl 28:16c83a2fdefa 111 int _previous_state; // record previous state
danstrider 22:a10ee088403b 112 int _sub_state; // substate on find_neutral function
danstrider 22:a10ee088403b 113 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 114 float _neutral_timer; // keep time for rise/sink/level timer incremnets
tnhnrl 17:7c16b5671d0e 115
tnhnrl 28:16c83a2fdefa 116 bool _isTimeoutRunning;
tnhnrl 17:7c16b5671d0e 117
tnhnrl 28:16c83a2fdefa 118 bool _isSubStateTimerRunning;
tnhnrl 17:7c16b5671d0e 119
tnhnrl 21:38c8544db6f4 120 float _neutral_bce_pos_mm;
tnhnrl 21:38c8544db6f4 121 float _neutral_batt_pos_mm;
tnhnrl 17:7c16b5671d0e 122
tnhnrl 24:c7d9b5bf3829 123 int _multi_dive_counter;
tnhnrl 28:16c83a2fdefa 124
tnhnrl 21:38c8544db6f4 125 currentSequenceStruct currentStateStruct; //type_of_struct struct_name
tnhnrl 21:38c8544db6f4 126
tnhnrl 21:38c8544db6f4 127 bool _neutral_sub_state_active; // controls neutral_sub_state
tnhnrl 21:38c8544db6f4 128
tnhnrl 21:38c8544db6f4 129 float _depth_KP;
tnhnrl 21:38c8544db6f4 130 float _depth_KI;
tnhnrl 21:38c8544db6f4 131 float _depth_KD;
tnhnrl 21:38c8544db6f4 132
tnhnrl 21:38c8544db6f4 133 float _pitch_KP;
tnhnrl 21:38c8544db6f4 134 float _pitch_KI;
tnhnrl 21:38c8544db6f4 135 float _pitch_KD;
tnhnrl 28:16c83a2fdefa 136
tnhnrl 28:16c83a2fdefa 137 int _state_array[1024]; //used to print out the states
tnhnrl 28:16c83a2fdefa 138 int _state_array_counter; //used to iterate through state records
tnhnrl 28:16c83a2fdefa 139 int _substate_array[1024]; //used to print out the sub-states
tnhnrl 28:16c83a2fdefa 140 int _substate_array_counter; //used to iterate through sub-state records
tnhnrl 23:434f04ef1fad 141
tnhnrl 24:c7d9b5bf3829 142 int _substate;
tnhnrl 24:c7d9b5bf3829 143 int _previous_substate;
tnhnrl 28:16c83a2fdefa 144
tnhnrl 28:16c83a2fdefa 145 float _max_recorded_depth_neutral;
tnhnrl 28:16c83a2fdefa 146 float _max_recorded_depth_dive;
tnhnrl 32:f2f8ae34aadc 147
tnhnrl 32:f2f8ae34aadc 148 float _neutral_sink_command_mm; //defaults for neutral finding sub-FSM
tnhnrl 32:f2f8ae34aadc 149 float _neutral_rise_command_mm;
tnhnrl 32:f2f8ae34aadc 150 float _neutral_pitch_command_mm;
tnhnrl 32:f2f8ae34aadc 151
tnhnrl 32:f2f8ae34aadc 152 float _max_recorded_auto_neutral_depth;
tnhnrl 32:f2f8ae34aadc 153
tnhnrl 32:f2f8ae34aadc 154 //float _logger_array[1048][5]; //array to hold the input, one log per dive (row/col)
tnhnrl 32:f2f8ae34aadc 155 bool _is_log_timer_running;
tnhnrl 32:f2f8ae34aadc 156 float _log_timer;
tnhnrl 32:f2f8ae34aadc 157
tnhnrl 32:f2f8ae34aadc 158 float _data_log[9];
tnhnrl 32:f2f8ae34aadc 159
tnhnrl 32:f2f8ae34aadc 160 volatile bool _file_closed;
tnhnrl 16:3363b9f14913 161 };
tnhnrl 20:8987a9ae2bc7 162
tnhnrl 16:3363b9f14913 163 #endif