我有个小车 需要一个超声波（有云台） 壁障小车程序 麻烦各位哥哥姐姐咯

我的单片机用的是C51  小车电机占的IO口 为P2口  前左电机为P1-0（正传）和P-1（反转）前右电机P2-2（正转）和P2-3（反转） 后左电机P-4（正转）和P2-5反转  后右电机P2-6（正转）和P2-7(反转)   。 其中（电机  正反转只需  其中一个为1 另一个为0）；
其中 超声波  输出口为P1-0口 接收口为P3-2口
  舵机输出口为P1-1口；

先谢过 各位哥哥姐姐咯   ；需要一个能超声波壁障的程序    其他程序都不用

/*****花了  几天的时间写出来了   对于我这初学者 还是蛮开心的*************/


#include<reg52.h>
#define car P2
#include <intrins.h>    //nop头文件
typedef unsigned int u16;
typedef unsigned char u8;
sbit car_0 = P2^0;       //前左电机正转
sbit car_1 = P2^1;       //前左电机反转
sbit car_2 = P2^2;       //前右电机正转
sbit car_3 = P2^3;       //前右电机反转
sbit car_4 = P2^4;       //后左电机正转
sbit car_5 = P2^5;       //后左电机反转
sbit car_6 = P2^6;       /后右电机正转
sbit car_7 = P2^7;       // 后右电机反转
sbit trig  = P1^0;       //超声波输出口
sbit echo  = P3^2;       //超声波接收口
sbit dj    = P1^1;       //舵机控制口
u16 time,S,S1,S2,S3,S4,push_val_left,pwm_val_left,timer;
#define Left_moto_go    {car_0=0,car_1=1,car_4=0,car_5=1;}   //左边两个电机向前走#define Left_moto_back  {car_0=1,car_1=0,car_4=1,car_5=0;}         //左边两个电机向后走#define Left_moto_Stop  {car_0=1,car_1=1,car_4=1,car_5=1;}         //左边两个电机停止
#define Right_moto_go   {car_2=0,car_3=1,car_6=0,car_7=1;}         //右边两个电机向前走
#define Right_moto_back {car_2=1,car_3=0,car_6=1,car_7=0;}         //右边两个电机向后走
#define Right_moto_Stop {car_2=1,car_3=1,car_6=1,car_7=1;}         //右边两个电机停止
/************************************************************************/
//前进
void qianj(void)                   //前进
{
        Left_moto_go ;                
        Right_moto_go ;               
}
/************************************************************************/
void hout(void)                                   //后退
{
        Left_moto_back ;        
        Right_moto_back ; }  
/************************************************************************/
//左转                         
void zuoz(void)
{
        Left_moto_back ;       
        Right_moto_go ;        
//        zxdzz ();                          
}
/************************************************************************/
//右转
void youz(void)                        
{
        Left_moto_go ;                
        Right_moto_back ;           
//        zxdyz ();                                 
}
/************************************************************************/
//STOP
void tingz(void)                           //停止
{                                                 
        Left_moto_Stop ;                  
        Right_moto_Stop ;        
//        zxdgd ();                       
}
/************************************************************************/
//超声波发射信号
void StartModule()
{
         trig = 1
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        _nop_();
        trig=0;
}
/***************************************************/
/**************************************************/
void         Conut (void) //计算距离
{
                while(!echo);   
                TR0=1;           
                while(echo);   
                TR0=0;         
                time=TH0*256+TL0
                TH0=0;
                TL0=0;
                S=(time*1.7)/100

}


void COMM( void )         //方向判断
{

        push_val_left=5;
        timer=0;
        while(timer<=4000);
        StartModule();     
        Conut();           
        S2=S;
       
        push_val_left=23;
        timer=0;
        while(timer<=4000);
        StartModule();     
        Conut();         
        S4=S;
       
        push_val_left=13;   
        timer=0;
        while(timer<=4000);
        StartModule();     
        Conut();         
        S1=S;
       
        if((S2<20)||(S4<20)
        {
                hout();         
                timer=0;
                while(timer<=4000);
        }

        if(S2>S4)
        {
                youz();      
                timer=0;
                while(timer<=4000);
        }
        else
        {
                        zuoz();     
                        timer=0;
                while(timer<=4000);
        }
}


void main (void)
{
          push_val_left =  13;
    car = 0xff;
          TMOD = 0x11;
          TH1 = 0x0FF;
    TL1 = 0x0A4;
          ET1 = 1;
          TR1 = 1;
          TH0 = 0;
    TL0 = 0;
    ET0 = 1;
          EA = 1;
  while(1)
        {        

       
                        if(timer>=1000)
                        {
                                timer = 0;
                                StartModule();
                                Conut();
                                if(S<45)
                                {
                                        hout();
                                        hout();
                                        hout();
                                        tingz();
                                        COMM();
                                }
                                else
                                        if(S>45)
                                        qianj();
                        }
        }


        }

/***************************************************/
///定时器3
void pwmdins()interrupt 3
{

    TH1 = 0x0FF;
    TL1 = 0x0A4;
    timer++;                                       
          pwm_val_left++;                                         
          if(pwm_val_left<=push_val_left)
               dj = 1;
           else
              dj = 0;
           if(pwm_val_left>=200)
              pwm_val_left=0;         
                          

}

我也是初学都 ，