Added a lot of api calls to ARAP, and improved the ADLP protocol

src/adlpCom.cpp

@@ -19,6 +19,8 @@ ADLPCom::ADLPCom(){
     incommingMessageIndex = -1;
     receivingMessage = false;
     sendingMessage = false;
+    waitingForResponse = false;
+    messageRead = false;
     memset(outgoingFormalities, false, 2);
 
     startThread(true, false);
@@ -38,32 +40,30 @@ void ADLPCom::queueMessage(ARAPMessage msg){
 }
 
 bool ADLPCom::available(){
-    bool ret = false;
-    if(lock()){
-        ret = (incommingMessages.size() > 0);
-        unlock();
-    }
-    return ret; 
+    return !messageRead; 
 }
 
 ARAPMessage ADLPCom::readMessage(){
     ARAPMessage ret;
     if(lock()){
-        ret = incommingMessages[0];
-        incommingMessages.erase(incommingMessages.begin());
+        ret = incommingMessage;
         unlock();
     }
     return ret;
 }
 
+void ADLPCom::markRead(){
+    messageRead = true;
+}
+
 void ADLPCom::threadedFunction(){
     while( isThreadRunning() != 0 ){
         if( lock() ){
             unsigned char bytesReturned;
             while(serial.available()  > 0){
                 bytesReturned = serial.readByte();
 
-                printf("%i: ",incommingMessageIndex);
+                printf("%i/%i: ",incommingMessageIndex, incommingMessageTemp.size);
                 switch (bytesReturned) {
                     case ACK:
                         printf("Recv: %x (ACK)\n",bytesReturned);
@@ -93,7 +93,7 @@ void ADLPCom::threadedFunction(){
 
             }
 
-            if(messageQueue.size() > 0 && !receivingMessage){
+            if(messageQueue.size() > 0 && !receivingMessage && !waitingForResponse){
                 //There are messages to be send
                 if(!sendingMessage){
                     //ENQ not send yet, lets send it
@@ -114,10 +114,11 @@ void ADLPCom::threadedFunction(){
                     message[3] = 0; //Mesesage from PC to controller
                     message[4] = msg.instruction; // Instruction
                     message[5] = msg.messageType;
-                    message[6] = 0; //function suffix
+                    message[6] = 0; //function suffix << 8 TODO
                     message[7] = msg.functionSuffix; //function suffix
                     for(int i=0;i<msg.size-8;i++){
                         message[8+i] = msg.data[i];
+                        cout<<"Add data: "<<8+i<<"  "<<(int)msg.data[i]<<endl;
                     }
 
                     unsigned char checksum = message[0];
@@ -133,6 +134,9 @@ void ADLPCom::threadedFunction(){
                     serial.writeByte(DLE);
                     serial.writeByte(ETX);
                     serial.writeByte(checksum ^ ETX);
+                    if(msg.messageType == query){
+                        waitingForResponse = true;
+                    }
                 }
             }
             unlock();
@@ -154,16 +158,7 @@ void ADLPCom::parseIncommingByte(unsigned char bytesReturned){
             outgoingFormalities[1] = true;
         }
     }
-    if(bytesReturned == ENQ){
-        //New message is incomming
-        //Return with an acknowledge 
-        serial.writeByte(ACK);
-        receivingMessage = true;
-        incommingMessageIndex = 0; //Reset the index counter
-        memset(incommingFormalities,false,5); //reset the formalities handler
-        incommingFormalities[0] = true; //Tell that the first formality is met
-
-    } else if(incommingMessageIndex >= 0 && incommingFormalities[0] && incommingFormalities[1] && incommingFormalities[2] && !incommingFormalities[3] && !incommingFormalities[4] && (incommingMessageIndex <= 1 || incommingMessageIndex < incommingMessageTemp.size) ){
+    if(incommingMessageIndex >= 0 && incommingFormalities[0] && incommingFormalities[1] && incommingFormalities[2] && !incommingFormalities[3] && !incommingFormalities[4] && (incommingMessageIndex <= 1 || incommingMessageIndex < incommingMessageTemp.size) ){
         //We are processing a message content that is incomming
         switch (incommingMessageIndex) {
             case 0:
@@ -193,10 +188,10 @@ void ADLPCom::parseIncommingByte(unsigned char bytesReturned){
                 }
                 break;
             case 6:
-                //function
+                incommingMessageTemp.functionSuffix = bytesReturned<<8;
                 break;
             case 7:
-                //function
+                incommingMessageTemp.functionSuffix += bytesReturned;
                 break;
 
             default:
@@ -209,13 +204,30 @@ void ADLPCom::parseIncommingByte(unsigned char bytesReturned){
         }
 
         incommingMessageIndex++;
-    } else if(bytesReturned == DLE){
+    }  else  if(bytesReturned == ENQ){
+        //New message is incomming
+        //Return with an acknowledge 
+        serial.writeByte(ACK);
+        receivingMessage = true;
+        incommingMessageIndex = 0; //Reset the index counter
+        memset(incommingFormalities,false,5); //reset the formalities handler
+        incommingFormalities[0] = true; //Tell that the first formality is met
+
+    }  else if(bytesReturned == DLE){
         if(!incommingFormalities[1]){
             incommingFormalities[1] = true;            
             cout<<"First DLE"<<endl;
-        }else {
+        }else if(!incommingFormalities[3]) {
             incommingFormalities[3] = true; //Second time DLE is received after message         
             cout<<"Second DLE"<<endl;
+        } else {
+            //New message
+            cout<<"----------"<<endl<<"Next message"<<endl;
+            incommingMessageIndex = 0; //Reset the index counter
+            memset(incommingFormalities,false,5); //reset the formalities handler
+            incommingFormalities[0] = true;
+            incommingFormalities[1] = true; 
+            receivingMessage = true;
         }
     } else if(bytesReturned == STXodd || bytesReturned == STXeven){
         incommingFormalities[2] = true;       
@@ -227,21 +239,31 @@ void ADLPCom::parseIncommingByte(unsigned char bytesReturned){
     } else if(bytesReturned == EOT){
         //End of transmission
         cout<<"got EOT"<<endl;
-        printf("Done receiving at msg %i\n",incommingMessageIndex);
-        printf("Received instruction: %i",incommingMessageTemp.instruction );
-        if(incommingMessageTemp.messageType == warning){
-            cout<<"WARNING"<<endl;
-        }
-        cout<<incommingMessageTemp.data<<endl;
-        incommingMessageIndex = -1;
-        incommingMessages.push_back(incommingMessageTemp);
-        incommingMessageTemp = ARAPMessage();
+        //if(incommingMessageTemp.instruction != 0x7F){
+            waitingForResponse = false;
+            printf("Done receiving at msg %i\n",incommingMessageIndex);
+            printf("Received instruction: %i",incommingMessageTemp.instruction );
+            incommingMessageIndex = -1;
+            incommingMessage = incommingMessageTemp;
+            messageRead = false;
+            incommingMessageTemp = ARAPMessage();
+      //  }
     } 
     else if(incommingMessageIndex >= 0 && incommingFormalities[0] && incommingFormalities[1] && incommingFormalities[2] && incommingFormalities[3] && incommingFormalities[4]){
         printf("Recevied checksum %x\n",bytesReturned);
         //All formalities met, check cheksum (TODO)
         serial.writeByte(ACK); 
         receivingMessage = false;
 
+       /* if(incommingMessageTemp.instruction == 0x7F){
+            //HACK: When we receive automatic status updates, we don't always get a EOT, so we cheat here. I don't know why
+            waitingForResponse = false;
+            printf("Received instruction: %i",incommingMessageTemp.instruction );
+            incommingMessageIndex = -1;
+            incommingMessage = incommingMessageTemp;
+            messageRead = false;
+            incommingMessageTemp = ARAPMessage();
+            
+        }*/
     }        
 }

src/adlpCom.h

@@ -31,8 +31,9 @@ class ADLPCom : public ofxThread{
 	void update();
 
 	void queueMessage(ARAPMessage message);
-    bool available();
-    ARAPMessage readMessage(); //Check if available()! 
+    bool available(); //if there is a readMessage
+    ARAPMessage readMessage(); 
+    void markRead(); //Mark the incomming message as read (so nobody else reads it)
 
 	bool connected;
 	bool receivingMessage;
@@ -44,11 +45,13 @@ class ADLPCom : public ofxThread{
     int incommingMessageIndex;
     ARAPMessage incommingMessageTemp;    
     bool incommingFormalities[5];
+    bool waitingForResponse;
 
     void parseIncommingByte(unsigned char bytesReturned);
 
     vector<ARAPMessage> messageQueue;
-    vector<ARAPMessage> incommingMessages;
+    ARAPMessage incommingMessage;
+    bool messageRead;
 
     bool outgoingFormalities[2];
 

src/arapInstructions.h

@@ -3,10 +3,14 @@
 enum ARAP_MODE {
     STANDBY = 0,
     OPERATION = 1,
-    EXECUTION = 2,
-    EMERGENCYSTOP = 3
+    SYNCHRONIZATION = 2,
+    OPERATIONANDSYNCHRONIZATION = 3,
+    EXECUTION = 20,
+    EMERGENCYSTOP = 30
 };
 
+
+
 struct ARAP_STATUS {
     int program_number;
     int instruction_number;

src/arapParser.cpp

@@ -1,12 +1,17 @@
 #include "arapParser.h"
 
-ARAPMessage ARAPParser::constructMessage(ARAPInstruction instruction, unsigned char * data, int datasize){
+ARAPMessage ARAPParser::constructMessage(ARAPInstruction instruction, int functionSuffix, unsigned char * data, int datasize){
     ARAPMessage msg;
     msg.size = datasize + 8;
     msg.fromComputer = true;
     msg.instruction = instruction;
     msg.messageType = query;
-    msg.data = data;
+    msg.functionSuffix = functionSuffix;
+    msg.data = (unsigned char*)malloc(datasize * sizeof(unsigned char));
+
+    for(int i=0;i<datasize;i++){
+        msg.data[i] = data[i];
+    }
 
     return msg;    
 }

src/arapParser.h

@@ -51,5 +51,5 @@ struct ARAPMessage {
 
 class ARAPParser {
 public:
-    ARAPMessage constructMessage(ARAPInstruction instruction, unsigned char * data, int datasize);
+    ARAPMessage constructMessage(ARAPInstruction instruction, int functionSuffix = 0, unsigned char * data = nil, int datasize = 0);
 };

src/ofxABBRobot.cpp

@@ -1,4 +1,5 @@
 #include "ofxABBRobot.h"
+#include "ofMain.h"
 
 ofxABBRobot::ofxABBRobot(){
     parser = new ARAPParser();
@@ -7,20 +8,65 @@ ofxABBRobot::ofxABBRobot(){
 
 void ofxABBRobot::update(){
     com->update();
-
-
+}
+
+bool ofxABBRobot::isErrorMessage(ARAPMessage msg){
+    if(msg.instruction == 127 && (msg.functionSuffix == 7 || msg.functionSuffix == 9 || msg.functionSuffix == 10 || msg.functionSuffix == 17)){
+        return true;
+    }
+    return false;
+}
+
+bool ofxABBRobot::isWarningMessage(ARAPMessage msg){
+    if(msg.messageType == warning){
+        return true;
+    }
+    return false;
+}
+
+string ofxABBRobot::errorMessageToString(ARAPMessage msg){
+    string ret;
+    if(isErrorMessage(msg)){
+        switch (msg.functionSuffix) {
+            case 7:
+                ret += "System error";
+                break;
+            case 9:
+                ret += "Vision init error";
+                break;
+            case 10:
+                ret += "Vision test error";
+                break;
+            case 17:
+                ret += "Operational error";
+                break;
+            default:
+                break;
+        }
+        ret += ofToString((msg.data[8-8]<<8) + msg.data[9-8])+"."+ofToString((msg.data[10-8] << 8) + msg.data[11-8]);
+    }
+    return ret;
+}
+
+string ofxABBRobot::warningMessageToString(ARAPMessage msg){
+    string ret;
+    if(isWarningMessage(msg)){
+        ret += "Warning returned: "+ofToString((msg.data[8-8]<<8) + msg.data[9-8]);
+    }
+    return ret;
 }
 
 ARAPMessage ofxABBRobot::responseSyncQuery(ARAPMessage msg){
     com->queueMessage(msg);
     bool gotResponse = false;
     ARAPMessage response;
     while (!gotResponse) {
-        while(com->available()){
+        if(com->available()){
             response = com->readMessage();
             if(response.instruction == msg.instruction){
                 cout<<"Got message ################"<<endl;
                 gotResponse = true;
+                com->markRead();
                 break;
             }
         }
@@ -30,8 +76,38 @@ ARAPMessage ofxABBRobot::responseSyncQuery(ARAPMessage msg){
     return response;
 }
 
+void ofxABBRobot::commandQuery(ARAPMessage msg){
+    com->queueMessage(msg);
+}
+
+
+#pragma mark ARAP api
+void ofxABBRobot::startProgram(bool fromStart, int program){
+    unsigned char data[2];
+    data[1] = program & 255;
+    data[0] = program - data[1];
+
+    ARAPMessage msg = parser->constructMessage(STARTPROGRAM, fromStart?0:1, data, 2);
+
+    cout<<"&&&&&&&&&&&&&&&& data: "<<(int)data[0]<<"    "<<(int)data[1]<<endl;
+    commandQuery(msg);
+} 
+
+void ofxABBRobot::stopProgram(){
+    commandQuery(parser->constructMessage(STOPPROGRAM));
+}
+
+void ofxABBRobot::writeMode(ARAP_MODE mode){
+    if(mode > 3){
+        cout<<"Wrong input mode in writeMode()"<<endl;
+        return;
+    }
+    ARAPMessage msg = parser->constructMessage(WRITEMODE, mode);    
+    commandQuery(msg);
+}
+
 ARAP_STATUS ofxABBRobot::readStatus(bool async){
-    ARAPMessage msg = parser->constructMessage(READSTATUS, nil, 0);
+    ARAPMessage msg = parser->constructMessage(READSTATUS);
 
     ARAPMessage response;
     bool gotResponse = false;

src/ofxABBRobot.h

@@ -11,9 +11,18 @@ class ofxABBRobot {
 
     ADLPCom * com;
     ARAPParser * parser;
-
+
+    void startProgram(bool fromStart, int program);
+    void stopProgram();
+    void writeMode(ARAP_MODE mode);
     ARAP_STATUS readStatus(bool async=false);
 
+    bool isErrorMessage(ARAPMessage msg);
+    bool isWarningMessage(ARAPMessage msg);
+    string errorMessageToString(ARAPMessage msg);
+    string warningMessageToString(ARAPMessage msg);
+
 private:
     ARAPMessage responseSyncQuery(ARAPMessage msg);
+    void commandQuery(ARAPMessage msg);
 };