超音波測距程式

超音波測距,正常需求，如果距離為0~5cm,則亮紅燈。5~50cm,亮黃燈。50~150cm,則亮綠燈。
而現在程式出現的問題是，距離算的不夠精準，而且距離近的時候，紅燈亮起，而稍微拉遠時，
黃燈亮起一下之後，及轉成紅燈。無法確切得知距離遠近，程式上面不知道是哪裡出錯，還請各
位前輩不吝指教，謝謝。
以下為我們的程式：
#include <REG51F.H>
sbit P10=P1^0;
sbit P11=P1^1;
sbit P12=P1^2;
sbit P36=P3^6;
sbit P37=P3^7;

void AddTime(void);
void CountDistance(void);
void delay_250US(void);
void delay_10US(void);
void delay_500MS(unsigned int count1);
unsigned int Time=250,Distance=0,Count=0;
main()
{
  MAIN_L:
    P10=1;                                         //紅燈
    P11=1;                                         //黃燈
    P12=1;                                         //綠燈
    P37=1;                                         //SINGLE_IN=H
    P36=0;
delay_250US();
P36=1;
  while(1)
  {
    while(P37==1)
{
   MAIN_H:
   TMOD=0x11;
      AddTime();
   Time++;
}
while(P37==0)
{
      delay_10US();
      if(P37==1)
      {
        goto MAIN_H;
      }
      else
      {
        Distance=Time/58;
        goto MAIN_N;
      }  
   MAIN_N:
      while(Distance>=0&&Distance<1&&Count<10)
      {
        P10=0;
        delay_500MS(10);
        P11=1;
        P12=1;
        Count++;
      }
      while(Distance>=5&&Distance<30&&Count<10)
      {
        P10=1;
        P11=0;
        delay_500MS(10);
        P12=1;
        Count++;
      }  
      while(Distance>=30&&Distance<300&&Count<10)
      {
        P10=1;
        P11=1;
        P12=0;
        delay_500MS(10);
        Count++;
      }
      Count=0;
   Time=0;
      goto MAIN_L;  
    }
  }
}
void delay_250US(void)
{
  TL1=65536-250;
  TH1=(65536-250)>>8;
  TR1=1;
  while(TF1==0);
  TR1=0;
  TF1=0;
}
void AddTime(void)
{
  TL0=65536-1;
  TH0=(65536-1)>>8;
  TR0=1;
  while(TF0==0);
  TR0=0;
  TF0=0;
}
void delay_10US(void)
{
  TL0=65536-10;
  TH0=(65536-10)>>8;
  TR0=1;
  while(TF0==0);
  TR0=0;
  TF0=0;
}
void delay_500MS(unsigned int count1)
{
  TL0=65536-50000;
  TH0=(65536-50000)>>8;
  TR0=1;
  while(TF0==0);
  TR0=0;
  TF0=0;
  count1--;
}

謝謝~