档位控制

uchar gear_mode = 0;                    // 当前档位 0/1/2/3
int  max_angle[4] = {0, 60, 120, 180}; // 每档最大角度
bit  sweep_up = 1;                      // 1=上升 0=下降
uint cur_angle = 0;                     // 当前角度(0~180)

uint angle_to_duty(uint angle) {
    return (uint)(250 + ((unsigned long)angle * 1000 / 180));
}

void servo_sweep(void) {
    uint step = 3;                     // 步长3°, 越大越快
    uint target = max_angle[gear_mode];

    if(gear_mode == 0) {
        cur_angle = 0;
        PWM_change(20, 250);           // 停住0°
        return;
    }

    if(sweep_up) {
        cur_angle += step;
        if(cur_angle >= target) {
            cur_angle = target;
            sweep_up = 0;              // 到顶, 转头
        }
    } else {
        if(cur_angle <= step) {
            cur_angle = 0;
            sweep_up = 1;              // 到底, 转头
        } else {
            cur_angle -= step;
        }
    }

    PWM_change(20, angle_to_duty(cur_angle));
}

void main(void) {
    // ===== 初始化 (不变) =====
    GPIO_init_allpin(0);
    GPIO_init_pin(27, 1);
    UART_init(1, 115200, 1, 1);
    UART_init(2, 115200, 1, 0);
    PWM_init(20, 50, 250);    // 初始0°
    EA = 1;

    uint sweep_timer = 0;     // 往复计时器

    while(1) {
        // ===== ① 串口帧处理 (不变) =====
        if(next != current) {
            switch(UART_FLAG) {
                case 0: {
                    if(UART_DATA[current] == 0x5a) {
                        uart2_data[ia] = UART_DATA[current];
                        ia++;
                        UART_FLAG = 1;
                    }
                } break;
                case 1: {
                    if(UART_DATA[current] == 0xa5) {
                        uart2_data[ia] = UART_DATA[current];
                        ia++;
                        UART_FLAG = 2;
                    }
                } break;
                case 2: {
                    if(ia == 2) {
                        uart2_data[ia] = UART_DATA[current];
                        ia++;
                        ja = uart2_data[2] + 1;
                        UART_FLAG = 3;
                    }
                } break;
                case 3: {
                    ja--;
                    uart2_data[ia] = UART_DATA[current];
                    ia++;
                    if(ja == 1) {
                        ia = 0;

                        // ★ 每收一帧 → 切一档, 不复位方向
                        gear_mode++;
                        if(gear_mode >= 4) gear_mode = 0;

                        UART_1(1, uart2_data);
                        led01(uart2_data);
                        UART_FLAG = 0;
                    }
                } break;
            }
            current++;
        }

        // ===== ② 舵机自动往复 (只此一处新增) =====
        sweep_timer++;
        if(sweep_timer >= 600) {       // 调这个数改速度
            sweep_timer = 0;
            servo_sweep();             // 每600次循环走1步
        }
    }
}

原来的 pwm_p20():  收帧→切档+走一步  (删掉不用了)
新的 servo_sweep(): 只做往复运动, 自己循环跑

main() 里:
  收帧 → gear_mode++  (只切档)
  while(1)末尾 → 自动调 servo_sweep() (舵机一直动)

if(sweep_timer >= 600)    // 600 = 慢
if(sweep_timer >= 200)    // 200 = 快
if(sweep_timer >= 50)     // 50  = 飞快

上电 → 0°静止
发1帧 → 切1档 → 舵机自己 0↔60° 来回摆
发1帧 → 切2档 → 舵机自己 0↔120° 来回摆
发1帧 → 切3档 → 舵机自己 0↔180° 来回摆
发1帧 → 切0档 → 舵机回0° 停住