Browse files

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

  • Loading branch information...
1 parent 353698a commit b038745d325008a8225ffd346e7c5496eee33786 @HalfdanJ HalfdanJ committed Jul 19, 2011
Showing with 166 additions and 47 deletions.
  1. +56 −34 src/adlpCom.cpp
  2. +6 −3 src/adlpCom.h
  3. +6 −2 src/arapInstructions.h
  4. +7 −2 src/arapParser.cpp
  5. +1 −1 src/arapParser.h
  6. +80 −4 src/ofxABBRobot.cpp
  7. +10 −1 src/ofxABBRobot.h
View
90 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();
+
+ }*/
}
}
View
9 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];
View
8 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;
View
9 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;
}
View
2 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);
};
View
84 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;
View
11 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);
};

0 comments on commit b038745

Please sign in to comment.