Lucas Citolin / Mbed OS Autoline

MBED experiment.

Autoline/Autoline.cpp@2:d1c24eae74d5, 2018-09-16 (annotated)

Committer:
citolin
Date:
Sun Sep 16 17:28:48 2018 +0000
Revision:
2:d1c24eae74d5
Parent:
1:cca3a4e419dd 
Added a mutex in the state memory space. Also added a example for power supply station.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
citolin 0:40a00c231fbd 1 #include "Autoline.h"
citolin 1:cca3a4e419dd 2 #include "ios.h"
citolin 0:40a00c231fbd 3
citolin 0:40a00c231fbd 4 char* allocate_vector(int size) {
citolin 0:40a00c231fbd 5 char *aux = (char*)malloc(sizeof(char)*size);
citolin 0:40a00c231fbd 6
citolin 0:40a00c231fbd 7 if(aux)
citolin 0:40a00c231fbd 8 memset(aux,'\0', sizeof(char)*size);
citolin 0:40a00c231fbd 9
citolin 0:40a00c231fbd 10 return aux;
citolin 0:40a00c231fbd 11 }
citolin 0:40a00c231fbd 12
citolin 0:40a00c231fbd 13 Autoline::Autoline() : pc(USBTX, USBRX) {
citolin 0:40a00c231fbd 14 // Register the parser callback
citolin 0:40a00c231fbd 15 connection.registerReadCallback(callback(this,&Autoline::parser));
citolin 1:cca3a4e419dd 16
citolin 1:cca3a4e419dd 17 // Starts on MAINTENANCE_MODE
citolin 1:cca3a4e419dd 18 state = MAINTENANCE_MODE;
citolin 0:40a00c231fbd 19 }
citolin 0:40a00c231fbd 20
citolin 0:40a00c231fbd 21 Autoline::~Autoline() { }
citolin 0:40a00c231fbd 22
citolin 2:d1c24eae74d5 23 // It needs a mutex because it's a variable controlled over two threads (ethernet thread and main thread)
citolin 2:d1c24eae74d5 24 void Autoline::set_state(short int newState) {
citolin 2:d1c24eae74d5 25 _mutex.lock();
citolin 2:d1c24eae74d5 26 state = newState;
citolin 2:d1c24eae74d5 27 _mutex.unlock();
citolin 2:d1c24eae74d5 28 }
citolin 2:d1c24eae74d5 29
citolin 0:40a00c231fbd 30 void Autoline::print_queue() {
citolin 0:40a00c231fbd 31 pc.printf("[AUTOLINE - QUEUE] Queue size: %d\n",queue.size());
citolin 0:40a00c231fbd 32 for(int i=0;i<queue.size();i++)
citolin 0:40a00c231fbd 33 pc.printf("[AUTOLINE - QUEUE] [%d] - %s\n",i, queue.at(i));
citolin 0:40a00c231fbd 34 }
citolin 0:40a00c231fbd 35
citolin 0:40a00c231fbd 36 void Autoline::protocoler(char *receivedFromEth, int size) {
citolin 0:40a00c231fbd 37 int j = 0;
citolin 0:40a00c231fbd 38 bool reading = false;
citolin 0:40a00c231fbd 39
citolin 0:40a00c231fbd 40 char local[MAX_SIZE_OF_CMMD];
citolin 0:40a00c231fbd 41 memset(local, '\0', sizeof(local));
citolin 0:40a00c231fbd 42
citolin 0:40a00c231fbd 43 for(int i=0;i<30;i++) {
citolin 0:40a00c231fbd 44
citolin 0:40a00c231fbd 45 // If it received the START of protocol: start to read
citolin 0:40a00c231fbd 46 if(receivedFromEth[i] == '{') {
citolin 0:40a00c231fbd 47 reading = true;
citolin 0:40a00c231fbd 48 continue;
citolin 0:40a00c231fbd 49 }
citolin 0:40a00c231fbd 50 // If it received the END of protocol:
citolin 0:40a00c231fbd 51 // Allocate a char* for the arrived data
citolin 0:40a00c231fbd 52 // Put everything in the queue and clear the buffer
citolin 0:40a00c231fbd 53 else if(reading && receivedFromEth[i] == '}') {
citolin 0:40a00c231fbd 54 j = 0;
citolin 0:40a00c231fbd 55 reading = false;
citolin 0:40a00c231fbd 56 char *aux = allocate_vector(MAX_SIZE_OF_CMMD);
citolin 0:40a00c231fbd 57
citolin 0:40a00c231fbd 58 for(int z=0;z<MAX_SIZE_OF_CMMD;z++)
citolin 0:40a00c231fbd 59 aux[z] = local[z];
citolin 0:40a00c231fbd 60
citolin 0:40a00c231fbd 61 queue.push_back(aux);
citolin 0:40a00c231fbd 62
citolin 0:40a00c231fbd 63 memset(local,'\0',sizeof(local));
citolin 0:40a00c231fbd 64 }
citolin 0:40a00c231fbd 65 // While reading
citolin 0:40a00c231fbd 66 else if(reading) {
citolin 0:40a00c231fbd 67 local[j] = receivedFromEth[i];
citolin 0:40a00c231fbd 68 j++;
citolin 0:40a00c231fbd 69 }
citolin 0:40a00c231fbd 70 }
citolin 0:40a00c231fbd 71 }
citolin 0:40a00c231fbd 72
citolin 0:40a00c231fbd 73 std::string Autoline::read_ios_in_range(int start, int end) {
citolin 0:40a00c231fbd 74 std::stringstream sstream;
citolin 0:40a00c231fbd 75 std::stringstream aux;
citolin 0:40a00c231fbd 76 std::string result;
citolin 0:40a00c231fbd 77
citolin 0:40a00c231fbd 78 unsigned long ulong_bits = 0;
citolin 0:40a00c231fbd 79
citolin 0:40a00c231fbd 80 // Charlean logic
citolin 0:40a00c231fbd 81 for(int i=end;i>=start;i--) {
citolin 0:40a00c231fbd 82 ulong_bits = dio.get_ulong(i);
citolin 0:40a00c231fbd 83 aux << std::hex << ulong_bits;
citolin 0:40a00c231fbd 84 result = aux.str();
citolin 0:40a00c231fbd 85 if(result.size()==1)
citolin 0:40a00c231fbd 86 result.push_back('0');
citolin 0:40a00c231fbd 87 sstream << result;
citolin 0:40a00c231fbd 88 }
citolin 0:40a00c231fbd 89
citolin 0:40a00c231fbd 90 return sstream.str();
citolin 0:40a00c231fbd 91 }
citolin 0:40a00c231fbd 92
citolin 0:40a00c231fbd 93 void Autoline::parser(char *cmmd, int size) {
citolin 0:40a00c231fbd 94 //pc.printf("[AUTOLINE - PARSER] - %s\n",cmmd);
citolin 0:40a00c231fbd 95
citolin 0:40a00c231fbd 96 protocoler(cmmd,size);
citolin 0:40a00c231fbd 97
citolin 0:40a00c231fbd 98 // Print queue
citolin 0:40a00c231fbd 99 //printQueue();
citolin 0:40a00c231fbd 100
citolin 0:40a00c231fbd 101 // If there is nothing in the queue, dont need to parse anything
citolin 0:40a00c231fbd 102 if(queue.size()==0)
citolin 0:40a00c231fbd 103 return;
citolin 0:40a00c231fbd 104
citolin 0:40a00c231fbd 105 // Parse command
citolin 0:40a00c231fbd 106 switch(queue.at(0)[0]) {
citolin 0:40a00c231fbd 107 case CMMD_1: {
citolin 0:40a00c231fbd 108 //pc.printf("CMMD 1\n");
citolin 0:40a00c231fbd 109
citolin 0:40a00c231fbd 110 // Read the input I/Os (bitset from 0 to 4)
citolin 0:40a00c231fbd 111 std::string cmmd1 = read_ios_in_range(0,4);
citolin 2:d1c24eae74d5 112 cmmd1 = "{1;" + cmmd1 + "}";
citolin 0:40a00c231fbd 113
citolin 0:40a00c231fbd 114 pc.printf("CMMD1 - %s\n", cmmd1);
citolin 2:d1c24eae74d5 115
citolin 2:d1c24eae74d5 116
citolin 2:d1c24eae74d5 117 // Send through the network
citolin 2:d1c24eae74d5 118 connection.send((char*)cmmd1.c_str(),cmmd1.size());
citolin 2:d1c24eae74d5 119
citolin 0:40a00c231fbd 120 break;
citolin 0:40a00c231fbd 121 }
citolin 0:40a00c231fbd 122
citolin 0:40a00c231fbd 123 case CMMD_2: {
citolin 0:40a00c231fbd 124 //pc.printf("CMMD 2\n");
citolin 0:40a00c231fbd 125 // { 2 XX Y } -- XX = ADDR of actuator || Y - Status of actuator (0 or 1).
citolin 0:40a00c231fbd 126
citolin 0:40a00c231fbd 127 unsigned int x = 0;
citolin 0:40a00c231fbd 128 bool status = (queue.at(0)[3] == '1')?true:false;
citolin 0:40a00c231fbd 129 char aux[3];
citolin 0:40a00c231fbd 130 memset(aux,'\0',sizeof(aux));
citolin 0:40a00c231fbd 131
citolin 0:40a00c231fbd 132 aux[0] = queue.at(0)[1];
citolin 0:40a00c231fbd 133 aux[1] = queue.at(0)[2];
citolin 0:40a00c231fbd 134
citolin 0:40a00c231fbd 135 std::stringstream ss;
citolin 0:40a00c231fbd 136 ss << std::hex << aux;
citolin 0:40a00c231fbd 137 ss >> x;
citolin 0:40a00c231fbd 138
citolin 0:40a00c231fbd 139 pc.printf("[CMMD2] Activating at ADDR: %d as status: %d",x,status);
citolin 0:40a00c231fbd 140 dio[x] = status;
citolin 0:40a00c231fbd 141
citolin 0:40a00c231fbd 142 break;
citolin 0:40a00c231fbd 143 }
citolin 1:cca3a4e419dd 144
citolin 0:40a00c231fbd 145 //case CMMD_3:
citolin 0:40a00c231fbd 146 // break;
citolin 0:40a00c231fbd 147
citolin 0:40a00c231fbd 148 case CMMD_4: {
citolin 0:40a00c231fbd 149 //pc.printf("CMMD 4\n");
citolin 0:40a00c231fbd 150
citolin 0:40a00c231fbd 151 std::string cmmd4 = read_ios_in_range(5,9);
citolin 2:d1c24eae74d5 152 cmmd4 = "{4;" + cmmd4 + "}";
citolin 0:40a00c231fbd 153
citolin 0:40a00c231fbd 154 // Read the output I/Os (bitset from 5 to 9)
citolin 0:40a00c231fbd 155 pc.printf("CMMD4 - %s\n", cmmd4);
citolin 2:d1c24eae74d5 156
citolin 2:d1c24eae74d5 157 // Send through the network
citolin 2:d1c24eae74d5 158 connection.send((char*)cmmd4.c_str(),cmmd4.size());
citolin 2:d1c24eae74d5 159
citolin 0:40a00c231fbd 160 break;
citolin 0:40a00c231fbd 161 }
citolin 0:40a00c231fbd 162
citolin 0:40a00c231fbd 163 case CMMD_5: {
citolin 0:40a00c231fbd 164 pc.printf("CMMD 5\n");
citolin 0:40a00c231fbd 165 // Enter MAINTENANCE MODE.
citolin 0:40a00c231fbd 166
citolin 2:d1c24eae74d5 167 set_state(MAINTENANCE_MODE);
citolin 0:40a00c231fbd 168 // code here ...
citolin 1:cca3a4e419dd 169
citolin 0:40a00c231fbd 170 break;
citolin 0:40a00c231fbd 171 }
citolin 0:40a00c231fbd 172 case CMMD_6: {
citolin 0:40a00c231fbd 173 pc.printf("CMMD 6\n");
citolin 0:40a00c231fbd 174 // Enter AUTOMATIC MODE.
citolin 0:40a00c231fbd 175
citolin 2:d1c24eae74d5 176 set_state(AUTOMATIC_MODE);
citolin 0:40a00c231fbd 177 // code here ...
citolin 1:cca3a4e419dd 178
citolin 0:40a00c231fbd 179 break;
citolin 0:40a00c231fbd 180 }
citolin 0:40a00c231fbd 181
citolin 0:40a00c231fbd 182 case CMMD_7: {
citolin 0:40a00c231fbd 183 pc.printf("CMMD 7\n");
citolin 0:40a00c231fbd 184 // CMMD 7 - { 7 X } X - ID of station that the test ended.
citolin 0:40a00c231fbd 185 // End of test on station X
citolin 0:40a00c231fbd 186
citolin 0:40a00c231fbd 187 // code here ...
citolin 0:40a00c231fbd 188 break;
citolin 0:40a00c231fbd 189 }
citolin 0:40a00c231fbd 190
citolin 0:40a00c231fbd 191 case CMMD_S: {
citolin 0:40a00c231fbd 192 pc.printf("CMMD S\n");
citolin 0:40a00c231fbd 193 // CMMD S - { S X X X X } X - If station is executable or not (1 or 0). 1 for executable, 0 for skip.
citolin 0:40a00c231fbd 194
citolin 0:40a00c231fbd 195 // code here ...
citolin 0:40a00c231fbd 196 break;
citolin 0:40a00c231fbd 197 }
citolin 0:40a00c231fbd 198
citolin 0:40a00c231fbd 199 case CMMD_P: {
citolin 0:40a00c231fbd 200 // CMMD P - { P X } X - Enter pause mode or not. (1 or 0). 1 for pause mode 0 to leave pause mode.
citolin 0:40a00c231fbd 201
citolin 2:d1c24eae74d5 202 set_state(PAUSED_MODE);
citolin 0:40a00c231fbd 203 // code here ...
citolin 1:cca3a4e419dd 204
citolin 0:40a00c231fbd 205 break;
citolin 0:40a00c231fbd 206 }
citolin 0:40a00c231fbd 207
citolin 0:40a00c231fbd 208 case CMMD_E: {
citolin 0:40a00c231fbd 209 pc.printf("CMMD E\n");
citolin 0:40a00c231fbd 210 // CMMD E - { E } - IHM wants to know if CLP is on emergency mode or not.
citolin 2:d1c24eae74d5 211
citolin 2:d1c24eae74d5 212 // Suggestion done by setting the general state as emergency or not.
citolin 2:d1c24eae74d5 213 char isEmergencyOn = '0';
citolin 2:d1c24eae74d5 214 if(state==EMERGENCY_MODE)
citolin 2:d1c24eae74d5 215 isEmergencyOn = '1';
citolin 0:40a00c231fbd 216
citolin 2:d1c24eae74d5 217 char cmmde[5];
citolin 2:d1c24eae74d5 218 memset(cmmde,'\0',sizeof(cmmde));
citolin 2:d1c24eae74d5 219 cmmde[0] = '{';
citolin 2:d1c24eae74d5 220 cmmde[1] = 'E';
citolin 2:d1c24eae74d5 221 cmmde[2] = isEmergencyOn;
citolin 2:d1c24eae74d5 222 cmmde[3] = '}';
citolin 2:d1c24eae74d5 223
citolin 2:d1c24eae74d5 224 connection.send(cmmde,sizeof(cmmde));
citolin 2:d1c24eae74d5 225
citolin 0:40a00c231fbd 226 break;
citolin 0:40a00c231fbd 227 }
citolin 0:40a00c231fbd 228
citolin 0:40a00c231fbd 229 default: {
citolin 0:40a00c231fbd 230 pc.printf("-- UNKOWN COMMAND --\n\t %s",queue.at(0));
citolin 0:40a00c231fbd 231 break;
citolin 0:40a00c231fbd 232 }
citolin 0:40a00c231fbd 233 }
citolin 0:40a00c231fbd 234
citolin 0:40a00c231fbd 235 // Delete first item from queue
citolin 0:40a00c231fbd 236 if(queue.size()>=1){
citolin 0:40a00c231fbd 237 free(queue.at(0));
citolin 0:40a00c231fbd 238 queue.erase(queue.begin());
citolin 0:40a00c231fbd 239 }
citolin 0:40a00c231fbd 240
citolin 0:40a00c231fbd 241 }
citolin 0:40a00c231fbd 242
citolin 0:40a00c231fbd 243 // -- MAIN --
citolin 0:40a00c231fbd 244 void Autoline::run() {
citolin 2:d1c24eae74d5 245 // General state machine
citolin 2:d1c24eae74d5 246 switch(state){
citolin 2:d1c24eae74d5 247
citolin 2:d1c24eae74d5 248 case MAINTENANCE_MODE:{
citolin 2:d1c24eae74d5 249 // code here ...
citolin 2:d1c24eae74d5 250 }
citolin 2:d1c24eae74d5 251
citolin 2:d1c24eae74d5 252 case AUTOMATIC_MODE: {
citolin 2:d1c24eae74d5 253 // code here ...
citolin 2:d1c24eae74d5 254
citolin 2:d1c24eae74d5 255 // Put all the station's code in here...
citolin 2:d1c24eae74d5 256 //input_elevator();
citolin 2:d1c24eae74d5 257 supply_station();
citolin 2:d1c24eae74d5 258 //hipot_wait_station();
citolin 2:d1c24eae74d5 259 //hipot_station();
citolin 2:d1c24eae74d5 260 //pf_station();
citolin 2:d1c24eae74d5 261 //ate_wait_station();
citolin 2:d1c24eae74d5 262 //ate1_station();
citolin 2:d1c24eae74d5 263 //ate2_station();
citolin 2:d1c24eae74d5 264 //eprom_station();
citolin 2:d1c24eae74d5 265 //remove_station();
citolin 2:d1c24eae74d5 266 //output_elevator();
citolin 2:d1c24eae74d5 267 }
citolin 2:d1c24eae74d5 268
citolin 2:d1c24eae74d5 269 case EMERGENCY_MODE: {
citolin 2:d1c24eae74d5 270 // code here ...
citolin 2:d1c24eae74d5 271 }
citolin 2:d1c24eae74d5 272
citolin 2:d1c24eae74d5 273 case PAUSED_MODE: {
citolin 2:d1c24eae74d5 274 // code here ...
citolin 2:d1c24eae74d5 275 }
citolin 0:40a00c231fbd 276
citolin 2:d1c24eae74d5 277 }
citolin 1:cca3a4e419dd 278 supply_station();
citolin 1:cca3a4e419dd 279
citolin 0:40a00c231fbd 280 // Put all the stations code in here
citolin 1:cca3a4e419dd 281 }
citolin 1:cca3a4e419dd 282
citolin 1:cca3a4e419dd 283
citolin 1:cca3a4e419dd 284 // EXAMPLE FOR SUPPLY STATION WITHOUT STATE MACHINE
citolin 1:cca3a4e419dd 285 // Coded as it is on google sheets by Matheus Castro
citolin 1:cca3a4e419dd 286 // No state machine yet (might not even be necessary)
citolin 1:cca3a4e419dd 287 void Autoline::supply_station() {
citolin 1:cca3a4e419dd 288 // In this example there are no else ifs, but there might be in the convyor algorithm.
citolin 1:cca3a4e419dd 289 // It's important to notice the difference between the IF's logic and the ELSE IF's logic.
citolin 1:cca3a4e419dd 290
citolin 1:cca3a4e419dd 291 // PALLET RELEASE
citolin 1:cca3a4e419dd 292 if( dio[PS_SUPP] && !dio[PS_WT_HIP] && dio[BT_GO_SUPP] && !dio[SSTOP_SUPP] ) {
citolin 1:cca3a4e419dd 293 // Do something ...
citolin 1:cca3a4e419dd 294 }
citolin 1:cca3a4e419dd 295
citolin 1:cca3a4e419dd 296 // STOP LIFTING
citolin 1:cca3a4e419dd 297 if( !dio[PS_SUPP] || dio[PS_WT_HIP] ) {
citolin 1:cca3a4e419dd 298 // Do something ...
citolin 1:cca3a4e419dd 299 }
citolin 1:cca3a4e419dd 300
citolin 1:cca3a4e419dd 301 // IDLE
citolin 1:cca3a4e419dd 302 if( (!dio[PS_SUPP] && !dio[SSTOP_SUPP]) || (dio[PS_SUPP] && !dio[BT_GO_SUPP]) ) {
citolin 1:cca3a4e419dd 303 // Do something ...
citolin 1:cca3a4e419dd 304 }
citolin 1:cca3a4e419dd 305
citolin 0:40a00c231fbd 306 }