#define pumpmotor 3
#define switchmotor 1
#define drivemotor 2
#define steermotor 0
#define leftswitchlimit 0
#define rightswitchlimit 2
#define clawcontrol 4
#define speedcontrol 23
#define steercontrol 21
#define steersensor 19
#define allowablesteererror 20

void main() {

  /* change this line to  (1 == 1)  to debug... */
  if (1 == 0) {
    while (1) {
      printf("%d %d %d %d %d %d\n",analog(speedcontrol), analog(steercontrol), analog(steersensor), digital(clawcontrol), digital(rightswitchlimit), digital(leftswitchlimit)); 
      sleep(0.5);
    }
  }

  while (1) {

    printf("Rrrrrrr....\n");

    motor(drivemotor,((analog(speedcontrol) * 100) / 255));

    if (digital(clawcontrol) == 1) {
      printf("activating the  CLAW!!!\n");
      motor(steermotor,0);
      claw();
    }

    if ((analog(steercontrol) < (analog(steersensor) - allowablesteererror)) && (analog(steersensor) > 50)) {
      printf("steering +...\n");
      motor(steermotor,100);
    }
    else if ((analog(steercontrol) > (analog(steersensor) + allowablesteererror)) && (analog(steersensor) < 200)) {
      printf("steering -...\n");
      motor(steermotor,-100);
    }
    else {
      motor(steermotor,0);
    }

  }

}

void claw() {

  motor(pumpmotor,100);
  if (digital(leftswitchlimit) == 1) {
    motor(switchmotor,100);
    while(digital(rightswitchlimit) == 0) {}
    motor(switchmotor,0);
  }
  else {
    motor(switchmotor,-100);
    while(digital(leftswitchlimit) == 0) {}
    motor(switchmotor,0);
  }
  sleep(0.7);               /* set how long it pumps? */
  motor(pumpmotor,0);

}