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;
}