#include <robot.h>

int main() {
 array double P1[4][4],P2[4][4],//Puma T-matrix 
              I1[4][4],I2[4][4],//IBM T-matrix  
  jold1[6]={0,0,0,0,0,0},//Puma old joint values
  jold2[4] = {0,0,0,0},  //IBM old joint values
  p1[6],p2[6],i1[4],i2[4],//Puma & IBM joint value
  c1[1];                 //conveyer1 joint value
 // Puma is configurated as flip, below and left
 int robot1conf = FLIP|ABOVE|LEFT;     
 //IBM is configurated as flip
 int robot2conf = FLIP;
 // construct the class with real-time running
 string_t flag="realtime";              
 int i;

 // setup, initialize and check PMAC DACs output 
 class CRobot robot1 = CRobot(ROBOT1,flag),//Puma 
    robot2 = CRobot(ROBOT2,flag),//IBM 7575
    conveyer = CRobot(CONVEYER1,flag);//Conveyer

 /* calibrate Robots */
 robot1.Calibrate();        // calibrate Puma 560
 robot2.Calibrate();        // calibrate IBM 7575
 
 // calculate the position of the arms and conveyer 
 P1 = P2 = robot1ZeroPosition; //zero position
 P1[0][3] += 300; P1[1][3] += 300;
 P1[2][3] += -600;         // Puma wrist position
 // inverse kinematics
 robot1.InverseKinematics(P1,jold1,p1,robot1conf); 
 P2[0][3] += 0; P2[1][3] += 300;
 P2[2][3] += -400;         // Puma wrist position
 // inverse kinematics
 robot1.InverseKinematics(P2,jold1,p2,robot1conf); 

 I1 = I2 = robot2ZeroPosition; // zero position
 I1[0][3] += 700; I1[1][3] += 200;
 I1[2][3] += -60;          // IBM wrist position
 // inverse kinematics
 robot2.InverseKinematics(I1,jold2,i1,robot2conf); 
 I2[0][3] += 500; I2[1][3] += -200;
 I2[2][3] += -20;          // IBM wrist position
 // inverse kinematics
 robot2.InverseKinematics(I2,jold2,i2,robot2conf); 

 c1[0] = -500;          // move conveyer1 250 mm
 
 // a loop for assembly operations
 for(i=0; i<5; i++) {
  robot1.GripperOpen(); // open Puma 560 gripper
  robot1.Drive(p1);     // move Puma to position P1
  robot2.Drive(i1);     // move IBM  to position I1
  conveyer.Drive(c1);   // move conveyer1 500mm

//waiting Puma,IBM and conveyer to complete motion
  robot1.MoveWait();      
  robot2.MoveWait();               
  conveyer.MoveWait();        

  robot1.GripperClose();//PUMA picks up a part at P1
  robot2.GripperOpen(); // IBM stacks a part at I1

  robot1.Drive(p2);     // move Puma to position P2
  robot2.Drive(i2);     // move IBM to position I2

//waiting Puma,IBM and conveyer to complete motion
  robot1.MoveWait();     
  robot2.MoveWait();    
  conveyer.MoveWait();     

  robot1.GripperOpen(); //PUMA releases the part at P2
  robot2.GripperClose();//IBM picks up the part at I2

 }
   /* shutdown the system */
 robot1.MoveReady();    // move Puma to ready position
 robot2.MoveReady();    // move IBM to ready position

 // waiting for Puma,IBM to complete  their motion
 robot1.MoveWait();       
 robot2.MoveWait();   

 exit(0);   // exit the CH language environment 
}