Autoline.cpp

00001 #include "Autoline.h"
00002 #include "ios.h"
00003 
00004 char* allocate_vector(int size) {
00005     char *aux = (char*)malloc(sizeof(char)*size);
00006     
00007     if(aux)
00008         memset(aux,'\0', sizeof(char)*size);
00009     
00010     return aux;    
00011 }
00012 
00013 Autoline::Autoline() : pc(USBTX, USBRX) { 
00014     // Register the parser callback
00015     connection.registerReadCallback(callback(this,&Autoline::parser));
00016     
00017     // Starts on MAINTENANCE_MODE
00018     state = MAINTENANCE_MODE;
00019 }
00020 
00021 Autoline::~Autoline() { }
00022 
00023 // It needs a mutex because it's a variable controlled over two threads (ethernet thread and main thread)
00024 void Autoline::set_state(short int newState) {
00025     _mutex.lock();
00026     state = newState;
00027     _mutex.unlock();    
00028 }
00029 
00030 void Autoline::print_queue() {
00031     pc.printf("[AUTOLINE - QUEUE] Queue size: %d\n",queue.size());
00032     for(int i=0;i<queue.size();i++)
00033         pc.printf("[AUTOLINE - QUEUE] [%d] - %s\n",i, queue.at(i));
00034 }
00035 
00036 void Autoline::protocoler(char *receivedFromEth, int size) {    
00037     int j = 0;
00038     bool reading = false;
00039     
00040     char local[MAX_SIZE_OF_CMMD];
00041     memset(local, '\0', sizeof(local));
00042     
00043     for(int i=0;i<30;i++) {
00044         
00045         // If it received the START of protocol: start to read
00046         if(receivedFromEth[i] == '{') {
00047             reading = true;
00048             continue; 
00049         }    
00050         // If it received the END of protocol:
00051         //      Allocate a char* for the arrived data 
00052         //      Put everything in the queue and clear the buffer
00053         else if(reading && receivedFromEth[i] == '}') {
00054             j = 0;
00055             reading = false;
00056             char *aux = allocate_vector(MAX_SIZE_OF_CMMD);
00057             
00058             for(int z=0;z<MAX_SIZE_OF_CMMD;z++)
00059                 aux[z] = local[z];
00060             
00061             queue.push_back(aux);
00062             
00063             memset(local,'\0',sizeof(local));   
00064         }
00065         // While reading
00066         else if(reading) {
00067             local[j] = receivedFromEth[i];
00068             j++;    
00069         }
00070     }
00071 }
00072 
00073 std::string Autoline::read_ios_in_range(int start, int end) {
00074     std::stringstream sstream;
00075     std::stringstream aux;
00076     std::string result;
00077             
00078     unsigned long ulong_bits = 0;
00079             
00080     // Charlean logic
00081     for(int i=end;i>=start;i--) {
00082         ulong_bits = dio.get_ulong(i);                
00083         aux << std::hex << ulong_bits;
00084         result = aux.str();
00085         if(result.size()==1)
00086             result.push_back('0');
00087         sstream << result;
00088     }
00089         
00090     return sstream.str();  
00091 }
00092 
00093 void Autoline::parser(char *cmmd, int size) {
00094     //pc.printf("[AUTOLINE - PARSER] -  %s\n",cmmd);
00095     
00096     protocoler(cmmd,size);
00097     
00098     // Print queue
00099     //printQueue();
00100     
00101     // If there is nothing in the queue, dont need to parse anything
00102     if(queue.size()==0)
00103         return;
00104     
00105     // Parse command
00106     switch(queue.at(0)[0]) {
00107         case CMMD_1: {
00108             //pc.printf("CMMD 1\n");
00109             
00110             // Read the input I/Os (bitset from 0 to 4)
00111             std::string cmmd1 = read_ios_in_range(0,4);
00112             cmmd1 = "{1;" + cmmd1 + "}";
00113             
00114             pc.printf("CMMD1 - %s\n", cmmd1);
00115             
00116             
00117             // Send through the network
00118             connection.send((char*)cmmd1.c_str(),cmmd1.size());
00119             
00120             break;
00121         }
00122         
00123         case CMMD_2: {
00124             //pc.printf("CMMD 2\n");
00125             // { 2 XX Y } -- XX = ADDR of actuator || Y - Status of actuator (0 or 1).
00126             
00127             unsigned int x = 0;
00128             bool status = (queue.at(0)[3] == '1')?true:false;
00129             char aux[3];
00130             memset(aux,'\0',sizeof(aux));
00131             
00132             aux[0] = queue.at(0)[1];
00133             aux[1] = queue.at(0)[2];
00134             
00135             std::stringstream ss;
00136             ss << std::hex << aux;
00137             ss >> x;
00138             
00139             pc.printf("[CMMD2] Activating at ADDR: %d as status: %d",x,status);
00140             dio[x] = status;
00141             
00142             break;
00143         }
00144         
00145         //case CMMD_3:
00146         //    break;
00147         
00148         case CMMD_4: {
00149             //pc.printf("CMMD 4\n");
00150             
00151             std::string cmmd4 = read_ios_in_range(5,9);
00152             cmmd4 = "{4;" + cmmd4 + "}";
00153             
00154             // Read the output I/Os (bitset from 5 to 9)
00155             pc.printf("CMMD4 - %s\n", cmmd4);
00156             
00157             // Send through the network
00158             connection.send((char*)cmmd4.c_str(),cmmd4.size());
00159             
00160             break;
00161         }
00162         
00163         case CMMD_5: {
00164             pc.printf("CMMD 5\n");
00165             // Enter MAINTENANCE MODE.
00166             
00167             set_state(MAINTENANCE_MODE);
00168             // code here ...
00169             
00170             break;
00171         }
00172         case CMMD_6: {
00173             pc.printf("CMMD 6\n");
00174             // Enter AUTOMATIC MODE.
00175             
00176             set_state(AUTOMATIC_MODE);
00177             // code here ...
00178             
00179             break;
00180         }
00181         
00182         case CMMD_7: {
00183             pc.printf("CMMD 7\n");
00184             // CMMD 7 - { 7 X } X - ID of station that the test ended.
00185             // End of test on station X
00186             
00187             // code here ...
00188             break;
00189         }
00190         
00191         case CMMD_S: {
00192             pc.printf("CMMD S\n");
00193             // CMMD S - { S X X X X } X - If station is executable or not (1 or 0). 1 for executable, 0 for skip.
00194             
00195             // code here ...
00196             break;   
00197         }
00198         
00199         case CMMD_P: {
00200             // CMMD P - { P X } X - Enter pause mode or not. (1 or 0). 1 for pause mode 0 to leave pause mode.   
00201             
00202             set_state(PAUSED_MODE);
00203             // code here ...
00204             
00205             break;
00206         }
00207         
00208         case CMMD_E: {
00209             pc.printf("CMMD E\n");
00210             // CMMD E - { E } - IHM wants to know if CLP is on emergency mode or not.
00211 
00212             // Suggestion done by setting the general state as emergency or not.
00213             char isEmergencyOn = '0';
00214             if(state==EMERGENCY_MODE)
00215                 isEmergencyOn = '1';
00216             
00217             char cmmde[5];
00218             memset(cmmde,'\0',sizeof(cmmde));
00219             cmmde[0] = '{';
00220             cmmde[1] = 'E';
00221             cmmde[2] = isEmergencyOn;
00222             cmmde[3] = '}';
00223             
00224             connection.send(cmmde,sizeof(cmmde));
00225             
00226             break;
00227         }
00228 
00229         default: {
00230             pc.printf("-- UNKOWN COMMAND --\n\t %s",queue.at(0));
00231             break;
00232         }
00233     }
00234     
00235     // Delete first item from queue
00236     if(queue.size()>=1){
00237         free(queue.at(0));
00238         queue.erase(queue.begin());    
00239     }
00240     
00241 }
00242 
00243 // -- MAIN --
00244 void Autoline::run() {
00245     // General state machine
00246     switch(state){
00247         
00248         case MAINTENANCE_MODE:{
00249             // code here ...    
00250         }    
00251         
00252         case AUTOMATIC_MODE: {
00253             // code here ...
00254             
00255             // Put all the station's code in here...
00256             //input_elevator();
00257             supply_station();    
00258             //hipot_wait_station();
00259             //hipot_station();
00260             //pf_station();
00261             //ate_wait_station();
00262             //ate1_station();
00263             //ate2_station();
00264             //eprom_station();
00265             //remove_station();
00266             //output_elevator();
00267         }
00268         
00269         case EMERGENCY_MODE: {
00270             // code here ...    
00271         }
00272         
00273         case PAUSED_MODE: {
00274             // code here ...    
00275         }
00276     
00277     }
00278     supply_station();
00279     
00280     // Put all the stations code in here
00281 }
00282 
00283 
00284 // EXAMPLE FOR SUPPLY STATION WITHOUT STATE MACHINE
00285 // Coded as it is on google sheets by Matheus Castro
00286 // No state machine yet (might not even be necessary)
00287 void Autoline::supply_station() {
00288     // In this example there are no else ifs, but there might be in the convyor algorithm.
00289     // It's important to notice the difference between the IF's logic and the ELSE IF's logic.
00290     
00291     // PALLET RELEASE
00292     if( dio[PS_SUPP] && !dio[PS_WT_HIP] && dio[BT_GO_SUPP] && !dio[SSTOP_SUPP] ) {
00293         // Do something ...
00294     }
00295     
00296     // STOP LIFTING
00297     if( !dio[PS_SUPP] || dio[PS_WT_HIP] ) {
00298         // Do something ...
00299     }
00300     
00301     // IDLE 
00302     if( (!dio[PS_SUPP] && !dio[SSTOP_SUPP]) || (dio[PS_SUPP] && !dio[BT_GO_SUPP]) ) {
00303         // Do something ...
00304     }
00305 
00306 }