将N个步进电机时序延迟数组合并为一个主数组的高效方法

针对Teensy 4.1上6台带独立加速度曲线的步进电机同步需求，核心思路是将每台电机的延迟间隔数组转换为时间戳事件队列，通过合并排序后按时间顺序触发脉冲，替代复杂的迭代比对逻辑。以下是模块化的实现方案：

核心思路 #

1. 事件建模：把每个电机的每一步脉冲抽象为「电机ID + 累计触发时间戳」的事件，时间戳由延迟数组累加得到。
2. 事件合并排序：将所有电机的事件合并到一个列表，按时间戳升序排序，得到全局的脉冲触发序列。
3. 同步执行：遍历排序后的事件列表，计算相邻事件的时间差，等待对应时长后触发电机脉冲。

代码实现 #

1. 定义事件结构体 #

#include <vector>
#include <algorithm>

// 步进电机事件：记录目标电机编号和触发的累计时间（微秒）
struct StepEvent {
    uint8_t motorId;
    uint32_t timestampUs;

    // 重载小于运算符，用于按时间戳升序排序
    bool operator<(const StepEvent& other) const {
        return timestampUs < other.timestampUs;
    }
};

2. 单电机延迟数组转事件列表 #

将单台电机的延迟间隔数组（每一步的脉冲间隔）转换为累计时间戳的事件列表：

// 将单电机的延迟数组（单位：微秒）转换为累计时间戳事件队列
std::vector<StepEvent> convertDelaysToEvents(uint8_t motorId, const uint32_t* delays, size_t stepCount) {
    std::vector<StepEvent> events;
    events.reserve(stepCount); // 预分配内存提升效率

    uint32_t currentTimestamp = 0;
    for (size_t i = 0; i < stepCount; ++i) {
        currentTimestamp += delays[i];
        events.push_back({motorId, currentTimestamp});
    }
    return events;
}

3. 多事件合并与排序 #

将所有电机的事件列表合并为一个全局序列，并按时间戳排序：

// 合并所有电机的事件队列，按时间戳升序排序
std::vector<StepEvent> mergeMotorEvents(const std::vector<std::vector<StepEvent>>& allMotorEvents) {
    std::vector<StepEvent> mergedEvents;

    // 合并所有事件到一个容器
    for (const auto& motorEvents : allMotorEvents) {
        mergedEvents.insert(mergedEvents.end(), motorEvents.begin(), motorEvents.end());
    }

    // 按时间戳排序，得到全局同步的触发序列
    std::sort(mergedEvents.begin(), mergedEvents.end());

    return mergedEvents;
}

4. 同步执行脉冲 #

遍历排序后的事件列表，精准控制每一步的触发时机：

// 执行同步步进：按时间序列触发对应电机的脉冲
// 参数：events - 排序后的事件列表；stepCallback - 电机脉冲触发回调函数
void executeSynchronizedSteps(const std::vector<StepEvent>& events, void (*stepCallback)(uint8_t)) {
    if (events.empty()) return;

    uint32_t prevTimestamp = micros();
    for (const auto& event : events) {
        // 计算需要等待的时长：当前事件时间戳 - 上一个事件时间戳
        uint32_t waitUs = event.timestampUs - prevTimestamp;
        if (waitUs > 0) {
            // 用micros()循环等待，避免delayMicroseconds的最大时长限制（Teensy支持大数值，但循环更可靠）
            uint32_t startWait = micros();
            while (micros() - startWait < waitUs);
        }

        // 触发对应电机的脉冲
        stepCallback(event.motorId);

        prevTimestamp = event.timestampUs;
    }
}

5. 使用示例 #

// 示例：定义6台电机的延迟数组（模拟不同的加速度曲线）
uint32_t motor0Delays[] = {1000, 900, 800, 750, 700, 700}; // 加速后匀速
uint32_t motor1Delays[] = {1200, 1100, 1000, 950};
uint32_t motor2Delays[] = {800, 750, 700, 650, 600, 600, 600};
// ... 补充剩余3台电机的延迟数组

// 电机引脚定义（示例）
const uint8_t motorPins[] = {2, 3, 4, 5, 6, 7};

// 电机脉冲触发回调：输出单步脉冲
void triggerMotorStep(uint8_t motorId) {
    digitalWrite(motorPins[motorId], HIGH);
    delayMicroseconds(10); // 脉冲宽度，根据电机驱动器调整
    digitalWrite(motorPins[motorId], LOW);
}

void setup() {
    // 初始化电机引脚为输出模式
    for (uint8_t pin : motorPins) {
        pinMode(pin, OUTPUT);
    }

    // 转换所有电机的延迟数组为事件队列
    std::vector<std::vector<StepEvent>> allMotorEvents;
    allMotorEvents.push_back(convertDelaysToEvents(0, motor0Delays, sizeof(motor0Delays)/sizeof(uint32_t)));
    allMotorEvents.push_back(convertDelaysToEvents(1, motor1Delays, sizeof(motor1Delays)/sizeof(uint32_t)));
    allMotorEvents.push_back(convertDelaysToEvents(2, motor2Delays, sizeof(motor2Delays)/sizeof(uint32_t)));
    // ... 添加剩余3台电机的事件队列

    // 合并排序得到全局同步序列
    std::vector<StepEvent> synchronizedEvents = mergeMotorEvents(allMotorEvents);

    // 执行同步步进
    executeSynchronizedSteps(synchronizedEvents, triggerMotorStep);
}

void loop() {
    // 按需重复执行同步运动或添加其他逻辑
}

优化方向 #

• 内存优化：如果单电机步数极多（如上万步），可改用std::priority_queue实时获取下一个最早触发的事件，无需一次性存储所有事件，大幅节省内存。
• 硬件精度提升：Teensy 4.1支持硬件定时器中断，可将事件列表加载到定时器，用中断触发脉冲，避免循环等待占用CPU，精度更高。
• 运动曲线模块化：封装AccelerationProfile类，用于生成梯形、S曲线等自定义加速度的延迟数组，统一管理每台电机的运动参数。

内容的提问来源于stack exchange，提问作者jonathan