int motl=0, motr=1;
int s=25;
int l=4, m=3, r=2;
float dt=0.0;

int l_notape,m_notape,r_notape;
int l_tape,m_tape,r_tape;
int l_tol,m_tol,r_tol,d;

void main(){
    calibrate();
    init();
    d=3;
    while(1){
      if (d==3) {
      while((analog(l)<=l_tol)&&(analog(m)<=m_tol)&&(analog(r)<=r_tol)){
         motor(motl,s);
         motor(motr,-s);
         printf("%d %d %d\n",analog(l),analog(m),analog(r));
         if (analog(l)>l_tol)
            d=1;
         if (analog(m)>m_tol)
            d=2;
         if (analog(r)>r_tol)
            d=3;
         }
      }
      while((analog(l)>l_tol)||(analog(m)>m_tol)||(analog(r)>r_tol)){
         motor(motl,2*s);
         motor(motr,2*s);
         printf("%d %d %d\n",analog(l),analog(m),analog(r));
         if (analog(l)>l_tol)
            d=1;
         if (analog(m)>m_tol)
            d=2;
         if (analog(r)>r_tol)
            d=3;
         }
      printf("%d %d %d\n",analog(l),analog(m),analog(r));
      if (d==1) {
      while((analog(l)<=l_tol)&&(analog(m)<=m_tol)&&(analog(r)<=r_tol)){
         motor(motl,-s);
         motor(motr,s);
         printf("%d %d %d\n",analog(l),analog(m),analog(r));
         if (analog(l)>l_tol)
            d=1;
         if (analog(m)>m_tol)
            d=2;
         if (analog(r)>r_tol)
            d=3;
         }
      }
    }
}

/********** Subroutines *******************************/
void calibrate(){
   int temp;
   while(!stop_button()){
      l_notape=analog(l);
      m_notape=analog(m);
      r_notape=analog(r);
      printf("No Tape: %d %d %d\n",l_notape,m_notape,r_notape);
      sleep(.1);
   }
   tone(800.,1.);
   while(!stop_button()){
      l_tape=analog(l);
      m_tape=analog(m);
      r_tape=analog(r);
      printf("Tape: %d %d %d\n",l_tape,m_tape,r_tape);
      sleep(.1);
   }
   temp=(l_tape-l_notape)/2;
   l_tol=l_notape+temp;
   temp=(m_tape-m_notape)/2;
   m_tol=m_notape+temp;
   temp=(r_tape-r_notape)/2;
   r_tol=r_notape+temp;
   printf("Tol: %d %d %d\n",l_tol,m_tol,r_tol);
   tone(800.,1.);
}
void init(){
   printf("Press Start to  Go!\n");
   while(!start_button()){}
   tone(800.,1.);
   }
void test(){
      while((analog(l)<=l_notape)&(analog(m)>=m_tape)&(analog(r)<=r_notape)){
         motor(motl,s);
         motor(motr,s);
         printf("%d %d %d\n",analog(l),analog(m),analog(r));
         }
      ao();
      while (analog(l)>=l_tape){
         motor(motl,-s);
         motor(motr,s);
         printf("%d %d %d\n",analog(l),analog(m),analog(r));
         }
      while (analog(r)>=r_tape){
         motor(motl,s);
         motor(motr,-s);
         printf("%d %d %d\n",analog(l),analog(m),analog(r));
         }
}