新人第一次发帖，求大神们帮我看看这个超声波测距和测速程序有哪些毛病，抖动得厉害而且容易卡住。

/********************************************************************
                各种申明及定义
*******************************************************************/
#include<reg52.h> //包含头文件，一般情况不需要改动，头文件包含特殊功能寄存器的定义
#include<intrins.h>//_nop_
sbit led = P3^5;
//LCD1602(LCD12864)共用位定义 液晶屏       
sbit RS = P2^6;   //定义LCD1602(LCD12864)端口
sbit RW = P2^5;
sbit EN = P2^7;
#define RS_CLR RS=0
#define RS_SET RS=1
#define RW_CLR RW=0
#define RW_SET RW=1
#define EN_CLR EN=0
#define EN_SET EN=1
#define DataPort P0
//液晶屏
//超声波位定义       
sbit TX1=P3^7;      //超声波控制位
sbit RX1=P3^6;      //超声波接收位
void Timer0_Init();//定时器0初始化
extern float                 ult_s1[4];        //超声波传感器距离
extern bit                         ult_flag; //测距溢出标志位
void ULT_Delay20us();                //@11.0592MHz
void  ULT_StartModule(void);   //启动信号 高电平持续时间至少10us
void ULT_Conut(unsigned char x);
void ULT_GetData(unsigned char x);
void LCD1602_Delay5ms();          //@11.0592MHz
bit LCD1602_Check_Busy(void);// 判忙函数
void LCD1602_Write_Com(unsigned char com);//写入命令函数
void LCD1602_Write_Data(unsigned char Data);//写入数据函数
void LCD1602_Init(void);//初始化函数
void LCD1602_Clear(void);//清屏函数
void LCD1602_SetPos(unsigned char X,unsigned char Y);                                                //设置坐标
void LCD1602_Write_Char(unsigned char x,unsigned char y,unsigned char Data);//写入字符函数  x--第几行         y--第几列
void LCD1602_Write_String(unsigned char x,unsigned char y,unsigned char *s);//写入字符串函数   x--第几行         y--第几列
int tt0=0,tt1=0;//定时器0 1 对应参数

void delay_ms(unsigned int z)        //延时函数
{
        unsigned int x,y;
        for(x=z;x>0;x--)
        for(y=110;y>0;y--);       
}  
/*****************************************************************************
                    主函数
******************************************************************************/
void main(void)
{                           
        float ult_s1_old=0;//存储上一时刻距离
        float speed=0;           //计算实时速度
        LCD1602_Init(); //液晶屏初始化
        LCD1602_Clear();//清屏
/////*************************0123456789012345*************************/                                               
        LCD1602_Write_String(0,0," Distance:***cm ");
        LCD1602_Write_String(1,0," Speed:***cm/s ");       
        Timer0_Init();
        while(1)
        {
                ULT_GetData(0);//采集通道1超声波信号 数据存于ult_s1【】范围0-999 单位cm
                ULT_GetData(1);
                ULT_GetData(2);
                if (ult_s1[0]<ult_s1[1])
                {
                        ult_s1[3]=ult_s1[0];
                        ult_s1[0]=ult_s1[1];
                        ult_s1[1]=ult_s1[3];
                }
                if (ult_s1[1]<ult_s1[2])
                {
                        ult_s1[3]=ult_s1[1];
                        ult_s1[1]=ult_s1[2];
                        ult_s1[2]=ult_s1[3];
                }
                if (ult_s1[0]<ult_s1[1])
                {
                        ult_s1[3]=ult_s1[0];
                        ult_s1[0]=ult_s1[1];
                        ult_s1[1]=ult_s1[3];
                }
                ult_s1[3]=ult_s1[1];
                if(ult_flag==0)//采集正常
                {
                        LCD1602_Write_Char(0,10,(unsigned int)ult_s1[3]%1000/100+0x30);
                        LCD1602_Write_Char(0,11,(unsigned int)ult_s1[3]%100/10+0x30);
                        LCD1602_Write_Char(0,12,(unsigned int)ult_s1[3]%10/1+0x30);
                        if(ult_s1[3]>ult_s1_old)
                                speed=(ult_s1[3]-ult_s1_old)*2;
                        else
                                speed=(ult_s1_old-ult_s1[3])*2;
                               
                        LCD1602_Write_Char(1,7,(unsigned int)speed%1000/100+0x30);
                        LCD1602_Write_Char(1,8,(unsigned int)speed%100/10+0x30);
                        LCD1602_Write_Char(1,9,(unsigned int)speed%10/1+0x30);
                        ult_s1_old=ult_s1[3];                 
                }
                delay_ms(500);
            led=~led;
        }
}
/*---------------------------------
                  定时器0初始化
----------------------------------*/
void Timer0_Init()
{
        EA=0;                         //暂时关闭总中断
        TMOD&=0xF0;          //定时器0模式控制在低4位
        TMOD|=0x01;            //定时器0工作在模式1，16位定时器/计数器
        TH0=0;                        //超声波用定时器0
        TL0=0;                        //超声波用定时器0
        PT0=0;                        //可忽略 优先级设置 1=高优先级 0=低优先级
        ET0=1;                        //允许定时器0中断
        TR0=1;                        //启动定时器0
        EA=1;                        //打开总中断
}
/*-----------------------------------
                定时器0中断服务函数
------------------------------------*/
void Timer0(void) interrupt 1                 
{
        TH0=0;                        //超声波用定时器0
        TL0=0;                       
        ult_flag=1;//定时中断溢出 说明此次超声波测距失败
}

