Mobile-C

<?xml version="1.0"?>

<!DOCTYPE myMessage SYSTEM "gafmessage.dtd">

<GAF_MESSAGE>
 <MESSAGE message="MOBILE_AGENT">
  <MOBILE_AGENT>
   <AGENT_DATA>
    <NAME>myagent</NAME>
    <OWNER>IEL</OWNER>
    <HOME>mouse2.engr.ucdavis.edu:5050</HOME>
    <TASK task="1" num="0">
     <DATA dim="0" name="no-return" complete="0" server="mouse2.engr.ucdavis.edu:5051">
     </DATA>
     <AGENT_CODE>
      <![CDATA[
	  
#include <stdio.h>
#include "robot.h"

#define X_PXCENTER 455.1459
#define Y_PXCENTER 322.8292
#define CAMALPHA (-.1271)

int getObjPos(array short objPXLoc[2]);
int calObjPos(array double ja[4], array short objPXLoc[2], array double objPos[2]);

int main(void) 
{
   array double jold[4] = {0,0,0,0},
                jnew[4],
                trans_matrix[4][4],
                end_tmatrix[4][4],
                ready2[4]   = {120.0, 143.0, -50.0,    0.0},
                visualAng[4]= {160.0, 210.0, -50.0, -110.0},
                objPos[2]={0};
   array short  objPXLoc[2]={0};
   int status,
       j,	
       ibm_conf = NOFLIP;
   char ans;

   // Instanciate CRobot class 
   class CRobot ibm = CRobot(ROBOT2, RUN_REALTIME);

   // Calibrate IBM robot and conveyor 
   status = ibm.Calibrate();    
   
   // Move IBM robot to ready position 
   //   ready2 position is ready position with Z = -50
   status = ibm.Drive(ready2); 
   status = ibm.MoveWait(); 
  
   while(1)
   { 
     // Move IBM to visually localize part
     ibm.Drive(visualAng);
     jold = visualAng;
     ibm.MoveWait();

     // Get Object Position
     getObjPXLoc(objPXLoc); 
     calObjPos(jold, objPXLoc, objPos); 
	 printf("AGENT: Calculated Obj Position: %0.4f\n", objPos);

     // Move IBM to Initial MoveLine Position
	 trans_matrix=robot2ZeroPosition;
	 trans_matrix[0][0] =  cos(110*3.1415/180);
	 trans_matrix[0][1] = -sin(110*3.1415/180);
	 trans_matrix[1][0] =  sin(110*3.1415/180);
	 trans_matrix[1][1] =  cos(110*3.1415/180);
     trans_matrix[0][3] =  objPos[0];
     trans_matrix[1][3] =  objPos[1]-40;
     trans_matrix[2][3] =    jold[2];
     printf("IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
        trans_matrix[0][3], trans_matrix[1][3], trans_matrix[2][3]);
     ibm.InverseKinematics(trans_matrix, jold, jnew, ibm_conf);
     ibm.Drive(jnew);
     ibm.MoveWait();
   
     // Drop the Z axis
     trans_matrix[2][3] = -150.0;
     printf("IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
        trans_matrix[0][3], trans_matrix[1][3], trans_matrix[2][3]);
     ibm.InverseKinematics(trans_matrix, jold, jnew, ibm_conf);
     ibm.Drive(jnew);
     ibm.MoveWait();

     // Open IBM robot gripper
     printf("IBM Opening Gripper\n");
     status = ibm.GripperOpen();

     // calculate required movement for desired position
     end_tmatrix = trans_matrix;
     end_tmatrix[1][3] = objPos[1];
     ibm.MoveLine(trans_matrix, end_tmatrix, 10, ibm_conf);

     // Close IBM robot gripper
     printf("IBM Closing Gripper\n");
     status = ibm.GripperClose();

     // Bring the Z axis up
     trans_matrix = end_tmatrix;
     trans_matrix[2][3] =  -20.0;
     printf("IBM Moving - X: %.2fmm Y: %.2fmm Z: %.2fmm\n",
        trans_matrix[0][3], trans_matrix[1][3], trans_matrix[2][3]);
     ibm.InverseKinematics(trans_matrix, jold, jnew, ibm_conf);
     ibm.Drive(jnew);
     ibm.MoveWait();

     // Move to Ready 2 Position
     jold = ready2;
     ibm.Drive(jold);
     ibm.MoveWait();

     // Open IBM robot gripper
     printf("Repeat (Y/N)?\n");
     ans = getc(stdin);
     if(ans != 'Y' && ans != 'y')
         break;
   }

   // Move IBM robot to ready position 
   //   ready2 position is ready position with Z = -50
   status = ibm.Drive(ready2); 
   status = ibm.MoveWait(); 
   status = ibm.MoveReady(); 
   status = ibm.MoveWait(); 

   // Close IBM robot gripper
   status = ibm.GripperClose();

   // Disable IBM robot
   status = ibm.Disable();

   printf("Program Complete.\n\n");

   return 0;
}

int getObjPXLoc(array short objPXLoc[2]) 
{ 
  short* data;

  printf("AGENT:Triggering the vision agent now.\n");
  mc_CondSignal(42);

  printf("AGENT:Waiting for a message.\n");
  data = (short *)mc_AclWaitRetrieve(mc_current_agent);
  objPXLoc[0] = data[0];
  objPXLoc[1] = data[1];

  printf("AGENT:Got the point: ");
  printf("%d %d\n", data[0], data[1]);

  return 0;
}

int calObjPos(array double ja[4], array short objPXLoc[2], array double objPos[2]) 
{
  array double alpha[4]  = {120, 137, 0, 0};
  array double joints[4] = ((double)(3.1415/180.0))*(ja-alpha);
  array double
    A1[4][4] =
      {{cos(joints[0]), -sin(joints[0]), 0,  0},
      {sin(joints[0]),  cos(joints[0]), 0,   0},
      {0, 0, 1, 0},
      {0, 0, 0, 1}},
    A2[4][4] =
      {{cos(joints[1]), -sin(joints[1]), 0,  325.0},
      {sin(joints[1]),  cos(joints[1]), 0,   0},
      {0, 0, 1, 0},
      {0, 0, 0, 1}},
    A3[4][4] =
      {{cos(CAMALPHA), -sin(CAMALPHA), 0, 225.0},
      {sin(CAMALPHA),  cos(CAMALPHA), 0, 0},
      {0, 0, 1, 0},
      {0, 0, 0, 1}},
    P[4][4] =
      {{1, 0, 0, (X_PXCENTER-objPXLoc[1])*60.0/265.1208},
      {0, 1, 0, -(Y_PXCENTER-objPXLoc[0])*60.0/265.1208},
      {0, 0, 1, 0},
      {0, 0, 0, 1}},
    A4[4][4] = A1*A2*A3*P;

	printf("AGENT: DIF: %0.6f %0.6f\n", P[0][3], P[1][3]);
	printf("AGENT: OBJ: %0.6f %0.6f\n", A4[0][3], A4[1][3]);

    objPos[0] = A4[0][3];
    objPos[1] = A4[1][3];

	A4 = A1*A2*A3;
	printf("AGENT: Ref position: %.6f %.6f\n", A4[0][3], A4[1][3]);

	return 0;
}
      ]]>
     </AGENT_CODE>
    </TASK>
   </AGENT_DATA>
  </MOBILE_AGENT>
 </MESSAGE>
</GAF_MESSAGE>