Example 2: Mobile Agent Deployment
Mobile Code in Agent 2
#include "robot.h" #define SEM 55 #define NUM_RUNS 3 int main(void) { MCAgent_t ma1; ma1 = mc_FindAgentByName("MA-1"); // Change the status of the first agent to neutral if(!ma1) { printf("MA-2: Unable to find agent 'MA-1'\n"); exit(1); } else mc_SetAgentStatus(ma1, MC_AGENT_NEUTRAL); // Wait until the cell resources are free if(mc_SemaphoreWait(SEM)) { printf("MA-2: semaphore error\n"); exit(1); } // Automation Assembly // 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; // 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-2: 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 right above the part ibm_tm[0][3] = ibm_PickupPos[0]; ibm_tm[1][3] = 340.0; ibm_tm[2][3] = -150.0; printf("MA-2: 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-2: PUMA Opening Gripper\n"); status = puma.GripperOpen(); // IBM: Open the gripper printf("MA-2: 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-2: PUMA Closing Gripper\n"); puma.GripperClose(); // IBM: Close the gripper printf("MA-2: 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-2: 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-2: 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-2: 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-2: PUMA Opening Gripper\n"); status = puma.GripperOpen(); // IBM: Open the gripper printf("MA-2: 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-2: 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-2: Freeing up robotic resources\n"); mc_SemaphorePost(SEM); printf("MA-2: Program Complete.\n\n"); return 0; } |
Integration Engineering Laboratory | UCD MTU Sandia |