Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

Added plane correction function, and improved speed

  • Loading branch information...
commit 1cc4d18a83f25cafa6cbcd31530dd297ebc001ee 1 parent 1d3eb36
@HalfdanJ HalfdanJ authored
View
43 src/adlpCom.cpp
@@ -123,6 +123,11 @@ void ADLPCom::threadedFunction(){
message[3] = 0; //Mesesage from PC to controller
message[4] = msg.instruction; // Instruction
message[5] = msg.messageType;
+
+ if(msg.ort == 1){
+ message[5] = msg.messageType + 0x10;
+ }
+
message[6] = 0; //function suffix << 8 TODO, but never used
message[7] = msg.functionSuffix; //function suffix
for(int i=0;i<msg.size;i++){
@@ -144,19 +149,30 @@ void ADLPCom::threadedFunction(){
}
//Write the data
- serial.writeByte(DLE);
- serial.writeByte((msg.size % 2)? STXodd : STXeven);
- serial.writeBytes(message, msg.size+8);
- serial.writeByte(DLE);
- serial.writeByte(ETX);
- serial.writeByte(checksum ^ ETX);
+ lastSending.clear();
+
+ lastSending.push_back(DLE);
+ lastSending.push_back((msg.size % 2)? STXodd : STXeven);
+ for(int i=0;i<msg.size+8;i++){
+ lastSending.push_back(message[i]);
+ }
+ lastSending.push_back(DLE);
+ lastSending.push_back(ETX);
+ lastSending.push_back(checksum ^ ETX);
+
+ for(int i=0;i<lastSending.size();i++){
+ // printf("%x - ",lastSending[i]);
+ serial.writeByte(lastSending[i]);
+ }
if(msg.messageType == query){
waitingForResponse = true;
}
+
+
}
}
unlock();
- usleep(1000*100);
+ usleep(1000*1);
}
}
}
@@ -164,7 +180,14 @@ void ADLPCom::threadedFunction(){
//----------------------
void ADLPCom::parseIncommingByte(unsigned char bytesReturned){
- if(bytesReturned == ACK && sendingMessage){
+ if(bytesReturned == NAK && sendingMessage){
+ cout<<"Resending packet"<<endl;
+ for(int i=0;i<lastSending.size();i++){
+ // printf("%x - ",lastSending[i]);
+ serial.writeByte(lastSending[i]);
+ }
+ }
+ else if(bytesReturned == ACK && sendingMessage){
if(outgoingFormalities[0] && outgoingFormalities[1]){
//Sending was succesfull
if(!sendingMultimessage){
@@ -189,7 +212,7 @@ void ADLPCom::parseIncommingByte(unsigned char bytesReturned){
//We are processing a message content that is incomming
switch (incommingMessageIndex) {
case 0: //Not used
-
+
break;
case 1: //Size of telegram
{
@@ -309,7 +332,7 @@ void ADLPCom::parseIncommingByte(unsigned char bytesReturned){
else if(incommingMessageIndex >= 0 && incommingFormalities[0] && incommingFormalities[1] && incommingFormalities[2] && incommingFormalities[3] && incommingFormalities[4]){
if(ADLP_DEBUG) printf("Recevied checksum %x\n",bytesReturned);
-
+
//All formalities met, check cheksum (TODO)
serial.writeByte(ACK);
receivingMessage = false;
View
1  src/adlpCom.h
@@ -62,6 +62,7 @@ class ADLPCom : public ofxThread{
int multimessageDataIndex;
bool sendingMultimessage;
bool readyToSendNextMultiMessage;
+ vector<unsigned char> lastSending;
//The loop for the serial stuff
void threadedFunction();
View
17 src/arapParser.cpp
@@ -55,10 +55,10 @@ ARAPMessage ARAPParser::constructMoveMessage(ARAP_COORDINATE coord, int velocity
data[30-8] = raw.q4 >> 8;
data[31-8] = raw.q4 & 255;
- data[32-8] = 255;
+/* data[32-8] = 255;
data[33-8] = 255;
data[34-8] = 250;
- data[35-8] = 220;
+ data[35-8] = 220;*/
ARAPMessage msg = constructMessage(MOVE, functionSuffix, data, 52);
return msg;
@@ -109,12 +109,21 @@ ARAP_STATUS ARAPParser::parseStatusMessage(ARAPMessage msg){
//----------------------
-ARAP_COORDINATE coordinateFromRaw(ARAP_COORDINATE_RAW raw){
+ARAP_COORDINATE ARAPParser::coordinateFromRaw(ARAP_COORDINATE_RAW raw){
ARAP_COORDINATE ret;
+ if(raw.x > 32767)
+ raw.x -= 2*32767;
+ if(raw.y > 32767)
+ raw.y -= 2*32767;
+ if(raw.z > 32767)
+ raw.z -= 2*32767;
+
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;
@@ -126,7 +135,7 @@ ARAP_COORDINATE coordinateFromRaw(ARAP_COORDINATE_RAW raw){
//----------------------
-ARAP_COORDINATE_RAW coordinateToRaw(ARAP_COORDINATE coord){
+ARAP_COORDINATE_RAW ARAPParser::coordinateToRaw(ARAP_COORDINATE coord){
ARAP_COORDINATE_RAW ret;
ret.x = coord.x * 1.0/0.125;
ret.y = coord.y * 1.0/0.125;
View
1  src/arapParser.h
@@ -47,6 +47,7 @@ struct ARAPMessage {
ARAPMessageType messageType;
int functionSuffix;
unsigned char * data;
+ int ort;
};
View
37 src/coordinateHandler.cpp
@@ -0,0 +1,37 @@
+#include "coordinateHandler.h"
+
+CoordinateHandler::CoordinateHandler(){
+
+}
+
+void CoordinateHandler::setCalibrationCorner(int corner, ARAP_COORDINATE coord){
+ corners[corner].x = coord.x;
+ corners[corner].y = coord.y;
+ corners[corner].z = coord.z;
+
+ if(corner == 0){
+ dir = ofxQuaternion(coord.q1, coord.q2, coord.q3, coord.q4);
+ }
+}
+
+ARAP_COORDINATE CoordinateHandler::convertToWorld(float x, float y, float z){
+ ARAP_COORDINATE ret;
+
+ ofxVec3f xdir = (corners[1] - corners[0]).normalized();
+ ofxVec3f ydir = (corners[2] - corners[0]).normalized();
+
+ ofxVec3f zdir = ydir.crossed(xdir);
+
+ ofxVec3f v = xdir*x + ydir*y + zdir*z;
+
+ ret.x = corners[0].x + v.x;
+ ret.y = corners[0].y + v.y;
+ ret.z = corners[0].z + v.z;
+
+ ret.q1 = dir[0];
+ ret.q2 = dir[1];
+ ret.q3 = dir[2];
+ ret.q4 = dir[3];
+
+ return ret;
+}
View
15 src/coordinateHandler.h
@@ -0,0 +1,15 @@
+#pragma once
+#include "ofxVectorMath.h"
+#include "arapInstructions.h"
+
+class CoordinateHandler {
+public:
+ CoordinateHandler();
+ void setCalibrationCorner(int corner, ARAP_COORDINATE coord);
+ ARAP_COORDINATE convertToWorld(float x, float y, float z);
+
+
+ ofxVec3f corners[3];
+
+ ofxQuaternion dir;
+};
View
109 src/ofxABBRobot.cpp
@@ -110,6 +110,39 @@ ofxABBRobot::ofxABBRobot(){
//Construct the sub objects
parser = new ARAPParser();
com = new ADLPCom();
+ coordHandler = new CoordinateHandler();
+
+ xml = new ofxXmlSettings();
+
+ //Settings loading
+ bool xmlLoaded = xml->loadFile("settings.xml");
+ if(xmlLoaded){
+ cout<<"Settings loaded"<<endl;
+ } else {
+ cout<<"Settings XML NOT loaded! "<<ofToDataPath("settings.xml")<<endl;
+ }
+
+ if(xmlLoaded){
+ for(int i=0;i<3;i++){
+ xml->pushTag("CALIBRATION",0);
+ xml->pushTag("CORNER",i);
+ ARAP_COORDINATE coord;
+ coord.x = xml->getValue("X", 0.0);
+ coord.y = xml->getValue("Y", 0.0);
+ coord.z = xml->getValue("Z", 0.0);
+ coord.q1 = xml->getValue("Q1", 0.0);
+ coord.q2 = xml->getValue("Q2", 0.0);
+ coord.q3 = xml->getValue("Q3", 0.0);
+ coord.q4 = xml->getValue("Q4", 0.0);
+
+ coordHandler->setCalibrationCorner(i, coord);
+
+ xml->popTag();
+ xml->popTag();
+ }
+ }
+
+
}
//----------------------
@@ -202,7 +235,8 @@ void ofxABBRobot::writeMode(ARAP_MODE mode){
ARAP_STATUS ofxABBRobot::readStatus(bool async){
ARAPMessage msg = parser->constructMessage(READSTATUS);
-
+ //msg.ort = 1;
+
ARAPMessage response;
bool gotResponse = false;
if(!async){
@@ -275,8 +309,17 @@ void ofxABBRobot::move(vector<ARAP_COORDINATE> coords, float velocity, float run
}
+ ARAP_COORDINATE nulCord;
+ nulCord.x = 0;
+ nulCord.y = 0;
+ nulCord.z = 0;
+ nulCord.q1 = 0;
+ nulCord.q2 = 0;
+ nulCord.q3 = 0;
+ nulCord.q4 = 0;
+
//Send start point (required by protocol)
- sendMoveMessage(coords[0], velocity, runSpeed, 2,2+robotBit, true);
+ sendMoveMessage(nulCord, velocity, runSpeed, 2,2+robotBit, true);
for(int i=0;i<coords.size();i++){
sendMoveMessage(coords[i], velocity, runSpeed, 0, 0+robotBit, true);
@@ -287,6 +330,20 @@ void ofxABBRobot::move(vector<ARAP_COORDINATE> coords, float velocity, float run
sendMoveMessage(endpoint, velocity, runSpeed, 3, 2+robotBit, true);
}
+//----------------------
+
+void ofxABBRobot::movePlane(vector<ofxVec3f> coords, float velocity, bool robotCord, float runSpeed){
+ vector<ARAP_COORDINATE> worldCoords;
+
+ for(int i=0;i<coords.size();i++){
+ worldCoords.push_back(coordHandler->convertToWorld(coords[i][0],coords[i][1],coords[i][2]));
+//
+// cout<<"worldcoord: "<<worldCoords[i].x<<" "<<worldCoords[i].y<<" "<<worldCoords[i].z<<" "<<worldCoords[i].q1<<" "<<worldCoords[i].q2<<" "<<worldCoords[i].q3<<" "<<worldCoords[i].q4<<endl;
+ }
+
+ move(worldCoords, velocity, runSpeed, true, robotCord);
+}
+
#pragma mark private calls
//----------------------
@@ -313,7 +370,7 @@ ARAPMessage ofxABBRobot::responseSyncQuery(ARAPMessage msg){
}
//Lets sleep a bit before checking again (to not hang on the lock)
if(!gotResponse)
- usleep(1000*100);
+ usleep(1000*10);
}
return response;
}
@@ -324,6 +381,52 @@ void ofxABBRobot::commandQuery(ARAPMessage msg){
com->queueMessage(msg);
}
+//----------------------
+
+void ofxABBRobot::storeCalibrationCorner(int corner){
+ ARAP_STATUS status = readStatus();
+
+ xml->pushTag("CALIBRATION",0);
+ xml->pushTag("CORNER",corner);
+ xml->setValue("X", status.location.x,0);
+ xml->setValue("Y", status.location.y,0);
+ xml->setValue("Z", status.location.z,0);
+
+ xml->setValue("Q1", status.location.q1,0);
+ xml->setValue("Q2", status.location.q2,0);
+ xml->setValue("Q3", status.location.q3,0);
+ xml->setValue("Q4", status.location.q4,0);
+ xml->popTag();
+ xml->popTag();
+
+ xml->saveFile("settings.xml");
+
+ coordHandler->setCalibrationCorner(corner, status.location);
+}
+
+
+//----------------------
+
+void ofxABBRobot::runDrawing(float speed){
+ drawing = new ofxXmlSettings();
+ drawing->loadFile("drawing.xml");
+ drawing->pushTag("XML",0);
+ int count = drawing->getNumTags("POINT");
+
+ vector<ofxVec3f> coords;
+
+ for(int i=0;i<count;i++){
+ drawing->pushTag("POINT",i);
+ float z = 20;
+ if(drawing->getValue("tool", 0.0)==1)
+ z = 0;
+ coords.push_back(ofxVec3f(drawing->getValue("x", 0.0), drawing->getValue("y", 0.0), z));
+ drawing->popTag();
+ }
+ drawing->popTag();
+
+ movePlane(coords, 100);
+}
View
27 src/ofxABBRobot.h
@@ -9,6 +9,9 @@
#include "adlpCom.h"
#include "arapParser.h"
#include "arapInstructions.h"
+#include "ofxXmlSettings.h"
+#include "ofxVectorMath.h"
+#include "coordinateHandler.h"
class ofxABBRobot {
public:
@@ -18,7 +21,8 @@ class ofxABBRobot {
ADLPCom * com;
//This is for creating ARAP messages correctly
ARAPParser * parser;
-
+ //This is coordinating the 3d space
+ CoordinateHandler * coordHandler;
//Tell the robot to start a program
@@ -36,9 +40,11 @@ class ofxABBRobot {
//Download a program (binary)
ARAP_PROGRAM receiveProgram(int program);
- //Ask the robot to move to a specific coordinate. (or vector of coordinates)
- 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);
+ //Ask the robot to move to a specific coordinate. (or vector of coordinates) unit is mm
+ void move(ARAP_COORDINATE coord, float velocity, float runSpeed=1, bool absolute=true, bool robotCoordinates=false);
+ void move(vector<ARAP_COORDINATE> coords, float velocity, float runSpeed=1, bool absolute=true, bool robotCoordinates=false);
+ //Move in the calibrated plane
+ void movePlane(vector<ofxVec3f> coords, float velocity, bool robotCord = false, float runSpeed=1);
//Returns if the supplied message is a warning or error
bool isErrorMessage(ARAPMessage msg);
@@ -47,7 +53,15 @@ class ofxABBRobot {
//Creates a human readable string from an error or warning message
string errorMessageToString(ARAPMessage msg);
string warningMessageToString(ARAPMessage msg);
-
+
+ //Move the arm to one of the corners, and call thsi function to calibrate and save the data
+ //Corner 0 is the reference point (top left)
+ //Corner 1 is the x axis (top right)
+ //Corner 2 is the y axis (bottom left)
+ void storeCalibrationCorner(int corner);
+
+ //Run the drawing code from drawing.xml (in the data dir)
+ void runDrawing(float speed);
private:
//Add a message to the queue, and wait for response message
@@ -58,4 +72,7 @@ class ofxABBRobot {
void sendMoveMessage(ARAP_COORDINATE coord, float velocity, float runSpeed, int functionSuffix, unsigned char moveData, bool last);
long responseCounter;
+
+ ofxXmlSettings * xml;
+ ofxXmlSettings * drawing;
};
Please sign in to comment.
Something went wrong with that request. Please try again.