Application: Robot Cell Agent Code
/**************************************************
*  Program Name:  workcell-pickup.ch
*
*  Description:
*    This program demostrates the coordinated 
*    motion of the IBM 7575 4 Axis Robotic Arm,
*    the Puma 560 6 Axis Robot Arm and a conveyor 
*    system.
*
* Author:  Stephen S. Nestinger, April 5, 2007
***************************************************/
#include "robot.h"

#define NUM_RUNS 3

int main(void) 
{
   // IBM Variables
   array double 

     // Old joint angle
     //   used with inverse kinematics, the angles the robot is at
     ibm_jold[4]         = {0,0,0,0},

     // New joint angle
     //   used with inverse kinematics, the angles the robot will be driven to
     ibm_jnew[4],

     // Puma transformation matrix used for single translation
     ibm_tm[4][4],

     // Puma end transformation matrix - used with MoveLine()
     ibm_etm[4][4],

     // Modified ready position, lab specific
     //   Done so that the robot does not hit the conveyor system when
     //   moving to the ready position from the home position
     ibm_ready2[4]       = {120.0,143.0,-0.21,0},

     // X and Y coordinates for IBM pickup up
     ibm_PickupPos[3][2] = {319,   387, 
                            271.5, 370, 
                            224,   363};
   // IBM 7575 Configuration
   //   Uses bitwise operation to setup configuration
   int ibm_conf = FLIP;

   // PUMA Variables
   array double 

     // Old joint angle
     //   used with inverse kinematics, the angles the robot is at
     puma_jold[6]       = {0,0,0,0,0,0},

     // New joint angle
     //   used with inverse kinematics, the angles the robot will be driven to
     puma_jnew[6],

     // Puma transformation matrix used for single translation
     puma_tm[4][4],

     // Puma end transformation matrix - used with MoveLine()
     puma_etm[4][4],

     // X coordinate for Puma drop off of parts, Y position is the same
     puma_DropoffPos[3] = {-643, -702, -765};

   // Puma 560 Configuration
   //   Uses bitwise operation to setup configuration
   int puma_conf = FLIP|ABOVE|RIGHT;

   // Drop off transformation matrix
   //   Contains orientation and position
   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
   //   Contains orientation and position
   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 
     // The 3 displacements for each part dropoff and pickup
     conveyorPos[3]      = {427, 203, 200},

     // Displacement array used with Drive()
     conveyorDisp[1]     = {0};

   // Helper variables
   int status, j;	

   // We do IBM first because of the Z axis
   //   When we release the break, the axis will fall
   //   If there is a following error in PEWIN32,
   //     restart this prog.
   //   Only enable the robot arm when following error = 0
   //     on all axes
   // Instanciate CRobot class 
   //
   // The constructor will initialize each robot.
   class CRobot  ibm = CRobot(ROBOT2,    RUN_REALTIME),
                puma = CRobot(ROBOT1,    RUN_REALTIME),
            conveyor = CRobot(CONVEYOR1, RUN_REALTIME);

   
   // Calibrate Robots
   //   This includes homing each robot and calibrate the Puma based
   //   on potentiometer feedback
   status = puma.Calibrate();    
   status = ibm.Calibrate();    
   status = conveyor.Calibrate();    
   
   // Move IBM robot to modified ready position 
   //   ibm_ready2 position is standard ready position with Z = -50
   status = ibm.Drive(ibm_ready2); 
   status = ibm.MoveWait(); 

   // Move IBM robot to ready position 
   status = ibm.MoveReady(); 
   status = ibm.MoveWait(); 
  
   for(j=0; j<NUM_RUNS; j++)
   { 
     // Move the conveyor to the desired jth position
     conveyorDisp[0] = conveyorPos[j];
     conveyor.Drive(conveyorDisp);

     // PUMA: Move to pickup position
     puma_tm = puma_PickupTM;
     puma.InverseKinematics(puma_tm,puma_jold,puma_jnew,puma_conf);
     puma.Drive(puma_jnew);

     // IBM: Move to pickup position
     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[j][0]; 
     ibm_tm[1][3] =  340.0;
     ibm_tm[2][3] =  -20.0;
     printf("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[j][0];
     ibm_tm[1][3] =  340.0;
     ibm_tm[2][3] = -150.0;
     printf("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("PUMA Opening Gripper\n");
     status = puma.GripperOpen();

     // IBM: Open the gripper
     printf("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[j][0];
     ibm_tm[1][3]  =  340.0;
     ibm_tm[2][3]  = -150.0;
     ibm_etm       = ibm_tm;
     ibm_etm[1][3] = ibm_PickupPos[j][1];
     ibm.MoveLine(ibm_tm, ibm_etm, 10, ibm_conf);

     // PUMA: Close the gripper
     printf("PUMA Closing Gripper\n");
     puma.GripperClose();

     // IBM: Close the gripper
     printf("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("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[j];
     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("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("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("PUMA Opening Gripper\n");
     status = puma.GripperOpen();

     // IBM: Open the gripper
     printf("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("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();
   }

   // 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("Program Complete.\n\n");

   return 0;
}