求一段c语言舵机程序

给段伪代码吧

int main()

{

    初始化 src_angle_A, dst_angle_A

    初始化 src_angle_B, dst_angle_B

    初始化 src_angle_C, dst_angle_C

    初始化 src_angle_D, dst_angle_D

    int A_flag = 0

    int B_flag = 0

    int CD_flag = 0

    

    while (1)

    {

        if (key1_down())

        {

            if  (A_flag == 0)

            {

                set_angle_A(dst_angle_A)

            }

            else

            {

                set_angle_A(src_angle_A)

            }

            A_flag = 1-A_flag

        }

        

        if (key2_down())

        {

            if  (B_flag == 0)

            {

                set_angle_B(dst_angle_B)

            }

            else

            {

                set_angle_B(src_angle_B)

            }

            B_flag = 1-B_flag

        }

        

        if (key3_down())

        {

            if (CD_flag == 0)

            {

                set_angle_C(dst_angle_C)

                set_angle_D(dst_angle_D)

            }

            else

            {

                set_angle_C(src_angle_C)

                set_angle_D(src_angle_D)

            }

            CD_flag = 1-CD_flag

        }

        delay_ms(10)

    }

    return 0

}

#include<reg52.h>typedef unsigned int uinttypedef unsigned char ucharsbit pwm=P0^3//输出PWM信号uchar count=0uint pwm_value=1500//初值为1.5msuint value[]={1500,1000,1500,2000,1500}//void delay_ms(uint x){uint iwhile(x--)for(i=0i<125i++)}void InitTimer(void){TMOD=0x11//开定时器0,1TH0=-20000/256//定时20MS,20MS为一个周期TL0=-20000%256TH1=-1500/256//定时1.5MS,这时舵机处于0度TL1=-1500%256EA=1//开总断TR0=1//开定时器0ET0=1TR1=1//开定时器1ET1=1}void timer0(void) interrupt 1//定时器0中断函数{pwm=1TH0=-20000/256TL0=-20000%256TR1=1count++}void timer1(void) interrupt 3//定时器1中断函数{pwm=0TH1=-pwm_value/256TL1=-pwm_value%256TR1=0}void main(void)//主函数{uchar iInitTimer()pwm_value=1500while(1){for(i=0i<5i++){pwm_value=value[i]//通过数组就可以改变舵机的转角度delay_ms(1000)}}}

---