/* grasping hand with breakbeam sensor... */

/*  analog 16: hand breakbeam object detector */
/*             (= 240 or higher for object, 1 to 220 open) */
	       

#define handclosed 7
#define handbreakbeam 16
#define throttle 27
#define leftmotor 0
#define rightmotor 3
#define handmotor 2

void main()
{
  float a,b;
  float analogmax = 0.48;

  while (1) {

    printf("Looking for     stuff...\n");

    while (analog(handbreakbeam) < 240) {
        motor(leftmotor, (int) ((float) analog(throttle)/analogmax));
	motor(rightmotor, (int) ((float) analog(throttle)/analogmax));
    }

    printf("Found something!\n");
    off(leftmotor);
    off(rightmotor);

    fd(handmotor);

    /* keep track of how long it takes to grab the object... */
    a=seconds();
    while (digital(handclosed) == 0) {}
    b=seconds();
    b=b-a;

    printf("Grabbed it!\n");
    off(handmotor);

    /* turn around erratically */
    motor(leftmotor, 100);
    motor(rightmotor, -100);
    sleep(1.5);
    off(leftmotor);
    off(rightmotor);

    /* release for same time as it took to grasp it */
    bk(handmotor);
    sleep(b);

    printf("Dropped it!\n");
    off(handmotor);

    motor(leftmotor, -1 * (int) ((float) analog(throttle)/analogmax));
    motor(rightmotor, -1 * (int) ((float) analog(throttle)/analogmax));
    sleep(1.5);
    motor(leftmotor, -100);
    motor(rightmotor, 100);
    sleep(1.5);

  }
}