/******************************************************************************
                        超声波相关函数
******************************************************************************/
float        ult_s1[4]=0;//超声波测距距离
bit                ult_flag =0; //测距溢出标志位
void ULT_Delay20us()                //@11.0592MHz       
{
        unsigned char i;
        _nop_();
        i = 6;
        while (--i);
}

void  ULT_StartModule(void)                          //启动信号 高电平持续时间至少10us
{
TX1=1;
ULT_Delay20us();
TX1=0;                                                                                                 
}
void ULT_Conut(unsigned char x)
{
        unsigned int  ult_time=0;        //超声波发射后接收到的高电平时间
        ult_time=TH0*256+TL0;
        TH0=0;
        TL0=0;
  ult_s1[x]=(ult_time*1.87)/100;        //CM
}

void ULT_GetData(unsigned char x)
{
        ULT_StartModule();
                while(！RX1);                //当RX为零时等待
                TR0=1;                            //开启计数
                while(RX1);                        //当RX为1计数并等待
                TR0=0;                                //关闭计数
    ULT_Conut(x);                    //计算
}

/**************************************************************************
                   LCD1602相关函数
**************************************************************************/
void LCD1602_Delay5ms()          //@11.0592MHz
{
        unsigned char i, j;
        i = 9;
        j = 244;
        do
        {
                while (--j);
        }while (--i);
}
/*------------------------------------------------
              判忙函数
------------------------------------------------*/
bit LCD1602_Check_Busy(void)
{
        DataPort= 0xFF;
        RS_CLR;
        RW_SET;
        EN_CLR;
        _nop_();
        EN_SET;
        return (bit)(DataPort & 0x80);
}
/*------------------------------------------------
              写入命令函数
------------------------------------------------*/
void LCD1602_Write_Com(unsigned char com)
{  
        while(LCD1602_Check_Busy()); //忙则等待
        RS_CLR;
        RW_CLR;
        EN_SET;
        DataPort= com;
        _nop_();
        EN_CLR;
}
/*------------------------------------------------
              写入数据函数
------------------------------------------------*/
void LCD1602_Write_Data(unsigned char Data)
{
        while(LCD1602_Check_Busy()); //忙则等待
        RS_SET;
        RW_CLR;
        EN_SET;
        DataPort= Data;
        _nop_();
        EN_CLR;
}
/*------------------------------------------------
              初始化函数
------------------------------------------------*/
void LCD1602_Init(void)
{
   LCD1602_Write_Com(0x38);    /*显示模式设置*/
   LCD1602_Delay5ms();
   LCD1602_Write_Com(0x38);
   LCD1602_Delay5ms();
   LCD1602_Write_Com(0x38);
   LCD1602_Delay5ms();
   LCD1602_Write_Com(0x38);  
   LCD1602_Write_Com(0x08);    /*显示关闭*/
   LCD1602_Write_Com(0x01);    /*显示清屏*/
   LCD1602_Write_Com(0x06);    /*显示光标移动设置*/
   LCD1602_Delay5ms();
   LCD1602_Write_Com(0x0C);    /*显示开及光标设置*/
}
/*------------------------------------------------
                清屏函数
------------------------------------------------*/
void LCD1602_Clear(void)
{
        LCD1602_Write_Com(0x01);
        LCD1602_Delay5ms();
}
/*------------------------------------------------
                设置坐标
                        x=0-1行 y=0-15列
------------------------------------------------*/
void LCD1602_SetPos(unsigned char x,unsigned char y)
{                          
        if (x == 0)
        {     
                LCD1602_Write_Com(0x80 + y);     //表示第一行
        }
        else
        {      
                LCD1602_Write_Com(0xC0 + y);      //表示第二行
        }        
}
/*------------------------------------------------
              写入字符函数
------------------------------------------------*/
void LCD1602_Write_Char(unsigned char x,unsigned char y,unsigned char Data)
{     
        LCD1602_SetPos(x,y);
        LCD1602_Write_Data(Data);  
}
/*------------------------------------------------
              写入字符串函数
                          x--第几行         y--第几列
------------------------------------------------*/
void LCD1602_Write_String(unsigned char x,unsigned char y,unsigned char *s)
{     
        LCD1602_SetPos(x,y);
        while (*s)
        {     
                LCD1602_Write_Data(*s++);     
        }
}