Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

Added conversion in coordinates to metric

  • Loading branch information...
commit 1d3eb3653f1482a3bc7ee0a6e3037b98c4fdc7de 1 parent e3bbefa
@HalfdanJ HalfdanJ authored
View
10 src/arapInstructions.h
@@ -12,6 +12,16 @@ enum ARAP_MODE {
};
struct ARAP_COORDINATE {
+ float x;
+ float y;
+ float z;
+ float q1;
+ float q2;
+ float q3;
+ float q4;
+};
+
+struct ARAP_COORDINATE_RAW {
int x;
int y;
int z;
View
103 src/arapParser.cpp
@@ -19,6 +19,8 @@ ARAPMessage ARAPParser::constructMessage(ARAPInstruction instruction, int functi
//----------------------
ARAPMessage ARAPParser::constructMoveMessage(ARAP_COORDINATE coord, int velocity, int runSpeed, int functionSuffix, unsigned char moveData){
+ ARAP_COORDINATE_RAW raw = coordinateToRaw(coord);
+
unsigned char data[52];
memset(data, 0, 52);
@@ -31,26 +33,27 @@ ARAPMessage ARAPParser::constructMoveMessage(ARAP_COORDINATE coord, int velocity
data[16-8] = runSpeed >> 8;
data[17-8] = runSpeed & 255;
- data[18-8] = coord.x >> 8;
- data[19-8] = coord.x & 255;
+ data[18-8] = raw.x >> 8;
+ data[19-8] = raw.x & 255;
+
+ data[20-8] = raw.y >> 8;
+ data[21-8] = raw.y & 255;
- data[20-8] = coord.y >> 8;
- data[21-8] = coord.y & 255;
+ data[22-8] = raw.z >> 8;
+ data[23-8] = raw.z & 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[24-8] = raw.q1 >> 8;
+ data[25-8] = raw.q1 & 255;
- data[26-8] = coord.q2 >> 8;
- data[27-8] = coord.q2 & 255;
+ data[26-8] = raw.q2 >> 8;
+ data[27-8] = raw.q2 & 255;
- data[28-8] = coord.q3 >> 8;
- data[29-8] = coord.q3 & 255;
+ data[28-8] = raw.q3 >> 8;
+ data[29-8] = raw.q3 & 255;
- data[30-8] = coord.q4 >> 8;
- data[31-8] = coord.q4 & 255;
+ data[30-8] = raw.q4 >> 8;
+ data[31-8] = raw.q4 & 255;
data[32-8] = 255;
data[33-8] = 255;
@@ -84,13 +87,69 @@ ARAP_STATUS ARAPParser::parseStatusMessage(ARAPMessage msg){
ret.mode = EMERGENCYSTOP;
}
- ret.location.x = (msg.data[18-8]<<8)+msg.data[19-8];
- ret.location.y = (msg.data[20-8]<<8)+msg.data[21-8];
- ret.location.z = (msg.data[22-8]<<8)+msg.data[23-8];
- ret.location.q1 = (msg.data[24-8]<<8)+msg.data[25-8];
- ret.location.q2 = (msg.data[26-8]<<8)+msg.data[27-8];
- ret.location.q3 = (msg.data[28-8]<<8)+msg.data[29-8];
- ret.location.q4 = (msg.data[30-8]<<8)+msg.data[31-8];
+ ARAP_COORDINATE_RAW raw;
+
+ raw.x = (msg.data[18-8]<<8)+msg.data[19-8];
+ raw.y = (msg.data[20-8]<<8)+msg.data[21-8];
+ raw.z = (msg.data[22-8]<<8)+msg.data[23-8];
+ raw.q1 = (msg.data[24-8]<<8)+msg.data[25-8];
+ raw.q2 = (msg.data[26-8]<<8)+msg.data[27-8];
+ raw.q3 = (msg.data[28-8]<<8)+msg.data[29-8];
+ raw.q4 = (msg.data[30-8]<<8)+msg.data[31-8];
+
+ ret.location = coordinateFromRaw(raw);
return ret;
-}
+}
+
+//----------------------
+
+
+
+
+//----------------------
+
+ARAP_COORDINATE coordinateFromRaw(ARAP_COORDINATE_RAW raw){
+ ARAP_COORDINATE ret;
+ ret.x = raw.x * 0.125;
+ ret.y = raw.y * 0.125;
+ ret.z = raw.z * 0.125;
+
+ ret.q1 = raw.q1 * 1.0/16384.0;
+ ret.q2 = raw.q2 * 1.0/16384.0;
+ ret.q3 = raw.q3 * 1.0/16384.0;
+ ret.q4 = raw.q4 * 1.0/16384.0;
+
+ return ret;
+}
+
+//----------------------
+
+
+ARAP_COORDINATE_RAW coordinateToRaw(ARAP_COORDINATE coord){
+ ARAP_COORDINATE_RAW ret;
+ ret.x = coord.x * 1.0/0.125;
+ ret.y = coord.y * 1.0/0.125;
+ ret.z = coord.z * 1.0/0.125;
+
+ ret.q1 = coord.q1 * 16384.0;
+ ret.q2 = coord.q2 * 16384.0;
+ ret.q3 = coord.q3 * 16384.0;
+ ret.q4 = coord.q4 * 16384.0;
+
+ return ret;
+}
+
+/*
+ Q1 = cos i/2
+ Q2 = nx sin i/2
+ Q3 = ny sin i/2
+ Q4 = nz sin i/2
+
+ where n = nx, ny, nz is the orientation of the rotation axis of the wrist coordinate system relative
+ the base coordinate system and i is the rotation angle around n.
+ Before calculating i, the Q1 - 04 should always be multiplied by the scale factor 1/16384 (0,000061).
+
+ Help!
+*/
+
View
3  src/arapParser.h
@@ -57,4 +57,7 @@ class ARAPParser {
ARAPMessage constructMoveMessage(ARAP_COORDINATE coord, int velocity, int runSpeed, int functionSuffix, unsigned char moveData);
ARAP_STATUS parseStatusMessage(ARAPMessage msg);
+
+ ARAP_COORDINATE coordinateFromRaw(ARAP_COORDINATE_RAW raw);
+ ARAP_COORDINATE_RAW coordinateToRaw(ARAP_COORDINATE coord);
};
View
13 src/ofxABBRobot.cpp
@@ -246,11 +246,13 @@ ARAP_PROGRAM ofxABBRobot::receiveProgram(int program){
//----------------------
-void ofxABBRobot::sendMoveMessage(ARAP_COORDINATE coord, int velocity, int runSpeed, int functionSuffix, unsigned char moveData, bool last){
- ARAPMessage msg = parser->constructMoveMessage(coord, velocity, runSpeed, functionSuffix, moveData);
+void ofxABBRobot::sendMoveMessage(ARAP_COORDINATE coord, float velocity, float runSpeed, int functionSuffix, unsigned char moveData, bool last){
+ int velocityRaw = velocity * 1.0/0.125;
+ int runSpeedRaw = runSpeed * 8192;
+ ARAPMessage msg = parser->constructMoveMessage(coord, velocityRaw, runSpeedRaw, functionSuffix, moveData);
if(!last){
- //Create it as multimessage
+ //Create it as multimessage. Not sure if it helps anything.
msg.messageType = multifrompc;
}
commandQuery(msg);
@@ -258,7 +260,7 @@ void ofxABBRobot::sendMoveMessage(ARAP_COORDINATE coord, int velocity, int runSp
//----------------------
-void ofxABBRobot::move(ARAP_COORDINATE coord, int velocity, int runSpeed, bool absolute, bool robotCoordinates){
+void ofxABBRobot::move(ARAP_COORDINATE coord, float velocity, float runSpeed, bool absolute, bool robotCoordinates){
vector<ARAP_COORDINATE> v;
v.push_back(coord);
move(v, velocity, runSpeed, absolute, robotCoordinates);
@@ -266,12 +268,13 @@ void ofxABBRobot::move(ARAP_COORDINATE coord, int velocity, int runSpeed, bool a
//----------------------
-void ofxABBRobot::move(vector<ARAP_COORDINATE> coords, int velocity, int runSpeed, bool absolute, bool robotCoordinates){
+void ofxABBRobot::move(vector<ARAP_COORDINATE> coords, float velocity, float runSpeed, bool absolute, bool robotCoordinates){
unsigned char robotBit = 0;
if(robotCoordinates){
robotBit = 4;
}
+
//Send start point (required by protocol)
sendMoveMessage(coords[0], velocity, runSpeed, 2,2+robotBit, true);
View
6 src/ofxABBRobot.h
@@ -37,8 +37,8 @@ class ofxABBRobot {
ARAP_PROGRAM receiveProgram(int program);
//Ask the robot to move to a specific coordinate. (or vector of coordinates)
- void move(ARAP_COORDINATE coord, int velocity, int runSpeed, bool absolute=true, bool robotCoordinates=false);
- void move(vector<ARAP_COORDINATE> coords, int velocity, int runSpeed, bool absolute=true, bool robotCoordinates=false);
+ void move(ARAP_COORDINATE coord, float velocity, float runSpeed, bool absolute=true, bool robotCoordinates=false);
+ void move(vector<ARAP_COORDINATE> coords, float velocity, float runSpeed, bool absolute=true, bool robotCoordinates=false);
//Returns if the supplied message is a warning or error
bool isErrorMessage(ARAPMessage msg);
@@ -56,6 +56,6 @@ class ofxABBRobot {
//Add message to queue and don't wait for any response
void commandQuery(ARAPMessage msg);
- void sendMoveMessage(ARAP_COORDINATE coord, int velocity, int runSpeed, int functionSuffix, unsigned char moveData, bool last);
+ void sendMoveMessage(ARAP_COORDINATE coord, float velocity, float runSpeed, int functionSuffix, unsigned char moveData, bool last);
long responseCounter;
};
Please sign in to comment.
Something went wrong with that request. Please try again.