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@74:d281aaef9766, 2018-08-14 (annotated)

Committer:
tnhnrl
Date:
Tue Aug 14 21:06:48 2018 +0000
Revision:
74:d281aaef9766
Parent:
73:f6f378311c8d
Child:
82:0981b9ada820
1) Fixed deadband on float broadcast ; 2) Fixed find neutral commands

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