超声波测距程序

采用凌阳的超声波测距模块芯片，要求用C51在伟福仿真器上实现，程序我已编过，不太理想，只能测到10CM的样子，哪位大虾帮我编个程序，或者帮我改进一下，请发我邮箱，qiyueliuxingyu@126.com，我会把资料和我的程序发给你的，谢谢了！！！
要求用C语言，不要汇编的

以下是用汇编语言编写的超声波测距控制源程序：采用AT89S51 12MHz晶振 显示缓冲单元在40H～43H，使用内存44H、45H、46H用于计算距离
20H用于标志
VOUT EQU P1.0 ;脉冲输出端口
*中断入口程序*
ORG 0000H
LJMP START
ORG 0003H
LJMP PINT0
ORG 000BH
LJMP INTT0
ORG 0013H
RETI
ORG 001BH
LJMP INTT1
ORG 0023H
RETI
ORG 002BH
RETI
*主程序*
START: MOV SP, #4FH
MOV R0, #40H ;40~43H为显示数据存放单元（40为最高位）
MOV R7,#0BH
CLEARDISP:MOV @R0, #00H
INC R0
DJNZ R7, CLEARDISP
MOV 20H, #00H
MOV TMOD, #21H ;T1为8位自动重装模式，T0为16位定时器
MOV TH0, #00H ;65ms初值
MOV TL0, #00H ;40KHz初值
MOV TH1, #0F2H
MOV TL1, #0F2H
MOV P0, #0FFH
MOV P1, #0FFH
MOV P2, #0FFH
MOV P3, #0FFH
MOV R4, #04H ；超声波脉冲个数控制（为赋值的一半）
SETB PX0
SETB ET0
STEB EA
CLR 00H