51单片机做了一个小车，用超声波进行避障，发现无法直走，但是有障碍物的时候可以后退，求大神

感觉主函数那里错了，但是不知道怎么改，求解
#include<reg52.H>
#define uchar unsigned char
#define uint unsigned int
sbit led1=P1^0;
sbit led2=P1^1;
sbit led3=P1^2;
sbit led4=P1^3;
sbit led5=P1^4;
sbit Trig=P1^5;
sbit Echo=P1^6;
sbit zuodeng=P0^1;
sbit youdeng=P0^0;
uchar S,time;
uchar code date[]={0x11,0x33,0x22,0x66,0x44,0xcc,0x88,0x99};//直走
uchar code date_zuo[]={0x10,0x30,0x20,0x60,0x40,0xc0,0x80,0x90};//左轮正转，右轮不转
uchar code date_big_zuo[]={0x19,0x38,0x2c,0x64,0x46,0xc2,0x83,0x91};//左轮正转，右轮反转
uchar code date_big_you[]={0x91,0x83,0xc2,0x46,0x64,0x2c,0x38,0x19};//右轮正转，左轮反转
uchar code date_you[]={0x01,0x03,0x02,0x06,0x04,0x0c,0x08,0x09};//右轮正转，左轮不转
uchar code date_back[]={0x99, 0x88,0xcc,0x44,0x66,0x22,0x33,0x11};
void delay(uchar x)
{
         uchar a,b;
          for(a=x;a>0;a--)
         for(b=80;b>0;b--);
}
void delay_20us()
{
    uchar a ;
    for(a=0;a<100;a++);
}
void init()        //启动测距信号
{
   TMOD=0X01;
   EA=1;
   TH0=0;
   TL0=0;
   ET0=1;
        }
void  StartModule()
{
   Trig=1;
   delay_20us();
   Trig=0;
   while(！Echo);             //当RX为零时等待
   TR0=1;                    //开启计数
   while(Echo);              //当RX为1计数并等待
   TR0=0;                    //关闭计数
   time=TH0*256+TL0;        //读取脉宽长度
   TH0=0;
   TL0=0;
   S=(time*1.7)/100;          //算出来是CM
   }         
void go()
{
        uint i;
        for(i=0;i<8;i++)
        {
                P2=date[i];
                delay(7);
        }
}
void gosd()
{
        uint i;
        for(i=0;i<8;i++)
        {
                P2=date[i];
                delay(5);
        }
}
void zuo()
{
        uint i;
        for(i=0;i<8;i++)
        {
                P2=date_zuo[i];
                delay(6);
        }       
}
void bigzuo()
{
        uint i;
        for(i=0;i<8;i++)
        {
                P2=date_big_zuo[i];
                delay(5);
        }       
}
void you()
{
        uint i;
        for(i=0;i<8;i++)
        {
                P2=date_you[i];
                delay(6);
        }       
}
void bigyou()
{
        uint i;
        for(i=0;i<8;i++)
        {
                P2=date_big_you[i];
                delay(5);
        }       
}
void back()
{
                uint i;
                for(i=0;i<8;i++)
                {
                        P2=date_back[i];
                        delay(7);
                }       
          }  
void main()
{
    init();  
        while(1)
        {
          go();
          StartModule();
          if(S<=5)
          {
            back();
           }                                                                                      
       }
}

小车不走直线很正常，没有完全相同的电机，要想走直线需要加入反馈来修正路线，比如沿墙走就测小车距墙距离为恒值，用测速码盘测速，分别测量两侧脉冲数是否一样，不一致就修正，或者采用陀螺仪