双电机龙门同步控制伪代码

system_init()
{
    init_encoder();
    init_drive();
    init_limit_input();
    init_emergency_stop();

    init_axis_controller(left_axis);
    init_axis_controller(right_axis);
    init_gantry_sync_controller();

    state = IDLE;
}

gantry_home()
{
    state = HOMING;

    home_single_axis(left_axis);
    left_axis.home_offset = read_raw_encoder(left_axis);

    home_single_axis(right_axis);
    right_axis.home_offset = read_raw_encoder(right_axis);

    left_axis.position = 0.0f;
    right_axis.position = 0.0f;

    sync_offset = left_axis.position - right_axis.position;

    planner.position_ref = 0.0f;
    planner.velocity_ref = 0.0f;
    planner.target_position = 0.0f;

    reset_pid(common_pid);
    reset_pid(left_axis.position_pid);
    reset_pid(left_axis.velocity_pid);
    reset_pid(right_axis.position_pid);
    reset_pid(right_axis.velocity_pid);

    state = READY;
}

control_tick_1khz()
{
    if (state == FAULT)
    {
        disable_drive_output();
        return;
    }

    // 1. Read sensor
    x1 = read_encoder(left_axis);
    x2 = read_encoder(right_axis);
    v1 = estimate_velocity(left_axis);
    v2 = estimate_velocity(right_axis);

    // 2. Update motion planner
    planner_step();
    x_ref = planner.position_ref;
    v_ref = planner.velocity_ref;

    // 3. Compute error
    x_avg = 0.5f * (x1 + x2);
    e_avg = x_ref - x_avg;
    e_sync = (x1 - x2) - sync_offset;
    v_sync = v1 - v2;

    // 4. Cross-coupled controller
    common_trim = PID_common(e_avg);
    sync_trim = Kps * e_sync + Kds * v_sync;

    sync_trim = clamp(sync_trim, -SYNC_TRIM_LIMIT, SYNC_TRIM_LIMIT);
    common_trim = clamp(common_trim, -COMMON_TRIM_LIMIT, COMMON_TRIM_LIMIT);

    pos_cmd_left  = x_ref + common_trim - sync_trim;
    pos_cmd_right = x_ref + common_trim + sync_trim;

    // 5. Axis servo loop
    axis_step(left_axis, pos_cmd_left, v_ref);
    axis_step(right_axis, pos_cmd_right, v_ref);

    // 6. Safety monitor
    if (abs(e_sync) > SYNC_LIMIT)
    {
        state = FAULT;
        emergency_stop();
    }
}

axis_step(axis, pos_cmd, vel_ff)
{
    pos_error = pos_cmd - axis.position;
    vel_cmd = PID_position(axis.position_pid, pos_error) + vel_ff;
    vel_cmd = clamp(vel_cmd, -VEL_LIMIT, VEL_LIMIT);

    vel_error = vel_cmd - axis.velocity;
    iq_cmd = PID_velocity(axis.velocity_pid, vel_error);
    iq_cmd = clamp(iq_cmd, -IQ_LIMIT, IQ_LIMIT);

    foc_current_control(axis, iq_cmd);
}

fault_handler()
{
    disable_drive_output();
    latch_fault_code();
    state = FAULT;
}

fault_reset()
{
    if (emergency_stop_released() && limit_status_ok())
    {
        clear_fault_latch();
        reset_pid(common_pid);
        reset_pid(left_axis.position_pid);
        reset_pid(left_axis.velocity_pid);
        reset_pid(right_axis.position_pid);
        reset_pid(right_axis.velocity_pid);
        state = READY;
    }
}