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