#include"ASEIO.h"
unsigned char m0,m1,m2,m3,m4,msj=1;
void fore(int sd,float sj);
void left(int sd,float sj,float rate);
void right(int sd,float sj,float rate);
char readligh();
void findline();
void stop();
int max[5]={0,0,0,0,0},min1[5]={255,255,255,255,255},mid1[5],leftc=0,rightc=0;

void main()
{
 unsigned char k,m;
 float sj1;
 InitArmRobot();
 resettime();
 k=readlight();
 while(1)
  {
   m=k;
   k=readligh();
   if (k==4) {fore(50,0);}
   if (k==0)
    {
     if ((m==4)||(m==0)) fore(40,0);
     if (m>4) left(50,0,0.5);
     if (m<4) right(50,0,0.5);
    }
   if (k==8) {left(50,0,0.2);}
   if (k==2) {right(50,0,0.2);}
   if ((k>15)&& msj) {leftc++;sj1=seconds();msj=0;}
   if ((m0)&& msj) {rightc++;sj1=seconds();msj=0}
   if (seconds()>sj1+0.3) msj=1;
   if (leftc==4) {leftc=5;left(100,0.3,-1);}
   if (leftc==7) {leftc=8;left(100,0.3,-1);}
  }
}

void fore(int sd,float sj)
{
 float sj1;
 sj1=seconds();
 SetMoto(0,sd);
 SetMoto(1,sd);
if(sj)
 {
  while(seconds()<sj1+sj);
   SetMoto(0,0);
   SetMoto(1,0);
 }
}

void left(int sd,float sj,float rate)
{
 float sj1;
 sj1=seconds();
 SetMoto(0,sd*rate);
 SetMoto(1,sd*rate);
if(sj)
 {
  while(seconds()<sj1+sj);
   SetMoto(0,0);
   SetMoto(1,0);
 }
}

void right(int sd,float sj,float rate)
{
 float sj1;
 sj1=seconds();
 SetMoto(0,sd);
 SetMoto(1,sd);
if(sj)
 {
  while(seconds()<sj1+sj);
   SetMoto(0,0);
   SetMoto(1,0);
 }
}

char readligh()
{
 unsigned char i,j,m;
 unsigned int k;
 j=0;m=1;
 for(i=0;i<5;i++)
  {
   k=AI(4+i);
   if (k>max1[i]) max1[i]=k;
   if (k<min1[i]) min1[i]=k;
   mid1[i]=(max1[i]+min1[i])/2;
   if (k>mid1[i]) j=j|m;
   m=m<<1;
  }
 if (j&0x10) m4=1;else m4=0;
 if (j&0x08) m3=1;else m3=0;
 if (j&0x04) m2=1;else m2=0;
 if (j&0x02) m1=1;else m1=0;
 if (j&0x01) m0=1;else m0=0;
 printf("%d%d%d%d%d %d %d",m4,m3,m2,m1,m0,leftc,rightc);
return(j);
}

void stop()
{SetMoto(0,0);SetMoto(1,0);}

void findline()
{

}