51单片机添加数码管自检程序显示0000、1111……

如何在现有程序中添加数码管自检程序，实现0000、1111、2222……9999显示后进入正常功能，有没有大神指导一下！！！！！！

#include <reg52.H>//器件配置文件
#include <intrins.h>
#include "eeprom52.h"
//传感器接口
sbit RX  = P2^3;
sbit TX  = P2^2;
//按键声明
sbit S1  = P1^4;
sbit S2  = P1^5;
sbit S3  = P1^6;

sbit DIAN=P0^5;
//蜂鸣器
sbit Feng= P2^0;

//变量声明
unsigned int  time=0;
unsigned int  timer=0;
unsigned char posit=0;
unsigned long S=0;
unsigned long BJS;//报警距离
char num=0;
//模式 0正常模式 1调整
char Mode=0;
bit  flag=0,flag_BJ;

unsigned char const discode[] ={0x5F,0x44,0x9D,0xD5,0xC6,0xD3,0xDB,0x47,0xDF,0xD7,0x80};        //数码管显示码0123456789-和不显示

unsigned char disbuff[4]           ={0,0,0,0};                 //数组用于存放距离信息
unsigned char disbuff_BJ[4]        ={0,0,0,0};//报警信息
sbit W0=P2^4;
sbit W1=P2^5;
sbit W2=P2^6;
sbit W3=P2^7;

/******************把数据保存到单片机内部eeprom中******************/
void write_eeprom()
{
        SectorErase(0x2000);
        byte_write(0x2000, BJS%255);
        byte_write(0x2001, BJS/255);
        byte_write(0x2060, a_a);       
}

/******************把数据从单片机内部eeprom中读出来*****************/
void read_eeprom()
{
        BJS   = (byte_read(0x2001)*255)+byte_read(0x2000);
        a_a      = byte_read(0x2060);
}

/**************开机自检eeprom初始化*****************/
void init_eeprom()
{
        read_eeprom();                //先读
        if(a_a != 1)                //新的单片机初始单片机内问eeprom
        {
                BJS   = 50;
                a_a = 1;
                write_eeprom();           //保存数据
        }       
}
//延时100ms（不精确）
void delay(void)
{
    unsigned char a,b,c;
    for(c=10;c>0;c--)
        for(b=38;b>0;b--)
            for(a=130;a>0;a--);
}

//按键扫描
void Key_()
{
        //+
        if(S1==0)
        {
                delay();           //延时去抖
                delay();           //延时去抖
                while(S1==0)
                {
                        P1=P1|0x0f;
                }
                        BJS++;         //报警值加
                if(BJS>500) //最大500
                {
                        BJS=1;
                }
                write_eeprom();                           //保存数据
        }
        //-
        else if(S2==0)
        {
                delay();
                delay();           //延时去抖
                while(S2==0)
                {
                        P1=P1|0x0f;
                }
                        BJS--;         //报警值减
                if(BJS<1)         //最小1
                {
                        BJS=500;
                }
                write_eeprom();                           //保存数据
        }
        //功能
        else if(S3==0)                //设置键
        {
                delay();
                delay();           //延时去抖
                while(S3==0)
                {
                        P1=P1|0x0f;
                }
                        Mode++;                //模式加
                        num=0;
                if(Mode>=2)                //加到2时清零
                {
                        Mode=0;
                }       
        }
}
/**********************************************************************************************************/
//扫描数码管
void Display(void)                                 
{
        //正常显示
        if(Mode==0)
        {
                num++;
                if(num==1)
                {
                        W3=1;
                        W0=1;
                        P0=~discode[disbuff[0]];
                        DIAN=0;
                        W1=0;       
                }
                else if(num==2)
                {
                        W1=1;
                        P0=~discode[disbuff[1]];
                        W2=0;
                }
                else if(num>=3)
                {
                        W2=1;
                        P0=~discode[disbuff[2]];
                        W3=0;
                        num=0;
                }
        }
        //报警显示
        else
        {
                num++;
                if(num==1)
                {
                        W3=1;
                        P0=~0xCE;                  //11001110
                        W0=0;
                }
                else if(num==2)
                {
                        W0=1;
                        P0=~discode[disbuff_BJ[0]];
                        DIAN=0;
                        W1=0;       
                }
                else if(num==3)
                {
                        W1=1;
                        P0=~discode[disbuff_BJ[1]];
                        W2=0;
                }
                else if(num>=4)
                {
                        W2=1;
                        P0=~discode[disbuff_BJ[2]];
                        W3=0;
                        num=0;
                }
        }
}
/**********************************************************************************************************/
//计算
void Conut(void)
{
        time=TH0*256+TL0;          //读出T0的计时数值
        TH0=0;
        TL0=0;                                  //清空计时器
        S=(time*1.7)/100;     //算出来是CM

        if(Mode==0)                          //非设置状态时
        {
                if((S>=700)||flag==1) //超出测量范围显示“-”
                {       
                        flag=0;
                        disbuff[0]=10;           //“-”
                        disbuff[1]=10;           //“-”
                        disbuff[2]=10;           //“-”
                }
                else
                {
                        //距离小于报警距
                        if(S<=BJS)                        //距离小于报警值
                        {
                                 flag_BJ=1;                //报警变量置一，定时器开始报警
                        }
                        else                                //距离大于报警值
                        {
                                flag_BJ=0;                //关闭报警
                                Feng=1;                        //蜂鸣器关闭
                        }
                        disbuff[0]=S%1000/100;                 //将距离数据拆成单个位赋值
                        disbuff[1]=S%100/10;
                        disbuff[2]=S%10;
                }
        }
        else
        {
                        Feng=1;
                        flag_BJ=0;                                                   //报警关闭
                        disbuff_BJ[0]=BJS%1000/100;
                        disbuff_BJ[1]=BJS%100/10;
                        disbuff_BJ[2]=BJS%10;
        }
}
/**********************************************************************************************************/
//定时器0
void zd0() interrupt 1                  //T0中断用来计数器溢出,超过测距范围
{
        flag=1;                                                 //中断溢出标志
}
/**********************************************************************************************************/
//定时器1
void zd3() interrupt 3                  //T1中断用来扫描数码管和计800MS启动模块
{
        TH1=0xf8;
        TL1=0x30;                                 //定时2ms
        Key_();                                        //扫描按键
        Display();                                //扫描显示
        timer++;                                //变量加
        if(flag_BJ==1)                                  //报警开
        {
                Feng=0;
        }
        if(timer>=400)                        //400次就是800ms
        {
                timer=0;
                TX=1;                                  //800MS  启动一次模块
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                _nop_();
                TX=0;
        }
}
/**********************************************************************************************************/
//主函数
void main(void)
{  
        TMOD=0x11;                   //设T0为方式1，GATE=1；
        TH0=0;
        TL0=0;         
        TH1=0xf8;                   //2MS定时
        TL1=0x30;
        ET0=1;                                //允许T0中断
        ET1=1;                           //允许T1中断
        TR1=1;                           //开启定时器
        EA=1;                                        //开启总中断
        init_eeprom();  //开始初始化保存的数据       
        while(1)
        {
                while(!RX);                //当RX为零时等待
                TR0=1;                        //开启计数
                while(RX);                //当RX为1计数并等待
                TR0=0;                        //关闭计数
                Count();                        //计算
        }
}