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

ruohuyang@163.com (Cygnus Yang) — Sat, 23 May 2026 00:00:00 +0800

1. 上电与初始化

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;
}

2. 回零流程

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;
}

3. 1 kHz 同步控制主循环

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();
 }
}

4. 单轴控制伪代码

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);
}

5. 故障与恢复

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;
 }
}

6. 现场落地时的替换点

将伪代码替换为具体硬件实现时，重点替换以下接口：

双电机龙门同步控制方案

ruohuyang@163.com (Cygnus Yang) — Sat, 23 May 2026 00:00:00 +0800

核心问题

双侧驱动龙门在长时间运行后，横梁会发生扭斜甚至卡死。

双电机龙门同步控制硬件方案

ruohuyang@163.com (Cygnus Yang) — Sat, 23 May 2026 00:00:00 +0800

1. 项目背景与需求

1.1 应用场景

双电机驱动的龙门式直线运动机构，用于精密定位同步控制。

无刷电机与有刷电机对比分析

ruohuyang@163.com (Cygnus Yang) — Sat, 23 May 2026 00:00:00 +0800

龙门同步控制场景下的电机选型指南

概述

在双电机龙门同步控制系统中，电机选型直接影响系统的性能、可靠性和成本。本文针对龙门同步控制场景，对比分析无刷电机（BLDC）与有刷电机（Brushed DC）的特性，并提供选型建议。

02 双电机龙门同步控制 - Cygnus Tech Blog

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

1. 上电与初始化

2. 回零流程

3. 1 kHz 同步控制主循环

4. 单轴控制伪代码

5. 故障与恢复

6. 现场落地时的替换点

双电机龙门同步控制方案

核心问题

双电机龙门同步控制硬件方案

1. 项目背景与需求

1.1 应用场景

无刷电机与有刷电机对比分析

龙门同步控制场景下的电机选型指南

概述