|
Example 2: Mobile Agent Deployment
Mobile Code in Agent 1
#include "robot.h"
#define SEM 55
#define NUM_RUNS 1
int main(void)
{
// IBM Variables
array double
ibm_jold[4] = {0,0,0,0},
ibm_jnew[4],
ibm_tm[4][4],
ibm_etm[4][4],
ibm_ready2[4] = {120.0,143.0,-0.21,0},
ibm_PickupPos[2] = {319, 387};
// IBM 7575 Configuration
int ibm_conf = FLIP;
// PUMA Variables
array double
puma_jold[6] = {0,0,0,0,0,0},
puma_jnew[6],
puma_tm[4][4],
puma_etm[4][4],
puma_DropoffPos[1] = {-643};
// Puma 560 Configuration
int puma_conf = FLIP|ABOVE|RIGHT;
// Setup Robot System Semaphore
int sem_id;
sem_id = mc_SyncInit(SEM);
if(sem_id != SEM)
{
printf("MA-1: Error while intiating semaphore\n");
exit(1);
}
// Drop off transformation matrix
array double puma_DropoffTM[4][4] =
{-0.996195, -0.087156, -0.000000, 270.00,
-0.087156, 0.996195, 0.000000, -646.00,
-0.000000, -0.000000, -1.000000, -200.00,
0.000000, 0.000000, 0.000000, 1.00};
// Pickup transformation matrix
array double puma_PickupTM[4][4] =
{0.358368, 0.933580, 0.000000, -653.00,
0.933580, -0.358368, 0.000000, 315.00,
0.000000, 0.000000, -1.000000, -250.00,
0.000000, 0.000000, 0.000000, 1.00};
// Conveyer Variables
array double
conveyorPos[2] = {427, 200},
conveyorDisp[1] = {0};
// Helper variables
int status, j;
class CRobot ibm = CRobot(ROBOT2, RUN_REALTIME),
puma = CRobot(ROBOT1, RUN_REALTIME),
conveyor = CRobot(CONVEYOR1, RUN_REALTIME);
status = puma.Calibrate();
status = ibm.Calibrate();
status = conveyor.Calibrate();
// Move conveyor to initial position
conveyorDisp[0] = conveyorPos[0];
status = conveyor.Drive(conveyorDisp);
// Parts are at 200 mm intervals
conveyorDisp[0] = conveyorPos[1];
status = ibm.Drive(ibm_ready2);
status = ibm.MoveWait();
status = ibm.MoveReady();
status = ibm.MoveWait();
while(1)
{
conveyor.Drive(conveyorDisp);
puma_tm = puma_PickupTM;
puma.InverseKinematics(puma_tm,puma_jold,puma_jnew,puma_conf);
puma.Drive(puma_jnew);
ibm_tm=robot2ZeroPosition;
ibm_tm[0][0] = cos(110*3.1415/180);
ibm_tm[0][1] = -sin(110*3.1415/180);
ibm_tm[1][0] = sin(110*3.1415/180);
ibm_tm[1][1] = cos(110*3.1415/180);
ibm_tm[0][3] = ibm_PickupPos[0];
ibm_tm[1][3] = 340.0;
ibm_tm[2][3] = -20.0;
printf("MA-1: IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
ibm_tm[0][3], ibm_tm[1][3], ibm_tm[2][3]);
ibm.InverseKinematics(ibm_tm, ibm_jold, ibm_jnew, ibm_conf);
ibm.Drive(ibm_jnew);
//ALL: Wait untill move complete
puma.MoveWait();
ibm.MoveWait();
conveyor.MoveWait();
// IBM: Drop Z down to the correct height
ibm_tm[0][3] = ibm_PickupPos[0];
ibm_tm[1][3] = 340.0;
ibm_tm[2][3] = -150.0;
printf("MA-1: IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
ibm_tm[0][3], ibm_tm[1][3], ibm_tm[2][3]);
ibm.InverseKinematics(ibm_tm, ibm_jold, ibm_jnew, ibm_conf);
ibm.Drive(ibm_jnew);
ibm.MoveWait();
// PUMA: Open the gripper
printf("MA-1: PUMA Opening Gripper\n");
status = puma.GripperOpen();
// IBM: Open the gripper
printf("MA-1: IBM Opening Gripper\n");
status = ibm.GripperOpen();
// PUMA: Slowly move the gipper into position
puma_etm = puma_PickupTM;
puma_etm[2][3] = -310.00;
puma.MoveLine(puma_tm, puma_etm, 8, puma_conf);
// IBM: Slowly move the gipper into position
ibm_tm[0][3] = ibm_PickupPos[0];
ibm_tm[1][3] = 340.0;
ibm_tm[2][3] = -150.0;
ibm_etm = ibm_tm;
ibm_etm[1][3] = ibm_PickupPos[1];
ibm.MoveLine(ibm_tm, ibm_etm, 10, ibm_conf);
// PUMA: Close the gripper
printf("MA-1: PUMA Closing Gripper\n");
puma.GripperClose();
// IBM: Close the gripper
printf("MA-1: IBM Closing Gripper\n");
status = ibm.GripperClose();
// PUMA: Move the part up slowly
puma_tm = puma_etm;
puma_etm[2][3] = -280.00;
puma.MoveLine(puma_tm, puma_etm, 5, puma_conf);
puma.MoveWait();
// PUMA: Move the part up
puma_tm[2][3] = 0;
puma.InverseKinematics(puma_tm,puma_jold,puma_jnew,puma_conf);
puma.Drive(puma_jnew);
puma.MoveWait();
// PUMA: Move to intermediate position
puma_tm = puma_DropoffTM;
puma_tm[0][3] = -400;
puma_tm[1][3] = -300;
puma_tm[2][3] = 0;
puma.InverseKinematics(puma_tm,puma_jold,puma_jnew,puma_conf);
puma.Drive(puma_jnew);
// IBM: Move the part up
ibm_tm = ibm_etm;
ibm_tm[2][3] = -20.0;
printf("MA-1: IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
ibm_tm[0][3], ibm_tm[1][3], ibm_tm[2][3]);
ibm.InverseKinematics(ibm_tm, ibm_jold, ibm_jnew, ibm_conf);
ibm.Drive(ibm_jnew);
// ALL: Wait for move complete
puma.MoveWait();
ibm.MoveWait();
// PUMA: Move the part to the drop off position
puma_tm = puma_DropoffTM;
puma_tm[1][3] = puma_DropoffPos[0];
puma.InverseKinematics(puma_tm,puma_jold,puma_jnew,puma_conf);
puma.Drive(puma_jnew);
// IBM: Move the part to the drop off position
ibm_tm=robot2ZeroPosition;
ibm_tm[0][0] = cos(20*3.1415/180);
ibm_tm[0][1] = -sin(20*3.1415/180);
ibm_tm[1][0] = sin(20*3.1415/180);
ibm_tm[1][1] = cos(20*3.1415/180);
ibm_tm[0][3] = 347.0;
ibm_tm[1][3] = -405.0;
ibm_tm[2][3] = -20.0;
printf("MA-1: IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
ibm_tm[0][3], ibm_tm[1][3], ibm_tm[2][3]);
ibm.InverseKinematics(ibm_tm, ibm_jold, ibm_jnew, ibm_conf);
ibm.Drive(ibm_jnew);
// ALL: Wait for move complete
puma.MoveWait();
ibm.MoveWait();
// PUMA: Slowly move the part down to drop off
puma_etm = puma_tm;
puma_etm[2][3] = -255.00;
puma.MoveLine(puma_tm, puma_etm, 8, puma_conf);
// IBM: Move the part down to drop off
ibm_tm[2][3] = -92.0;
printf("MA-1: IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
ibm_tm[0][3], ibm_tm[1][3], ibm_tm[2][3]);
ibm.InverseKinematics(ibm_tm, ibm_jold, ibm_jnew, ibm_conf);
ibm.Drive(ibm_jnew);
// ALL: Wait for move complete
puma.MoveWait();
ibm.MoveWait();
// PUMA: Open the gripper
printf("MA-1: PUMA Opening Gripper\n");
status = puma.GripperOpen();
// IBM: Open the gripper
printf("MA-1: IBM Opening Gripper\n");
status = ibm.GripperOpen();
// PUMA: Slowly move the gripper away from the dropped part
puma_tm = puma_etm;
puma_etm[2][3] = -200.00;
puma.MoveLine(puma_tm, puma_etm, 8, puma_conf);
puma.MoveWait();
// PUMA: Move to the intermediat position
puma_tm = puma_DropoffTM;
puma_tm[0][3] = -400;
puma_tm[1][3] = -300;
puma_tm[2][3] = 0;
puma.InverseKinematics(puma_tm,puma_jold,puma_jnew,puma_conf);
puma.Drive(puma_jnew);
// IBM: Move the gripper away from the dropped part
ibm_tm[2][3] = -20.0;
printf("MA-1: IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
ibm_tm[0][3], ibm_tm[1][3], ibm_tm[2][3]);
ibm.InverseKinematics(ibm_tm, ibm_jold, ibm_jnew, ibm_conf);
ibm.Drive(ibm_jnew);
// ALL: Wait until move complete
puma.MoveWait();
ibm.MoveWait();
// Check the current status of the this agent
if(mc_GetAgentStatus(mc_current_agent) == MC_AGENT_NEUTRAL)
break;
}
// PUMA: Move to ready position
status = puma.MoveReady();
// IBM: move to modified ready position
// ibm_ready2 position is ready position with Z = -50
status = ibm.Drive(ibm_ready2);
status = ibm.MoveWait();
// IBM: move to ready position
status = ibm.MoveReady();
// ALL: Move wait
status = puma.MoveWait();
status = ibm.MoveWait();
// ALL: Close the gripper
status = puma.GripperClose();
status = ibm.GripperClose();
// ALL: Disable
status = puma.Disable();
status = ibm.Disable();
status = conveyor.Disable();
printf("MA-1: Freeing up robotic resources\n");
mc_SemaphorePost(sem_id);
printf("MA-1: Program Complete.\n\n");
return 0;
}
|
| Integration Engineering Laboratory | UCD MTU Sandia |