求一段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

}

//12MHz

#include <reg51.h>

void InitTimer0(void)

{

TMOD = 0x01

TH0 = 0x0B1

TL0 = 0x0E0

EA = 1

ET0 = 1

TR0 = 1

}

void delay(void) //误差 0us 延时1ms 此处可以修改高电平汪陆周期

//修改困逗顷此处的延时可以更改舵机转的角度 ，45度具体是多少 你指备可以试试

{

unsigned char a,b,c

for(c=1c>0c--)

for(b=142b>0b--)

for(a=2a>0a--)

}

void main(void)

{

InitTimer0()

P1_2=0

while(1)

}

void Timer0Interrupt(void) interrupt 1

{

//20ms中断

TH0 = 0x0B1

TL0 = 0x0E0

P1_2=1

delay()

P1_2=0

}

---