51单片机ad转换以及舵机控制求救

我用单片机做的控制程序，希望各路大神帮帮我，我的思路是AD采样，转换成数字信号，数字进行对比，然后输出。
#include"reg51.h"
#include"stdio.h"
#include"intrins.h"
#define uchar unsigned char
//-------------------------------------------------------
#define VCC  5000
sbit pwm1=P0^6;                                        //定义东西方向为PWM1
sbit pwm2=P0^7;                                        //定义南北方向为PWM2
unsigned int DX,NB;
sfr P1ASF     = 0x9D;        //12C5A60AD/S2系列模拟输入(AD或LVD)选择
sfr ADC_CONTR = 0xBC;        //带AD系列
sfr ADC_RES   = 0xBD;        //带AD系列
sfr ADC_RESL  = 0xBE;        //带AD系列
float a,b,c,d,e,f,g,h;
unsigned int xd=20,bn=40;
//-------------------------------------------------------
void AD_init();
void delay(unsigned int a);
float AD_work(unsigned char channel); //数字值转换成电压值
unsigned int AD_get(unsigned char channel);//一次AD转换
//-------------------------------------------------------
void timer_int()
{
        TMOD=0x11;
        TH0=(65536-50)/256;
        TL0=(65536-50)%256;
                    TH1=(65536-50)/256;
        TL1=(65536-50)%256;
        EA=1;
        ET0=1;
        ET1=1;
        TR1=1;
        TR0=1;                                                  
}
void main()
{
  timer_int();
        while(1)
        {
         if(DX<xd)                                //东西调节范围 20 - 100
                 pwm1=1;
                else
                pwm1=0;
           if(DX==400)
                DX=0;
        if(NB<bn)                                 //南北调节范围         35 - 60
                 pwm2=1;
                else
                pwm2=0;
                if(NB==400)
            NB=0;

        }
AD_init();    //A/D转换初始化
while(1)
{   
                                
   b=AD_work(1);//p1.1         //东西方向1     20~100
   c=AD_work(2);//p1.2         //东西方向2           20~100
   d=AD_work(3);//p1.3         //南北方向1           35~60
   e=AD_work(4);//p1.4   //南北方向2           35~60
   if(b>c)
    xd++ ;
   if(b<c)
    xd--;
   if(d>e)
     bn++;
   if(d<e)
    bn--;
   //delay(10);  
}
}
//---------------------------------------------------------
unsigned int AD_get(unsigned char channel)
{
         unsigned int adc;
         ADC_CONTR=0x88|channel;    //开启AD转换1000 1000 即POWER SPEED1 SPEED0 ADC_FLAG   ADC_START CHS2 CHS1 CHS0
         _nop_();
         _nop_();
         _nop_();
         _nop_();//要经过4个CPU时钟的延时，其值才能够保证被设置进ADC_CONTR 寄存器
         delay(10);   //等待转换完成
         adc = (unsigned int)ADC_RES;
         adc = (adc << 2) | (ADC_RESL & 3);               
         ADC_CONTR&=0xe7;      //关闭AD转换，ADC_FLAG位由软件清0  
         return        adc;
}
                  //----------------------------------------------------
float AD_work(unsigned char channel)
{
          float AD_val;     //定义处理后的数值AD_val为浮点数
          unsigned char i;
          for(i=0;i<100;i++)
          AD_val+=AD_get(channel); //转换100次求平均值(提高精度)
          AD_val/=100;
          AD_val=(AD_val*5)/1024; //AD的参考电压是单片机上的5v，所以乘5即为实际电压值
          return AD_val;
}
                  //------------------------------------------------------------------------------
void delay(unsigned int a) //延时约1ms
{
          unsigned int i;
          while (--a！=0)
          for(i=20;i>0;i--);   //1T单片机i=600，若是12T单片机i=125
}
                  //------------------------------------------------------------------------------
                  //------------------------------------------------------------------------------
void AD_init()
{
          P1ASF=0xff; //P1口全部作为模拟功能AD使用
          ADC_RES=0;   //清零转换结果寄存器高8位
          ADC_RESL=0; //清零转换结果寄存器低2位
          ADC_CONTR=0x80;//开启AD电源
          delay(2);   //等待1ms，让AD电源稳定
}

void t0() interrupt 1
{
        TH0=(65536-50)/256;
        TL0=(65536-50)%256;
        DX++;                        
}
void t1() interrupt 3
{
        TH1=(65536-50)/256;
        TL1=(65536-50)%256;
        NB++;                        
}
求求各路大神帮帮我