Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

Lots of api calls

  • Loading branch information...
commit aeeb7c041e45e37b0afe2b2bb5aab889d72d0733 1 parent b038745
@HalfdanJ HalfdanJ authored
View
109 src/adlpCom.cpp
@@ -20,8 +20,8 @@ ADLPCom::ADLPCom(){
receivingMessage = false;
sendingMessage = false;
waitingForResponse = false;
- messageRead = false;
memset(outgoingFormalities, false, 2);
+ readMessageCounter = 0;
startThread(true, false);
@@ -39,21 +39,28 @@ void ADLPCom::queueMessage(ARAPMessage msg){
}
}
-bool ADLPCom::available(){
- return !messageRead;
-}
-ARAPMessage ADLPCom::readMessage(){
- ARAPMessage ret;
+vector<ARAPMessage> ADLPCom::readMessages(){
+ vector<ARAPMessage> ret;
if(lock()){
- ret = incommingMessage;
+ ret = incommingMessages;
unlock();
}
return ret;
}
-void ADLPCom::markRead(){
- messageRead = true;
+vector<ARAPMessage> ADLPCom::readMessagesAfterCount(long count){
+ vector<ARAPMessage> ret ;
+ long diff = readMessageCounter - count;
+ if(diff <= 0){
+ return ret;
+ } else {
+ vector<ARAPMessage> v = readMessages();
+ for(int i=0;i<diff;i++){
+ ret.push_back(v[v.size()-1-i]);
+ }
+ return ret;
+ }
}
void ADLPCom::threadedFunction(){
@@ -116,13 +123,13 @@ void ADLPCom::threadedFunction(){
message[5] = msg.messageType;
message[6] = 0; //function suffix << 8 TODO
message[7] = msg.functionSuffix; //function suffix
- for(int i=0;i<msg.size-8;i++){
+ for(int i=0;i<msg.size;i++){
message[8+i] = msg.data[i];
cout<<"Add data: "<<8+i<<" "<<(int)msg.data[i]<<endl;
}
unsigned char checksum = message[0];
- for(int i=1;i<msg.size;i++){
+ for(int i=1;i<msg.size+8;i++){
checksum = checksum ^ message[i];
}
// message[7] = 1;
@@ -130,7 +137,7 @@ void ADLPCom::threadedFunction(){
serial.writeByte(DLE);
serial.writeByte((msg.size % 2)? STXodd : STXeven);
- serial.writeBytes(message, msg.size);
+ serial.writeBytes(message, msg.size+8);
serial.writeByte(DLE);
serial.writeByte(ETX);
serial.writeByte(checksum ^ ETX);
@@ -153,21 +160,45 @@ void ADLPCom::parseIncommingByte(unsigned char bytesReturned){
outgoingFormalities[0] = false;
outgoingFormalities[1] = false;
sendingMessage = false;
+ serial.writeByte(EOT);
} else if(outgoingFormalities[0] && !outgoingFormalities[1]) {
//Start sending message
outgoingFormalities[1] = true;
}
}
- 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 + multimessageDataIndex < incommingMessageTemp.size+8) ){
//We are processing a message content that is incomming
switch (incommingMessageIndex) {
case 0:
//Not used
break;
case 1:
+ {
cout<<"Start new message with size "<<(int)bytesReturned<<endl;
- incommingMessageTemp.size = bytesReturned;
- incommingMessageTemp.data = (unsigned char*) malloc(incommingMessageTemp.size*sizeof(unsigned char));
+ int oldSize = incommingMessageTemp.size;
+ unsigned char temp[oldSize];
+ if(oldSize > 0){
+ cout<<"Already "<<(int)oldSize<<" bytes in the message to add to"<<endl;
+ for(int i=0;i<oldSize;i++){
+ temp[i] = incommingMessageTemp.data[i];
+ }
+// memcpy(temp , incommingMessageTemp.data, oldSize*sizeof(unsigned char));
+ //Now we got a copy of the old data, so we can allocate a bigger one
+ free(incommingMessageTemp.data);
+ }
+ incommingMessageTemp.size += bytesReturned-8;
+ incommingMessageTemp.data = (unsigned char*) malloc((incommingMessageTemp.size+100)*sizeof(unsigned char));
+
+ if(oldSize > 0){
+ //If we had some old data, lets copy it back
+ for(int i=0;i<oldSize;i++){
+ incommingMessageTemp.data[i] = temp[i];
+ }
+// memcpy(incommingMessageTemp.data, temp, oldSize*sizeof(unsigned char));
+ multimessageDataIndex = oldSize;
+ // free(temp);
+ }
+ }
break;
case 2:
if(bytesReturned == 1){
@@ -183,9 +214,6 @@ void ADLPCom::parseIncommingByte(unsigned char bytesReturned){
break;
case 5:
incommingMessageTemp.messageType = (ARAPMessageType)bytesReturned;
- if(incommingMessageTemp.messageType == warning){
- cout<<"WARNING"<<endl;
- }
break;
case 6:
incommingMessageTemp.functionSuffix = bytesReturned<<8;
@@ -196,10 +224,10 @@ void ADLPCom::parseIncommingByte(unsigned char bytesReturned){
default:
//Everything else is data
- printf("\t\t\t\tData: %x %i\n",bytesReturned,bytesReturned);
+ printf("\t\t\t\tData %i: %x %i\n",multimessageDataIndex+incommingMessageIndex-8, bytesReturned,bytesReturned);
if(bytesReturned > 31 && bytesReturned < 127)
printf("%c",bytesReturned);
- incommingMessageTemp.data[incommingMessageIndex-8] = bytesReturned;
+ incommingMessageTemp.data[multimessageDataIndex+incommingMessageIndex-8] = bytesReturned;
break;
}
@@ -212,6 +240,8 @@ void ADLPCom::parseIncommingByte(unsigned char bytesReturned){
incommingMessageIndex = 0; //Reset the index counter
memset(incommingFormalities,false,5); //reset the formalities handler
incommingFormalities[0] = true; //Tell that the first formality is met
+ multimessageDataIndex = 0;
+ incommingMessageTemp.size = 0;
} else if(bytesReturned == DLE){
if(!incommingFormalities[1]){
@@ -240,14 +270,17 @@ void ADLPCom::parseIncommingByte(unsigned char bytesReturned){
//End of transmission
cout<<"got EOT"<<endl;
//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();
- // }
+ waitingForResponse = false;
+ printf("Done receiving at msg %i\n",incommingMessageIndex);
+ printf("Received instruction: %i",incommingMessageTemp.instruction );
+ incommingMessageIndex = -1;
+ incommingMessages.push_back(incommingMessageTemp);
+ readMessageCounter ++;
+ while (incommingMessages.size() > incommingBufferSize){
+ incommingMessages.erase(incommingMessages.begin());
+ }
+ incommingMessageTemp = ARAPMessage();
+ // }
}
else if(incommingMessageIndex >= 0 && incommingFormalities[0] && incommingFormalities[1] && incommingFormalities[2] && incommingFormalities[3] && incommingFormalities[4]){
printf("Recevied checksum %x\n",bytesReturned);
@@ -255,15 +288,15 @@ void ADLPCom::parseIncommingByte(unsigned char bytesReturned){
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();
-
- }*/
+ /* 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
13 src/adlpCom.h
@@ -22,6 +22,7 @@
#define ETX 0x03
#define EOT 0x04
+#define incommingBufferSize 100
class ADLPCom : public ofxThread{
@@ -31,9 +32,10 @@ class ADLPCom : public ofxThread{
void update();
void queueMessage(ARAPMessage message);
- bool available(); //if there is a readMessage
- ARAPMessage readMessage();
- void markRead(); //Mark the incomming message as read (so nobody else reads it)
+ vector<ARAPMessage> readMessages();
+ vector<ARAPMessage> readMessagesAfterCount(long count);
+
+ long readMessageCounter; //For each new message that is received this is ++
bool connected;
bool receivingMessage;
@@ -50,11 +52,12 @@ class ADLPCom : public ofxThread{
void parseIncommingByte(unsigned char bytesReturned);
vector<ARAPMessage> messageQueue;
- ARAPMessage incommingMessage;
- bool messageRead;
+ vector<ARAPMessage> incommingMessages;
bool outgoingFormalities[2];
void threadedFunction();
+ int multimessageDataIndex;
+
};
View
20 src/arapInstructions.h
@@ -1,5 +1,7 @@
#pragma once
+
+
enum ARAP_MODE {
STANDBY = 0,
OPERATION = 1,
@@ -9,6 +11,16 @@ enum ARAP_MODE {
EMERGENCYSTOP = 30
};
+struct ARAP_COORDINATE {
+ int x;
+ int y;
+ int z;
+ int q1;
+ int q2;
+ int q3;
+ int q4;
+};
+
struct ARAP_STATUS {
@@ -21,4 +33,10 @@ struct ARAP_STATUS {
bool programming_unit; //connected or not connected
bool key; //false = auto | true = auto or test 100%
ARAP_MODE mode;
-};
+ ARAP_COORDINATE location;
+};
+
+struct ARAP_PROGRAM {
+ unsigned char * data;
+};
+
View
2  src/arapParser.cpp
@@ -2,7 +2,7 @@
ARAPMessage ARAPParser::constructMessage(ARAPInstruction instruction, int functionSuffix, unsigned char * data, int datasize){
ARAPMessage msg;
- msg.size = datasize + 8;
+ msg.size = datasize;
msg.fromComputer = true;
msg.instruction = instruction;
msg.messageType = query;
View
195 src/ofxABBRobot.cpp
@@ -1,6 +1,102 @@
#include "ofxABBRobot.h"
#include "ofMain.h"
+string errorString(int errorNum){
+ switch (errorNum) {
+ case 0:
+ return "Not specified";
+ break;
+ case 1:
+ return "Program running";
+ break;
+ case 2:
+ return "Wrong function suffix";
+ break;
+ case 3:
+ return "Wrong program number";
+ break;
+ case 4:
+ return "Wrong BLOCK number";
+ break;
+ case 5:
+ return "Wrong key mode";
+ break;
+ case 6:
+ return "Wrong operation mode";
+ break;
+ case 7:
+ return "Non existent program number";
+ break;
+ case 8:
+ return "Not specified";
+ break;
+ case 9:
+ return "Block is to big";
+ break;
+ case 10:
+ return "No more space left";
+ break;
+ case 11:
+ return "Wrong function code";
+ break;
+ case 12:
+ return "Floppydisc error";
+ break;
+ case 13:
+ return "Stop, emergency stop";
+ break;
+ case 14:
+ return "Not specified";
+ break;
+ case 15:
+ return "Outside the work area or a lack of start point";
+ break;
+ case 16:
+ return "Workstop";
+ break;
+ case 17:
+ return "Dependency is NULL";
+ break;
+ case 18:
+ return "No welding robot";
+ break;
+
+ case 30:
+ return "Incorrect checksum in the configuration data";
+ break;
+ case 31:
+ return "Not possible to proceed";
+ break;
+ case 32:
+ return "Incorrect welding data release";
+ break;
+ case 33:
+ return "LOCATION reading a register, with 20-29 active TCP is not possible";
+ break;
+ case 34:
+ return "Move the robot with an active TCP 20-29 is not possible";
+ break;
+ case 35:
+ return "Change the RESOLVER DATA not possible";
+ break;
+ case 36:
+ return "ANALOG OUTPUTS value out of range";
+ break;
+ case 99:
+ return "Timeout";
+ break;
+ case 255:
+ return "Did not get the whole telegram content";
+ break;
+
+ default:
+ return "Wrong error code";
+ break;
+ }
+}
+
+
+
ofxABBRobot::ofxABBRobot(){
parser = new ARAPParser();
com = new ADLPCom();
@@ -51,22 +147,24 @@ string ofxABBRobot::errorMessageToString(ARAPMessage msg){
string ofxABBRobot::warningMessageToString(ARAPMessage msg){
string ret;
if(isWarningMessage(msg)){
- ret += "Warning returned: "+ofToString((msg.data[8-8]<<8) + msg.data[9-8]);
+ ret += "Warning returned: "+ofToString((msg.data[8-8]<<8) + msg.data[9-8])+" "+errorString((msg.data[8-8]<<8)+ msg.data[9-8]);
}
return ret;
}
ARAPMessage ofxABBRobot::responseSyncQuery(ARAPMessage msg){
+ responseCounter = com->readMessageCounter;
com->queueMessage(msg);
bool gotResponse = false;
ARAPMessage response;
while (!gotResponse) {
- if(com->available()){
- response = com->readMessage();
- if(response.instruction == msg.instruction){
+ vector<ARAPMessage> messages = com->readMessagesAfterCount(responseCounter);
+ responseCounter = com->readMessageCounter;
+ for(int i=0;i<messages.size();i++){
+ if(messages[i].instruction == msg.instruction){
+ response = messages[i];
cout<<"Got message ################"<<endl;
gotResponse = true;
- com->markRead();
break;
}
}
@@ -82,21 +180,19 @@ void ofxABBRobot::commandQuery(ARAPMessage 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;
@@ -132,8 +228,89 @@ ARAP_STATUS ofxABBRobot::readStatus(bool async){
ret.mode = EMERGENCYSTOP;
}
+ ret.location.x = (response.data[18-8]<<8)+response.data[19-8];
+ ret.location.y = (response.data[20-8]<<8)+response.data[21-8];
+ ret.location.z = (response.data[22-8]<<8)+response.data[23-8];
+ ret.location.q1 = (response.data[24-8]<<8)+response.data[25-8];
+ ret.location.q2 = (response.data[26-8]<<8)+response.data[27-8];
+ ret.location.q3 = (response.data[28-8]<<8)+response.data[29-8];
+ ret.location.q4 = (response.data[30-8]<<8)+response.data[31-8];
}
return ret;
}
+ARAP_PROGRAM ofxABBRobot::receiveProgram(int program){
+ unsigned char data[4];
+ data[1] = program & 255;
+ data[0] = program - data[1];
+ data[2] = 0;
+ data[3] = 0;
+ ARAPMessage msg = parser->constructMessage(RECEIVEPROGRAM, 1, data, 4);
+
+ ARAPMessage response = responseSyncQuery(msg);
+ ARAP_PROGRAM ret;
+ ret.data = (unsigned char*)malloc((response.size - 4) * sizeof(unsigned char));
+ for(int i=0;i<(response.size - 4);i++){
+ ret.data[i] = response.data[4+i];
+ }
+ printf("Error / program number: %i %i",response.data[0], response.data[1]);
+ //memcpy((void*)ret.data, response.data+4, (response.size - 4) * sizeof(unsigned char));
+ cout<<"--------"<<endl<<endl;
+ for(int i=0;i<(response.size - 4);i++){
+ // if(ret.data[i] == 0){
+ // ret.data[i] = ' ';
+ // }
+ printf("%i\n",ret.data[i]);
+ }
+ // cout<<ret.data<<endl;
+ return ret;
+}
+
+
+void ofxABBRobot::move(ARAP_COORDINATE coord, int velocity, int runSpeed, bool absolute){
+ unsigned char data[52];
+ memset(data, 0, 52);
+
+ data[13-8] = 0; //Move data
+
+ data[14-8] = velocity >> 8;
+ data[15-8] = velocity & 255;
+
+ data[16-8] = runSpeed >> 8;
+ data[17-8] = runSpeed & 255;
+
+ data[18-8] = coord.x >> 8;
+ data[19-8] = coord.x & 255;
+
+ data[20-8] = coord.y >> 8;
+ data[21-8] = coord.y & 255;
+
+ data[22-8] = coord.z >> 8;
+ data[23-8] = coord.z & 255;
+
+ data[24-8] = coord.q1 >> 8;
+ data[25-8] = coord.q1 & 255;
+
+ data[26-8] = coord.q2 >> 8;
+ data[27-8] = coord.q2 & 255;
+
+ data[28-8] = coord.q3 >> 8;
+ data[29-8] = coord.q3 & 255;
+
+ data[30-8] = coord.q4 >> 8;
+ data[31-8] = coord.q4 & 255;
+
+ data[32-8] = 255;
+ data[33-8] = 255;
+ data[34-8] = 255;
+ data[35-8] = 114;
+ // ARAPMessage msg = parser->constructMessage(MOVE, absolute?1:0, data, 52);
+ ARAPMessage msg = parser->constructMessage(MOVE, 0, data, 52);
+ commandQuery(msg);
+
+
+}
+
+
+
View
8 src/ofxABBRobot.h
@@ -16,13 +16,19 @@ class ofxABBRobot {
void stopProgram();
void writeMode(ARAP_MODE mode);
ARAP_STATUS readStatus(bool async=false);
-
+ ARAP_PROGRAM receiveProgram(int program);
+ void move(ARAP_COORDINATE coord, int velocity, int runSpeed, bool absolute = true);
+
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);
+
+ long responseCounter;
};
Please sign in to comment.
Something went wrong with that request. Please try again.