#include
#include "robot.h"
#define X_PXCENTER 455.1459
#define Y_PXCENTER 322.8292
#define CAMALPHA (-.1271)
int getObjPos(array short objPXLoc[2]);
int calObjPos(array double ja[4], array short objPXLoc[2], array double objPos[2]);
int main(void)
{
array double jold[4] = {0,0,0,0},
jnew[4],
trans_matrix[4][4],
end_tmatrix[4][4],
ready2[4] = {120.0, 143.0, -50.0, 0.0},
visualAng[4]= {160.0, 210.0, -50.0, -110.0},
objPos[2]={0};
array short objPXLoc[2]={0};
int status,
j,
ibm_conf = NOFLIP;
char ans;
// Instanciate CRobot class
class CRobot ibm = CRobot(ROBOT2, RUN_REALTIME);
// Calibrate IBM robot and conveyor
status = ibm.Calibrate();
// Move IBM robot to ready position
// ready2 position is ready position with Z = -50
status = ibm.Drive(ready2);
status = ibm.MoveWait();
while(1)
{
// Move IBM to visually localize part
ibm.Drive(visualAng);
jold = visualAng;
ibm.MoveWait();
// Get Object Position
getObjPXLoc(objPXLoc);
calObjPos(jold, objPXLoc, objPos);
printf("AGENT: Calculated Obj Position: %0.4f\n", objPos);
// Move IBM to Initial MoveLine Position
trans_matrix=robot2ZeroPosition;
trans_matrix[0][0] = cos(110*3.1415/180);
trans_matrix[0][1] = -sin(110*3.1415/180);
trans_matrix[1][0] = sin(110*3.1415/180);
trans_matrix[1][1] = cos(110*3.1415/180);
trans_matrix[0][3] = objPos[0];
trans_matrix[1][3] = objPos[1]-40;
trans_matrix[2][3] = jold[2];
printf("IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
trans_matrix[0][3], trans_matrix[1][3], trans_matrix[2][3]);
ibm.InverseKinematics(trans_matrix, jold, jnew, ibm_conf);
ibm.Drive(jnew);
ibm.MoveWait();
// Drop the Z axis
trans_matrix[2][3] = -150.0;
printf("IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
trans_matrix[0][3], trans_matrix[1][3], trans_matrix[2][3]);
ibm.InverseKinematics(trans_matrix, jold, jnew, ibm_conf);
ibm.Drive(jnew);
ibm.MoveWait();
// Open IBM robot gripper
printf("IBM Opening Gripper\n");
status = ibm.GripperOpen();
// calculate required movement for desired position
end_tmatrix = trans_matrix;
end_tmatrix[1][3] = objPos[1];
ibm.MoveLine(trans_matrix, end_tmatrix, 10, ibm_conf);
// Close IBM robot gripper
printf("IBM Closing Gripper\n");
status = ibm.GripperClose();
// Bring the Z axis up
trans_matrix = end_tmatrix;
trans_matrix[2][3] = -20.0;
printf("IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
trans_matrix[0][3], trans_matrix[1][3], trans_matrix[2][3]);
ibm.InverseKinematics(trans_matrix, jold, jnew, ibm_conf);
ibm.Drive(jnew);
ibm.MoveWait();
// Move to Ready 2 Position
jold = ready2;
ibm.Drive(jold);
ibm.MoveWait();
// Open IBM robot gripper
printf("Repeat (Y/N)?\n");
ans = getc(stdin);
if(ans != 'Y' && ans != 'y')
break;
}
// Move IBM robot to ready position
// ready2 position is ready position with Z = -50
status = ibm.Drive(ready2);
status = ibm.MoveWait();
status = ibm.MoveReady();
status = ibm.MoveWait();
// Close IBM robot gripper
status = ibm.GripperClose();
// Disable IBM robot
status = ibm.Disable();
printf("Program Complete.\n\n");
return 0;
}
int getObjPXLoc(array short objPXLoc[2])
{
short* data;
printf("AGENT:Triggering the vision agent now.\n");
mc_CondSignal(42);
printf("AGENT:Waiting for a message.\n");
data = (short *)mc_AclWaitRetrieve(mc_current_agent);
objPXLoc[0] = data[0];
objPXLoc[1] = data[1];
printf("AGENT:Got the point: ");
printf("%d %d\n", data[0], data[1]);
return 0;
}
int calObjPos(array double ja[4], array short objPXLoc[2], array double objPos[2])
{
array double alpha[4] = {120, 137, 0, 0};
array double joints[4] = ((double)(3.1415/180.0))*(ja-alpha);
array double
A1[4][4] =
{{cos(joints[0]), -sin(joints[0]), 0, 0},
{sin(joints[0]), cos(joints[0]), 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}},
A2[4][4] =
{{cos(joints[1]), -sin(joints[1]), 0, 325.0},
{sin(joints[1]), cos(joints[1]), 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}},
A3[4][4] =
{{cos(CAMALPHA), -sin(CAMALPHA), 0, 225.0},
{sin(CAMALPHA), cos(CAMALPHA), 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}},
P[4][4] =
{{1, 0, 0, (X_PXCENTER-objPXLoc[1])*60.0/265.1208},
{0, 1, 0, -(Y_PXCENTER-objPXLoc[0])*60.0/265.1208},
{0, 0, 1, 0},
{0, 0, 0, 1}},
A4[4][4] = A1*A2*A3*P;
printf("AGENT: DIF: %0.6f %0.6f\n", P[0][3], P[1][3]);
printf("AGENT: OBJ: %0.6f %0.6f\n", A4[0][3], A4[1][3]);
objPos[0] = A4[0][3];
objPos[1] = A4[1][3];
A4 = A1*A2*A3;
printf("AGENT: Ref position: %.6f %.6f\n", A4[0][3], A4[1][3]);
return 0;
}
