Hi again Robin,
Great to hear that youâve got the speed control working. This part is a lot more complex, but hopefully explained it well enough. Fingers crossed it works.
Their demo does not include the definition for a lot of things. Worst part, it has no example of receiving any information from the robot. So this is mostly guesswork. I donât have access to the robot at work at the moment, as itâs at the machine shop getting some final touches. Weâre using it to prototype a tool we want, and once itâs finished I probably wonât be doing much with it. It should be back within a week or two, so if this doesnât work then I will an opportunity to test some of this. Iâd make sure to have a backup of your current working files before trying this.
First we need to add this struct into the command.h file, anywhere along with the other typedef structs will work.
typedef struct tagPose {
float x;
float y;
float z;
float r;
float jointAngle[ROBOT_AXIS];
} Pose __attribute__ ((aligned(4)));
We also need to add a declaration for the GetPose command into command.h. This will be after #pragma pack(pop), alongside the other command declarations.
/*********************************************************************************************************
** Pose
*********************************************************************************************************/
extern int GetPose(Pose *pose);
Finally, we need to add the actual command into command.cpp.
/*********************************************************************************************************
** Function name: GetPose
** Descriptions: Get Robot Pose
** Input parameters: Pose structure
** Output parameters: none
** Returned value: true
*********************************************************************************************************/
int GetPose(Pose *pose)
{
Message tempMessage;
memset(&tempMessage, 0, sizeof(Message));
tempMessage.id = ProtocolGetPose;
tempMessage.rw = false;
tempMessage.isQueued = false;
tempMessage.paramsLen = 0;
memcpy(tempMessage.params, (uint8_t *)ptpCommonParams, tempMessage.paramsLen);
MessageWrite(&gSerialProtocolHandler, &tempMessage);
memcpy(pose, (void *)gParamsPointer, sizeof(Pose));
return true;
}
With some luck, that will have added the command and with luck it works. Iâm not 100% certain, as their demo files are written differently than the actual library files. Now, to access it youâll need to add a structure to hold the information. Iâm using the variable ârobotPoseâ. At the beginning where you declare your variables add this.
Pose robotPose;
Now, in your code where you want to check the pose you add this.
GetPose(&robotPose);
ProtocolProcess();
x = robotPose.x; // x, y, z and r variables will have to be defined somewhere in your program
y = robotPose.y; // Again these can be whatever name you choose. The x, y, z and r after robotPose
z = robotPose.z; // have to be that as it's what they're defined as in the struct we're using.
r = robotPose.r; // axis0 - axis3 are also whatever variable you choose them to be. I don't bother
axis0 = robotPose.jointAngle[0]; // getting the jointAngles as I'm mostly interested in the robot being at the coordinates
axis1 = robotPose.jointAngle[1]; // I specified. You only need to get the ones you need.
axis2 = robotPose.jointAngle[2];
axis3 = robotPose.jointAngle[3];
If youâre checking the coordinates, be sure to give it tolerance for some small amount of error. This is the exact subroutine from my program. Iâve created a struct called coordinates that is just 4 floats (x, y, z, and r). The offset allows me to move only in âzâ and still be able to check my position. So, it calls this routine with a variable âstationâ (of type coordinates) which is the same variable I sent to my move command and an offset. It then checks the position against where I told it to go.
/*********************************************************************************************************
** Function name: waitForRobot
** Descriptions: Checks pose status of robot and waits for it to get to position called for
** Input parameters: Position to wait for and offset
** Output parameters: none
** Returned value: none
*********************************************************************************************************/
void waitForRobot(coordinates station, float offset){
float error[4];
error[4] = 100;
Pose pose;
while(error[4]>=.8){
GetPose(&pose);
error[0] = station.x - pose.x;
error[1] = station.y - pose.y;
error[2] = station.z - (pose.z - offset);
error[3] = station.r - pose.r;
error[4] = abs(error[0]) + abs(error[1]) + abs(error[2]) + abs(error[3]);
}
delay(1000);
}
I wish I could guarantee this will work, but having to do all this from just reading code with no tests at the moment. Whether it works or not, Iâll get a copy of my modified version of their library uploaded this weekend and try and point out some of the differences in the way it works. You can see from my subroutine that I donât have to call ProtocolProcess after I call GetPose. One of the differences, along with a lot more commands that have been written and will work. Also, it cleans up your program space by having your program be the only thing in the sketch and the library routines for the robot in the library folder as they should be.
Kevin