void main()
{
  int position,side;
  while (!stop_button())
    {
      printf("%d %d %d\n",analog(18),analog(19),analog(20));
    }
  start_machine(17);
  initialize();
  position=findposition();
  if (position==1)
    {
      right(.8);
    }
  if (position==2)
    {
      left(1.65);
    }
  if (position==3)
    {
      left(.75);
    }
  if (position==0)
    {
      right(.2);
    }
  side=analog(18);
  fd(0);
  fd(1);
  fd(2);
  fd(3);
  sleep(2.5);
  run();
  ao();
}  

int findposition()
{
  while (1)
    {
  if (((analog(18)>40) & (analog(19)>40) & ((int)(analog(20))>55))|((analog(18)<40) & (analog(19)<40) & ((int)(analog(20))<55)))
    {
      printf("left\n");
      return 2;
    }
  if (((analog(18)>40) & (analog(19)>40) & ((int)(analog(20))<55))|((analog(18)<40) & (analog(19)<40) & ((int)(analog(20))>55)))
    {
      printf("forward\n");
      return 1;
    }
  if (((analog(18)<40) & (analog(19)>40) & ((int)(analog(20))>55))|((analog(18)>40) & (analog(19)<40) & ((int)(analog(20))<55)))
    {
      printf("right\n");
      return 0;
    }
  if (((analog(18)>40) & (analog(19)<40) & ((int)(analog(20))>55))|((analog(18)<40) & (analog(19)>40) & ((int)(analog(20))<55)))
    {
      printf("back\n");
      return 3;
    }
    }
}


void left(float time)
{
  fd(0);
  fd(2);
  bk(1);
  bk(3);
  sleep(time);
  ao();
  fd(4);
}

void right(float time)
{
  fd(1);
  fd(3);
  bk(0);
  bk(2);
  sleep(time);
  ao();
  fd(4);
}

void initialize()
{
  enable_encoder(7);
  fd(4);
}

void run()
{
  motor(1,-100);
  motor(3,-100);
  motor(0,-75);
  motor(2,-75);
  sleep(.5);
  motor(1,-100);
  motor(3,-100);
  motor(0,-100);
  motor(2,-100);
  sleep(1.15);
  motor(1,0);
  motor(3,0);
  motor(0,0);
  motor(2,0);
  sleep(.1);
  motor(1,25);
  motor(3,25);
  motor(0,100);
  motor(2,100);
  sleep(1.95);
  motor(1,-100);
  motor(3,-100);
  motor(0,-25);
  motor(2,-25);
  sleep(3.5);
  motor(1,-100);
  motor(3,-100);
  motor(0,-100);
  motor(2,-100);
  sleep(.5);
  motor(1,100);
  motor(3,100);
  motor(0,100);
  motor(2,100);
  sleep(2.);  
  bk(0);
  bk(1);
  bk(2);
  bk(3);
  sleep(.3);
  left(.85);
  fd(1);
  fd(3);
  motor(0,75);
  motor(2,75);
  sleep(8.);
  bk(0);
  bk(1);
  bk(2);
  bk(3);
  sleep(.3);
  left(.85);
  fd(1);
  fd(3);
  motor(0,75);
  motor(2,75);
  sleep(10.); 
  ao();
}