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

SequenceController/SequenceController.cpp@45:16b8162188ca, 2018-02-15 (annotated)

Committer:
tnhnrl
Date:
Thu Feb 15 02:39:13 2018 +0000
Revision:
45:16b8162188ca
Parent:
30:2964617e7676
Child:
73:f6f378311c8d
version for USB testing with reduced class sizes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tnhnrl 17:7c16b5671d0e 1 #include "SequenceController.hpp"
tnhnrl 17:7c16b5671d0e 2 #include "StaticDefs.hpp"
tnhnrl 17:7c16b5671d0e 3
tnhnrl 17:7c16b5671d0e 4 SequenceController::SequenceController() {
tnhnrl 17:7c16b5671d0e 5 pc().baud(57600); //text was garbled before setting this
tnhnrl 17:7c16b5671d0e 6
tnhnrl 17:7c16b5671d0e 7 _sequence_counter = 0;
tnhnrl 17:7c16b5671d0e 8 }
tnhnrl 17:7c16b5671d0e 9
tnhnrl 17:7c16b5671d0e 10 void SequenceController::loadSequence() {
tnhnrl 21:38c8544db6f4 11 pc().printf("\n\rLoading Dive Sequence File:");
tnhnrl 17:7c16b5671d0e 12
tnhnrl 17:7c16b5671d0e 13 ConfigFile read_sequence_cfg;
tnhnrl 17:7c16b5671d0e 14 char value[256];
tnhnrl 17:7c16b5671d0e 15
tnhnrl 17:7c16b5671d0e 16 //read configuration file stored on MBED
tnhnrl 45:16b8162188ca 17 if (!read_sequence_cfg.read("/local/sequence.txt")) {
tnhnrl 45:16b8162188ca 18 pc().printf("\n\rERROR:Failure to read sequence.txt file.");
tnhnrl 17:7c16b5671d0e 19 }
tnhnrl 45:16b8162188ca 20 else {
tnhnrl 17:7c16b5671d0e 21 /* Read values from the file until you reach an "exit" character" */
tnhnrl 45:16b8162188ca 22 //up to 256 items in the sequence
tnhnrl 45:16b8162188ca 23 for (int i = 0; i < 256; i++) { //works
tnhnrl 17:7c16b5671d0e 24 /* convert INT to string */
tnhnrl 17:7c16b5671d0e 25 char buf[256];
tnhnrl 17:7c16b5671d0e 26 sprintf(buf, "%d", i); //searching for 0,1,2,3...
tnhnrl 17:7c16b5671d0e 27 /* convert INT to string */
tnhnrl 17:7c16b5671d0e 28
tnhnrl 17:7c16b5671d0e 29 if (read_sequence_cfg.getValue(buf, &value[0], sizeof(value))) {
tnhnrl 17:7c16b5671d0e 30 pc().printf("\n\rsequence %d = %s",i,value);
tnhnrl 17:7c16b5671d0e 31
tnhnrl 17:7c16b5671d0e 32 sequenceStructLoaded[i] = process(value); //create the structs using process(string randomstring)
tnhnrl 17:7c16b5671d0e 33 }
tnhnrl 17:7c16b5671d0e 34
tnhnrl 17:7c16b5671d0e 35 if (sequenceStructLoaded[i].title == "exit") {
tnhnrl 17:7c16b5671d0e 36 _number_of_sequences = i; //before the exit
tnhnrl 17:7c16b5671d0e 37 break;
tnhnrl 17:7c16b5671d0e 38 }
tnhnrl 17:7c16b5671d0e 39 }
tnhnrl 17:7c16b5671d0e 40 } //end of successful read
tnhnrl 17:7c16b5671d0e 41 }
tnhnrl 17:7c16b5671d0e 42
tnhnrl 17:7c16b5671d0e 43 sequenceStruct SequenceController::process(string randomstring) {
tnhnrl 17:7c16b5671d0e 44 //this is the struct that is loaded from the config variables
tnhnrl 17:7c16b5671d0e 45
tnhnrl 21:38c8544db6f4 46 sequenceStruct loadStruct; //local struct
tnhnrl 17:7c16b5671d0e 47
tnhnrl 17:7c16b5671d0e 48 /* CONVERT STRING TO CHAR ARRAY */
tnhnrl 17:7c16b5671d0e 49 const char *cstr = randomstring.c_str();
tnhnrl 17:7c16b5671d0e 50 /* CONVERT STRING TO CHAR ARRAY */
tnhnrl 17:7c16b5671d0e 51
tnhnrl 17:7c16b5671d0e 52 /* DIVE */
tnhnrl 45:16b8162188ca 53 //this can only be in the first position
tnhnrl 45:16b8162188ca 54 if ((signed int) randomstring.find("dive") != -1) {
tnhnrl 17:7c16b5671d0e 55 loadStruct.title = "dive";
tnhnrl 17:7c16b5671d0e 56 loadStruct.state = MULTI_DIVE; //NEW: separate state handles multiple dives
tnhnrl 17:7c16b5671d0e 57 }
tnhnrl 17:7c16b5671d0e 58 /* DIVE */
tnhnrl 17:7c16b5671d0e 59
tnhnrl 17:7c16b5671d0e 60 /* PITCH */
tnhnrl 45:16b8162188ca 61 if ((signed int) randomstring.find("neutral") != -1) {
tnhnrl 17:7c16b5671d0e 62 loadStruct.title = "neutral";
tnhnrl 45:16b8162188ca 63 pc().printf("\n\rLOAD neutral. %d", randomstring.find("neutral"));
tnhnrl 17:7c16b5671d0e 64 loadStruct.state = FIND_NEUTRAL;
tnhnrl 17:7c16b5671d0e 65 }
tnhnrl 17:7c16b5671d0e 66 /* PITCH */
tnhnrl 17:7c16b5671d0e 67
tnhnrl 17:7c16b5671d0e 68 /* EXIT */
tnhnrl 45:16b8162188ca 69 if ((signed int) randomstring.find("exit") != -1) {
tnhnrl 17:7c16b5671d0e 70 loadStruct.title = "exit";
tnhnrl 45:16b8162188ca 71 pc().printf("\n\rReminder. Exit command is state FLOAT_BROADCAST\n\r");
tnhnrl 30:2964617e7676 72 loadStruct.state = FLOAT_BROADCAST; //this is the new exit condition of the dive-rise sequence (11/4/17)
tnhnrl 17:7c16b5671d0e 73 }
tnhnrl 17:7c16b5671d0e 74 /* EXIT */
tnhnrl 17:7c16b5671d0e 75
tnhnrl 17:7c16b5671d0e 76 /* DEPTH TO FLOAT */
tnhnrl 45:16b8162188ca 77 if ((signed int) randomstring.find("depth") != -1) {
tnhnrl 17:7c16b5671d0e 78 if (randomstring.find("neutral") || randomstring.find("dive")) {
tnhnrl 17:7c16b5671d0e 79 int depth_pos = randomstring.find("depth") + 6; //11 in example literally "depth="
tnhnrl 17:7c16b5671d0e 80 char depth_array[256] = {0}; //clear memory
tnhnrl 17:7c16b5671d0e 81 int depth_counter = 0;
tnhnrl 17:7c16b5671d0e 82 for (int i = depth_pos; i < randomstring.length(); i++) {
tnhnrl 17:7c16b5671d0e 83 if (cstr[i] == ',')
tnhnrl 17:7c16b5671d0e 84 break;
tnhnrl 17:7c16b5671d0e 85 else if (cstr[i] == ';')
tnhnrl 17:7c16b5671d0e 86 break;
tnhnrl 17:7c16b5671d0e 87 else {
tnhnrl 17:7c16b5671d0e 88 depth_array[depth_counter] = cstr[i];
tnhnrl 17:7c16b5671d0e 89 depth_counter++;
tnhnrl 17:7c16b5671d0e 90 }
tnhnrl 17:7c16b5671d0e 91 }
tnhnrl 17:7c16b5671d0e 92 loadStruct.depth = atof(depth_array);
tnhnrl 17:7c16b5671d0e 93 }
tnhnrl 17:7c16b5671d0e 94 }
tnhnrl 17:7c16b5671d0e 95 /* DEPTH TO FLOAT */
tnhnrl 17:7c16b5671d0e 96
tnhnrl 17:7c16b5671d0e 97 /* PITCH TO FLOAT */
tnhnrl 45:16b8162188ca 98 if ((signed int) randomstring.find("pitch") != -1) {
tnhnrl 17:7c16b5671d0e 99 if (randomstring.find("neutral") || randomstring.find("dive")) {
tnhnrl 17:7c16b5671d0e 100 int pitch_pos = randomstring.find("pitch") + 6; //11 in example
tnhnrl 17:7c16b5671d0e 101 char pitch_array[256] = {0}; //clear memory
tnhnrl 17:7c16b5671d0e 102 int pitch_counter = 0;
tnhnrl 17:7c16b5671d0e 103 for (int i = pitch_pos; i < randomstring.length(); i++) {
tnhnrl 17:7c16b5671d0e 104 if (cstr[i] == ',')
tnhnrl 17:7c16b5671d0e 105 break;
tnhnrl 17:7c16b5671d0e 106 else if (cstr[i] == ';')
tnhnrl 17:7c16b5671d0e 107 break;
tnhnrl 17:7c16b5671d0e 108 else {
tnhnrl 17:7c16b5671d0e 109 pitch_array[pitch_counter] = cstr[i];
tnhnrl 17:7c16b5671d0e 110 pitch_counter++;
tnhnrl 17:7c16b5671d0e 111 }
tnhnrl 17:7c16b5671d0e 112 }
tnhnrl 17:7c16b5671d0e 113 loadStruct.pitch = atof(pitch_array);
tnhnrl 17:7c16b5671d0e 114 }
tnhnrl 17:7c16b5671d0e 115 }
tnhnrl 17:7c16b5671d0e 116 /* PITCH TO FLOAT */
tnhnrl 17:7c16b5671d0e 117
tnhnrl 17:7c16b5671d0e 118 /* PAUSE */
tnhnrl 45:16b8162188ca 119 if ((signed int) randomstring.find("pause") != -1) {
tnhnrl 17:7c16b5671d0e 120 loadStruct.title = "pause";
tnhnrl 17:7c16b5671d0e 121 }
tnhnrl 17:7c16b5671d0e 122 /* PAUSE */
tnhnrl 17:7c16b5671d0e 123
tnhnrl 17:7c16b5671d0e 124 /* TIME TO FLOAT */
tnhnrl 45:16b8162188ca 125 if ((signed int) randomstring.find("timeout") != -1) {
tnhnrl 45:16b8162188ca 126
tnhnrl 17:7c16b5671d0e 127 int time_pos = randomstring.find("timeout") + 8; //position of timeout + "timeout=" so 8
tnhnrl 17:7c16b5671d0e 128 char time_array[256] = {0};
tnhnrl 17:7c16b5671d0e 129 int time_counter = 0;
tnhnrl 17:7c16b5671d0e 130 for (int i = time_pos; i < randomstring.length(); i++) {
tnhnrl 17:7c16b5671d0e 131 //pc().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
tnhnrl 17:7c16b5671d0e 132
tnhnrl 17:7c16b5671d0e 133 if (cstr[i] == ',')
tnhnrl 17:7c16b5671d0e 134 break;
tnhnrl 17:7c16b5671d0e 135 else if (cstr[i] == ';')
tnhnrl 17:7c16b5671d0e 136 break;
tnhnrl 17:7c16b5671d0e 137 else {
tnhnrl 17:7c16b5671d0e 138 //pc().printf("time string cstr[i] = %c\n\r", cstr[i]); //debug
tnhnrl 17:7c16b5671d0e 139 time_array[time_counter] = cstr[i];
tnhnrl 17:7c16b5671d0e 140 time_counter++;
tnhnrl 17:7c16b5671d0e 141 }
tnhnrl 17:7c16b5671d0e 142 }
tnhnrl 17:7c16b5671d0e 143 loadStruct.timeout = atof(time_array);
tnhnrl 17:7c16b5671d0e 144 }
tnhnrl 17:7c16b5671d0e 145 /* TIME TO FLOAT */
tnhnrl 17:7c16b5671d0e 146
tnhnrl 45:16b8162188ca 147 // /* EXIT */
tnhnrl 45:16b8162188ca 148 // if (randomstring.find("exit") != 0) {
tnhnrl 45:16b8162188ca 149 // loadStruct.title = "exit";
tnhnrl 45:16b8162188ca 150 // pc().printf("\n\rEXIT.");
tnhnrl 45:16b8162188ca 151 // }
tnhnrl 45:16b8162188ca 152 // /* EXIT */
tnhnrl 17:7c16b5671d0e 153
tnhnrl 17:7c16b5671d0e 154 return loadStruct; //each iteration this returns a completed struct
tnhnrl 17:7c16b5671d0e 155 }
tnhnrl 17:7c16b5671d0e 156
tnhnrl 17:7c16b5671d0e 157 void SequenceController::sequenceFunction() {
tnhnrl 17:7c16b5671d0e 158 //pc().printf("sequenceFunction\n\r"); //debug (verified it is working correctly)
tnhnrl 17:7c16b5671d0e 159
tnhnrl 17:7c16b5671d0e 160 int check_current_state = stateMachine().getState();
tnhnrl 17:7c16b5671d0e 161 pc().printf("State Machine State: %d\n\r", check_current_state);
tnhnrl 17:7c16b5671d0e 162 pc().printf("State Machine: isTimeoutRunning? %d\n\r", stateMachine().timeoutRunning());
tnhnrl 17:7c16b5671d0e 163
tnhnrl 17:7c16b5671d0e 164 if (stateMachine().getState() == SIT_IDLE) {
tnhnrl 17:7c16b5671d0e 165 //system starts idle
tnhnrl 17:7c16b5671d0e 166 //set the state machine to the current sequence in the array
tnhnrl 17:7c16b5671d0e 167 //example, set to "dive" and set pitch and depth and timeout
tnhnrl 17:7c16b5671d0e 168
tnhnrl 17:7c16b5671d0e 169 _current_state = sequenceStructLoaded[_sequence_counter].state;
tnhnrl 17:7c16b5671d0e 170 pc().printf("_current_state: %d\n\r", _current_state);
tnhnrl 17:7c16b5671d0e 171 pc().printf("_sequence_counter: %d\n\r", _sequence_counter);
tnhnrl 17:7c16b5671d0e 172 pc().printf("_number_of_sequences: %d\n\r", _number_of_sequences);
tnhnrl 17:7c16b5671d0e 173
tnhnrl 17:7c16b5671d0e 174 stateMachine().setState(_current_state);
tnhnrl 17:7c16b5671d0e 175 stateMachine().setDepthCommand(sequenceStructLoaded[_sequence_counter].depth);
tnhnrl 17:7c16b5671d0e 176 stateMachine().setPitchCommand(sequenceStructLoaded[_sequence_counter].pitch);
tnhnrl 17:7c16b5671d0e 177 stateMachine().setTimeout(sequenceStructLoaded[_sequence_counter].timeout);
tnhnrl 17:7c16b5671d0e 178
tnhnrl 17:7c16b5671d0e 179 if (_sequence_counter == _number_of_sequences-1) //end when you finish all of the sequences
tnhnrl 17:7c16b5671d0e 180 sequenceTicker.detach();
tnhnrl 17:7c16b5671d0e 181
tnhnrl 17:7c16b5671d0e 182 _sequence_counter++; //exit ticker when counter complete
tnhnrl 17:7c16b5671d0e 183 }
tnhnrl 17:7c16b5671d0e 184 }
tnhnrl 17:7c16b5671d0e 185
tnhnrl 17:7c16b5671d0e 186 int SequenceController::getSequenceState() {
tnhnrl 17:7c16b5671d0e 187 return _current_state;
tnhnrl 17:7c16b5671d0e 188 }