/* --------------------------------------------------------------------- */
/* -                        BLOCKS.C                                   - */
/* --------------------------------------------------------------------- */
/* -    Some necessary includes...                                     - */
/* --------------------------------------------------------------------- */
#include </sys/h/types.h>
#include <stdio.h>
#include </sys/h/time.h> 
#define yes 1

/* --------------------------------------------------------------------- */
/* -    Global variables and structures...                             - */
/* --------------------------------------------------------------------- */

int result;
int x,y; 
float robot_x, robot_y, pix_per_inch = 24.0;

int length, xvals[1024], yvals[1024];
float small_x, small_y;
char *string;

main()
{ int result, r;
  char req;
  
  	int dx, threshold, going = 1;
  	int ylimit1, ylimit2, xlimit1;
  	int minx, miny, maxx, maxy;
  	int xcentroid, ycentroid, Area;
  
          ROBOT$INIT();
	  ROBOT$MOVE (240.0, 0.0, -10.0, 6.0, -90.0, 0.0);
	  robot_x = 2.0; 
	  robot_y = 2.0;  /* BLOCK */
/* small BLOCK */ pick_up_block(robot_x, robot_y, 2.5);  /*  BLOCK */
	  small_x = 0.0;
	  small_y = -10.0;
	  ROBOT$MOVE (240.0, 0.0, -10.0, 6.0, -90.0, 0.0);
          ROBOT$MOVE (240.0, 0.0, -10.0, 0.5, -90.0, 0.0);
          ROBOT$OPEN_GRIPPER( 2.5);
          ROBOT$MOVE (240.0, 0.0, -10.0, 6.0, -90.0, 0.0);
          ROBOT$CLOSE_GRIPPER();
	       
	  robot_x = 2.0; 
	  robot_y = 2.0;  /* BLOCK */
 /* BIG BLOCK */ pick_up_block(robot_x, robot_y, 2.5);  /*  BLOCK */
	  ROBOT$MOVE (240.0, 6.0, 0.0, 6.0, -90.0, 0.0);
          ROBOT$MOVE (240.0, 6.0, 0.0, 0.5, -90.0, 0.0);
          ROBOT$OPEN_GRIPPER( 2.5);
          ROBOT$MOVE (240.0, 6.0, 0.0, 6.0, -90.0, 0.0);
          ROBOT$CLOSE_GRIPPER();

/* put small one one top */	
	  ROBOT$MOVE (240.0, small_x, small_y, 6.0, -90.0, 0.0);
          ROBOT$OPEN_GRIPPER( 2.5);
          ROBOT$MOVE (240.0, small_x, small_y, 0.5, -90.0, 0.0);
          ROBOT$CLOSE_GRIPPER();
          ROBOT$MOVE (240.0, small_x, small_y, 5.0, -90.0, 0.0);
	  ROBOT$MOVE (240.0, 6.0, 0.0, 5.0, -90.0, 0.0);
          ROBOT$MOVE (240.0, 6.0, 0.0, 2.0, -90.0, 0.0);
          ROBOT$OPEN_GRIPPER( 2.5);
          ROBOT$MOVE (240.0, 6.0, 0.0, 6.0, -90.0, 0.0);
          ROBOT$CLOSE_GRIPPER();
          ROBOT$HOME();
} /* end main function */

pick_up_block(robot_x, robot_y, opening)
          float robot_x, robot_y, opening;
{
   ROBOT$MOVE (240.0, robot_x, robot_y, 3.0, -90.0, 0.0);
   ROBOT$OPEN_GRIPPER( opening);
   ROBOT$MOVE (240.0, robot_x, robot_y, 0.5, -90.0, 0.0);
   ROBOT$CLOSE_GRIPPER ();
   ROBOT$MOVE (240.0, robot_x, robot_y, 6.0, -90.0, 0.0);
}