求大神帮忙看看3路超声波控制小车程序，始终不动 -

# include "reg52.h"
# include "intrins.h"
#define uchar unsigned char
#define uint unsigned int
sbit L298N_INA = P1^0;
sbit L298N_INB = P1^1;
sbit L298N_INC = P1^2;
sbit L298N_IND = P1^3;
sbit ENA_pwm = P2^6;
sbit ENB_pwm = P2^7;
sbit Trlg1 = P2^0;
sbit Echo1 = P2^1;
sbit Trlg2 = P3^1;
sbit Echo2 = P3^2;
sbit Trlg3 = P3^3;
sbit Echo3 = P3^4;
bit flag = 0;
unsigned char pwm_val_left =0; //pwm变量定义
unsigned char push_val_left =0; // 左电机占空比N/10
unsigned char pwm_val_right =0;
unsigned char push_val_right=0; // 右电机占空比N/10
unsigned int time=0;
unsigned long S1 = 0; //距离
unsigned long S2 = 0;
unsigned long S3 = 0;
/************************************************************************/
/* 运动模块*/
void Go_forward()
{
push_val_left=4;
push_val_right=4;
L298N_INA = 0;
L298N_INB = 1;
L298N_INC = 1;
L298N_IND = 0;
}
void Go_back()
{
push_val_left=4;
push_val_right=4;
L298N_INA = 1;
L298N_INB = 0;
L298N_INC = 0;
L298N_IND = 1;
}
void Go_left()
{
push_val_left=3;
push_val_right=3;
L298N_INA = 0;
L298N_INB = 1;
L298N_INC = 1;
L298N_IND = 1;
}
void Go_right()
{
push_val_left=3;
push_val_right=3;
L298N_INA = 1;
L298N_INB = 0;
L298N_INC = 0;
L298N_IND = 0;
}
void Stop()
{
L298N_INA = 0;
L298N_INB = 0;
L298N_INC = 0;
L298N_IND = 0;
}
/************************************************************************/
/* 延时模块*/
void delay(uint c) //1ms延时函数
{
uint a, b;
for(a=c;a>0;a--)
{
for(b=110;b>0;b--);
}
}
void Delay10us_CSB1(unsigned char i) //10us延时函数启动超声波模块时使用
{
unsigned char j;
do{
j = 10;
do{
_nop_();
_nop_();
}while(--j);
}while(--i);
}
void Delay10us_CSB2(unsigned char i)
{
unsigned char j;
do{
j = 10;
do{
_nop_();
_nop_();
}while(--j);
}while(--i);
}
void Delay10us_CSB3(unsigned char i)
{
unsigned char j;
do{
j = 10;
do{
_nop_();
_nop_();
}while(--j);
}while(--i);
}
/************************************************************************/
/* PWM调制电机转速 */
void pwm_out_left_moto(void)
{
if(pwm_val_left<=push_val_left)
ENA_pwm=1;
else
ENA_pwm=0;
if(pwm_val_left==10) // 0~10 , 0最小，10最大
pwm_val_left=0;
}
void pwm_out_right_moto(void)
{
if(pwm_val_right<=push_val_right)
ENB_pwm=1;
else
ENB_pwm=0;
if(pwm_val_right==10) // 0~10 , 0最小，10最大
pwm_val_right=0;
}
/*中断函数产生PWM信号*/
void timer1()interrupt 3
{
TH1=(65536-10)/256; //10US定时
TL1=(65536-10)%256;
pwm_val_left++;
pwm_val_right++;
pwm_out_left_moto();
pwm_out_right_moto(); //定时器100US为准。在这个基础上延时
}
/* 计时器溢出模块*/
void timer0() interrupt 1
{
flag=1;
}
/************************************************************************/
/*计算超声波所测距离并显示 */
void Conut1(void) //计算距离
{
while(!Echo1); //当（ECHO信号回响）为零时等待
TR0=1; //开启计数
while(Echo1);
TR0=0; //关闭计数
time=TH0*256+TL0; //读取脉宽长度
TH0=0;
TL0=0;
S1=(float)(time*1.085)*0.17;
if((S1>=7000)||(flag == 1))
{
flag = 0;
}
}
void Conut2(void) //计算距离
{
while(!Echo2); //当（ECHO信号回响）为零时等待
TR0=1; //开启计数
while(Echo2);
TR0=0; //关闭计数
time=TH0*256+TL0;
TH0=0;
TL0=0;
S2=(float)(time*1.085)*0.17;
if((S2>=7000)||(flag == 1))
{
flag = 0;
}
}
void Conut3(void) //计算距离
{
while(!Echo3); //当（ECHO信号回响）为零时等待
TR0=1; //开启计数
while(Echo3);
TR0=0; //关闭计数
time=TH0*256+TL0;
TH0=0;
TL0=0;
S3=(float)(time*1.085)*0.17;
if((S3>=7000)||(flag == 1))
{
flag = 0;
}
}
void bizhang (void)
{
Trlg1=1;
Delay10us_CSB1(5);
Trlg1=0; //启动超声波测距
Conut1(); //计算距离
Trlg2=1;
Delay10us_CSB2(5);
Trlg2=0;
Conut2();
Trlg3=1;
Delay10us_CSB3(5);
Trlg3=0;
Conut3();
if(S1<50 && S2<50 && S3<50)
{
Go_back();
delay(50);
Stop();
if(S2<50 && S3>50)
{
Go_right();
}
if(S2>50 && S3<50)
{
Go_left();
}
}
else if(S1>50 && S2<50 && S3<50)
{
Stop();
delay(50);
if(S2<50 && S3>50)
{
Go_right();
}
if(S2>50 && S3<50)
{
Go_left();
}
}
else if(S1>50&& S2>50 && S3>50)
{
Go_forward();
}
}
void main(void)
{
TMOD |= 0x20;//定时器1工作模式2,8位自动重装。用于产生PWM
TMOD |= 0x01;//定时器0工作模块1,16位定时模式。T0用测Echo脉冲长度
TH1=220;
TL1=220;
TH0 = 0;
TL0 = 0;
ET1 = 1; //允许T1中断
ET0 = 1; //允许T0中断
TR1 = 1; //启动定时器1
EA = 1; //启动总中断
while(1)
{
bizhang();
delay(65);
}
}

复制代码