我正好手上有写个舵机的程序，贴出来给你参考一下。
我看了一下，你这个程序主要只分了20个档，要增加档位就要把定时器中断时间给减小，但是51单片机定时器中断时间越短就越不准，我建议用delay来写，我就是用的delay的方法。


#include "include.h"

//declare SFR associated with the ADC
sfr                ADC_CONTR = 0xC5;                //ADC control register
sfr                ADC_RES = 0xC6;                        //ADC high 8-bit result register
sfr                ADC_LOW2 = 0xC7;                //ADC low 2-bit result register
sfr                P1ASF = 0X97;                        //P1 secondary function control register

//Define ADC operation const for ADC_CONTR
#define        ADC_FLAG        0x10                //ADC complete
#define        ADC_START        0x08                //ADC start control bit
#define ADC_SPEEDHH        0x00                //89 clocks
#define ADC_SPEEDH        0x20                //179 clocks
#define ADC_SPEEDL        0x40                //356 clocks
#define ADC_SPEEDLL        0x60                //534 clocks

void        InitADC(void);
u16                GetADCResult(u8 ch);
sbit         PWM0 = P2^0;
bit                p_flag = 1;
u8                Speed_Level=1;
u8                wait_count = 0;
u8                ADC_AngleValue = 0;
u8                ADC_PreAngleValue = 0;
u8                ADC_SpeedValue = 0;
u8                ADC_PreSpeedValue = 0;
u8                PWM_Flag = 0;
s16                PWM_PreValue = 0;
u16                PWM_TrueValue = 0;
s16                PWM_Value = 0;
u8                 Median_data0[10] = {0};
u8                Median_num0 = 0;
u8                Average0[2] = {0};
u8                Average0_num = 0;
u8                 Median_data1[10] = {0};
u8                Median_num1 = 0;
u8                Average1[2] = {0};
u8                Average1_num = 0;
u8                Final_data;
bit                start_key, start_trg, start_cont, done_flag = 0, key_flag = 0;

u8 Median(u8 *str, u8 num)
{
        u8 i = 0;
        u8 j = 0;
        u8 Temp = 0;

        for(i=0;i<num-1;i++)
        {
                for(j=i+1;j<num;j++)
                {
                        if(str[i]<str[j])
                        {
                                Temp=str[i];
                                str[i]=str[j];
                                str[j]=Temp;
                        }
                }
        }
        return str[num/2];
}

u16        GetADCResult(u8 ch)
{
        ADC_CONTR = ADC_SPEEDLL | ch | ADC_START;
        _nop_();                                                        //must wait before inquiry
        _nop_();
        _nop_();
        _nop_();
        while(!(ADC_CONTR & ADC_FLAG));                //wait complete flag
        ADC_CONTR &= ~ADC_FLAG;                                //closed ADC

        return ADC_RES;
}

void InitADC()
{
        P1ASF = 0xff;                                //open 8 channels ADC function
        ADC_RES = 0;                                //clear previous result
        ADC_CONTR = ADC_SPEEDLL;
        delayms(5);
}

void ADC_Handle(void)
{
                Median_data1[Median_num1] = GetADCResult(1);
                Median_num1++;
                if(Median_num1>9)
                {
                        Median_num1 = 0;
                        Average1[Average1_num] = Median(Median_data1,10);        //&#214;D&#214;μ&#194;&#203;2¨
                        Average1_num++;
                        if(Average1_num>1)
                        {
                                Average1_num = 0;
                                ADC_AngleValue = (Average1[0]/2 + Average1[1]/2)/2;        //&#198;&#189;&#190;ù&#214;μ
                        }
                }

                Median_data0[Median_num0] = GetADCResult(0);
                Median_num0++;
                if(Median_num0>9)
                {
                        Median_num0 = 0;
                        Average0[Average0_num] = Median(Median_data0,10);        //&#214;D&#214;μ&#194;&#203;2¨
                        Average0_num++;
                        if(Average0_num>1)
                        {
                                Average0_num = 0;                                                          
                                ADC_SpeedValue = Average0[0]/2 + Average0[1]/2;        //&#198;&#189;&#190;ù&#214;μ
                        }
                }


                if(abs(ADC_PreSpeedValue-ADC_SpeedValue)>16)
                {
                        ADC_PreSpeedValue = ADC_SpeedValue;
                }
                else
                {
                        ADC_SpeedValue = ADC_PreSpeedValue;
                }
                Speed_Level = ADC_SpeedValue/32 + 1;

                if(abs(ADC_PreAngleValue-ADC_AngleValue)>4)
                {
                        ADC_PreAngleValue = ADC_AngleValue;
                }
                else
                {
                        ADC_AngleValue = ADC_PreAngleValue;
                }
}

void Motor_Handle(u8 speed, u8 angle)
{
        u16 i;
        if(done_flag==1)
        {
                if(p_flag==1)
                {
                        PWM_Value += speed;

                        if(PWM_Value >= angle)
                        {
                                PWM_Value = angle;
                                if(PWM_Value == angle)
                                {
                                        wait_count++;
                                        if(wait_count>=20)
                                        {
                                                wait_count = 0;
                                                p_flag = 0;
                                        }
                                }
                        }
                }
                else
                {
                        if(speed>4)
                        {
                                if(PWM_Value<40)
                                {
                                        speed = 4;
                                }
                                if(speed>2)
                                {
                                        if(PWM_Value<10)
                                        {
                                                speed = 2;
                                        }
                                }
                        }
                        PWM_Value-=speed;
                        if(PWM_Value<=0)
                        {
                                PWM_Value = 0;
                                if(PWM_Value == 0)
                                {
                                        done_flag = 0;
                                }
                        }
                }
        }
        PWM_TrueValue = PWM_Value;

        PWM0 = 1;
        for(i=0; i<=65; i++){}
        for(i=0; i<=PWM_TrueValue; i++){}
        PWM0 = 0;
}

void main()
{
        u16 i;       
        TIMER_Init();
        UART_Init();
        InitADC();
        while(1)
        {
                if(vKeyScan()==1)
                {
                        start_key = 1;
                        start_trg = start_key & (start_key ^ start_cont);
                        start_cont = start_key;
                }
                else
                {
                        start_key = 0;
                        start_trg = 0;
                        start_cont = 0;
                }
                if(start_trg && start_cont)
                {
                        key_flag = 1;
                }
                if(done_flag==0)
                {
                        if(start_trg && start_cont)
                        {
                                done_flag = 1;
                                p_flag = 1;
                                key_flag = 0;
                        }
                }
                else
                {
                        key_flag = 0;
                }               
                ADC_Handle();
                if(timer1_count>=21)
                {
                        timer1_count = 0;
                        Motor_Handle(Speed_Level, ADC_AngleValue);
//                        UART_SendData(Speed_Level);
//                        UART_SendData(ADC_AngleValue);               
                }               
        }
}