/* a line-following program based on two downward-facing */
/*  reflectance sensors */

#define leftline 27
#define rightline 25
#define sensitivity 21
#define leftmotor 3
#define rightmotor 0
#define powerswitch 7

void main()
{

  while (digital(powerswitch) == 0) {
    printf("Threshold is %d\n",analog(sensitivity));
  }
  while (digital(powerswitch) == 1) {}        /* debounce switch */

  while (digital(powerswitch) == 0) {

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

    motor(leftmotor,100);
    motor(rightmotor,100);

    if ((analog(leftline) < analog(sensitivity)) && (analog(rightline) >= analog(sensitivity))) {

      /* right sensor is off the line... */

      printf("Drifting back to the left...\n");
      motor(leftmotor,100);
      motor(rightmotor,50);

      while ((analog(leftline) < (analog(sensitivity) + 5)) && (analog(rightline) >= analog(sensitivity))) {}
      /* wait until back on the line... */

    }

    if ((analog(leftline) >= analog(sensitivity)) && (analog(rightline) < analog(sensitivity))) {

      printf("Drifting back to the right...\n");
      motor(leftmotor,50);
      motor(rightmotor,100);

      while ((analog(rightline) < (analog(sensitivity) + 5)) && (analog(leftline) >= analog(sensitivity))) {}

    }

    if ((analog(leftline) < analog(sensitivity)) && (analog(rightline) < analog(sensitivity))) {

      /* completely off the line... */
      printf("Turning to find the line...\n");

      motor(leftmotor,100);
      motor(rightmotor,-100);

      while ((analog(rightline) < (analog(sensitivity) + 5)) && (analog(leftline) < (analog(sensitivity) + 5))) {}

    }

  }
  off(leftmotor);
  off(rightmotor);
}