Programación C integrada para conectar el sensor ultrasónico al microcontrolador 89s51

Estoy intentando conectar un sensor ultrasónico HC-SR04 a un microcontrolador de la familia 51 como 89s51. Después de aprender cómo funciona el sensor ultrasónico, escribí un programa según mi conocimiento sobre el sensor ultrasónico, pero ahora este programa no parece funcionar. Lo que el programa básicamente hace es que detecta cualquier obstrucción como la pared frente al sensor ultrasónico y si la distancia es inferior a 30 cm, deja de funcionar ambos motores y solo funciona con el motor correcto durante un tiempo y nuevamente comienza a funcionar con ambos motores y cuando detecta una pared dentro 30 cm La segunda vez que hace funcionar el motor izquierdo una y otra vez comienza a funcionar con ambos motores la tercera vez, solo el motor derecho y, de esta manera, el ciclo continúa para siempre, mientras que el ultrasonido constantemente detecta la distancia entre éste y la pared. El programa es el siguiente -

#include <reg51.h>
#include <stdio.h>
sfr16 DPTR =0x82                ;
sbit RM1_X = P2^0               ;    //Positive of Right Motors
sbit LM1_X = P2^1               ;    //Positive of Left Motors
sbit RM2_X = P2^2               ;    //Positive of Right Motorss
sbit LM2_X = P2^3               ;    //Positive of Left Motors
sbit US_E = P3^2                ;    //Echo pin of Ultrasonic sensor
sbit US_T= P3^5                 ;    //Trigger pin of Ultrasonic sensor
//function prototypes
void RM_F(void)                 ;
void RM_S(void)                 ;
void LM_F(void)                 ;
void LM_S(void)                 ;
void trig(void)                 ;
void MSDelay(unsigned int)          ;
unsigned char get_range(void)           ;
void main()
{ TMOD=0x09                 ;
  TR0=1                     ;
  TH0=0x00                  ;
  TL0=0x00                  ;
  P3=0x04                   ;
  P2=0x00                   ;
while(1)                        
{ unsigned char R=0             ;
   R=get_range()                ;
if(P0^0==0)
{if(R<=30) 
 {P0=0x01;
  LM_F()                    ;
  MSDelay(4000)                 ;
  }                   
else 
 {RM_F()                    ;
  LM_F()                    ;
  }}
else
{if(R<=30) 
 {P0=0                      ;
  RM_F()                    ;
  MSDelay(4000)                 ;
  }                   
else 
 {RM_F()                    ;
  LM_F()                    ;
  }}}}
unsigned char get_range() 
{unsigned char range                ;
 trig()                     ;
 MSDelay(40)                    ;
 DPH=TH0                    ;
 DPL=TL0                    ; 
 TH0=0xFF                   ;
 TL0=0xFF                   ;
 if(DPTR<35000)
 range=DPTR/59                  ;
 else
 range=0                    ;
 return range                   ; 
}
void trig()
{TH0=0x00                   ;
TL0=0x00                    ; 
US_T=1                      ;
MSDelay(0.01)                   ;                        
US_T=0                      ;
}
void RM_F()
{RM1_X = 1                  ;    //Make positive of motor 1
RM2_X = 1                   ;    //Make positive of motor 1
}
void RM_S()
{RM1_X = 0                  ;    //Make positive of motor 0
RM2_X = 0                   ;    //Make positive of motor 0
}
void LM_F()
{LM1_X = 1                  ;    //Make positive of motor 1
LM2_X = 1                   ;    //Make positive of motor 1
}
void LM_S()
{LM1_X = 0                  ;    //Make positive of motor 0
LM2_X = 0                   ;    //Make positive of motor 0
} 
//Some delay...
void MSDelay(unsigned int itime)
{unsigned int i,j               ;
for(i=0;i<itime;i++)
for(j=0;j<1275;j++)             ;
}

¿Qué cambios debo hacer para que este programa funcione? Gracias por ayudar. PD: utilicé este blog como referencia para mi programa enlace