14. 插补状态机设计：状态定义、状态转换、异常处理

状态机这东西，说白了就是让程序知道自己「现在在干嘛」以及「下一步该干嘛」。做插补算法时，如果没有一个清晰的状态机，代码很容易变成一团乱麻。我见过不少同行，上来就写一个大循环，里面塞满各种标志位，最后调试到崩溃。

嗯，今天我们就来聊聊，怎么把插补状态机设计得既清晰又健壮。

14.1 为什么插补需要状态机？

你想想看，一个插补任务从开始到结束，要经历多少阶段？

  • 先要解析指令，看看是走直线还是圆弧
  • 然后要规划速度，算加减速
  • 接着要实时计算插补点
  • 最后还要处理终点判断和停止

这些阶段之间，还有各种边界情况。比如中途收到急停指令怎么办？速度规划时发现距离太短怎么办？

我个人习惯，遇到这种多阶段、多条件切换的逻辑，第一反应就是上状态机。它能把复杂的流程拆成一个个独立的状态，每个状态只关心自己的事，状态之间的转换清晰明了。

核心思想： 每个RTOS任务周期，状态机只做一件事——检查当前状态，执行对应动作，然后决定下一个状态。

14.2 状态定义：插补的五个核心阶段

我在项目中，通常把插补状态机定义为以下五个状态。不多不少，刚好覆盖整个生命周期。

状态名称 枚举值 说明
IDLE 0 空闲状态，等待新的插补指令
PARSING 1 解析指令参数，计算起点终点
PLANNING 2 速度规划，计算加减速参数
RUNNING 3 实时插补，输出位置脉冲
STOPPING 4 减速停止或异常终止

为什么是这五个？我解释一下。

IDLE 是起点也是终点。没有任务时，状态机就待在这里，不消耗CPU。我曾经见过有人把IDLE省掉，结果系统一启动就乱跑——因为没有明确的「无事可做」状态。

PARSING 和 PLANNING 是准备阶段。这两个状态只在任务开始时执行一次，不占用实时周期。很多新手把解析和规划放在RUNNING里做，导致每个周期都要重复计算，白白浪费算力。

RUNNING 是核心状态。每个RTOS tick都要执行一次，计算插补点、输出脉冲。这个状态对实时性要求最高。

STOPPING 是安全兜底。不管是正常结束还是异常中断，都要经过这个状态做减速处理。直接跳回IDLE？那电机可能会「哐」一声急停，机械结构受不了。

14.3 状态转换：谁可以到谁？

状态不是随便跳的。我画了一张转换图，你看一眼就明白了。

IDLE 空闲 PARSING 解析 PLANNING 规划 RUNNING 运行 STOPPING 停止 收到指令 解析完成 规划完成 完成/异常 停止完成 解析失败 规划失败 急停信号

这张图里，实线是正常流程，虚线是异常路径。注意看，所有状态都可以直接跳到STOPPING——这是安全设计。一旦出现任何异常，比如电机堵转、指令解析出错，立刻进入停止流程。

我特别强调一点：RUNNING 状态不能直接跳回 IDLE。必须经过 STOPPING 做减速。这是血的教训——我以前有个项目，为了省事让 RUNNING 直接回 IDLE，结果急停时电机直接抱死，把联轴器都扭断了。

14.4 异常处理：状态机的「安全网」

异常处理是状态机设计中最容易被忽视的部分。很多人觉得「正常流程跑通就行了」，但工业现场什么情况都可能发生。

我总结了几类常见异常，以及对应的处理策略：

异常类型 触发条件 处理方式
指令解析错误 G代码格式不对、参数超范围 记录错误码，进入STOPPING
速度规划失败 距离太短无法完成加减速 降速重算，若仍失败则停止
硬件限位触发 碰到行程开关 立即停止，标记紧急停止
跟随误差超限 实际位置与指令位置偏差过大 减速停止，防止撞机
看门狗超时 任务周期未及时喂狗 硬件复位，状态机重置
注意： 异常处理要分级。不是所有异常都要急停。比如速度规划失败，可以先尝试降速重算。但硬件限位触发，必须立即停止，没有商量余地。

14.4 代码实现：状态机骨架

说了这么多理论，来看看实际代码怎么写。我习惯用一个 switch-case 结构，清晰明了。

typedef enum {
    STATE_IDLE = 0,
    STATE_PARSING,
    STATE_PLANNING,
    STATE_RUNNING,
    STATE_STOPPING
} InterpState_t;

typedef struct {
    InterpState_t currentState;
    uint32_t errorCode;
    bool     emergencyStop;
    // 其他插补参数...
} InterpStateMachine_t;

void InterpStateMachine_Run(InterpStateMachine_t *sm) {
    switch (sm->currentState) {
        case STATE_IDLE:
            // 检查是否有新指令
            if (HasNewCommand()) {
                sm->currentState = STATE_PARSING;
            }
            break;

        case STATE_PARSING:
            if (ParseCommand() == OK) {
                sm->currentState = STATE_PLANNING;
            } else {
                sm->errorCode = ERR_PARSE_FAIL;
                sm->currentState = STATE_STOPPING;
            }
            break;

        case STATE_PLANNING:
            if (PlanVelocity() == OK) {
                sm->currentState = STATE_RUNNING;
            } else {
                // 尝试降速重算
                if (RetryWithLowerSpeed() == OK) {
                    sm->currentState = STATE_RUNNING;
                } else {
                    sm->errorCode = ERR_PLAN_FAIL;
                    sm->currentState = STATE_STOPPING;
                }
            }
            break;

        case STATE_RUNNING:
            // 检查急停
            if (sm->emergencyStop || CheckLimitSwitch()) {
                sm->currentState = STATE_STOPPING;
                break;
            }
            // 执行插补
            if (InterpolateOneStep() == FINISHED) {
                sm->currentState = STATE_STOPPING;
            }
            break;

        case STATE_STOPPING:
            if (DoDecelerate() == STOPPED) {
                sm->currentState = STATE_IDLE;
            }
            break;

        default:
            // 未知状态，强制复位
            sm->currentState = STATE_IDLE;
            break;
    }
}
我的经验： 状态机函数要在RTOS任务中周期调用，比如每1ms一次。不要在里面加阻塞等待，否则整个系统都会卡住。如果某个状态需要等待硬件响应，用状态机内部的子状态来实现，而不是用 delay()。

14.5 避坑指南：我踩过的几个坑

做状态机设计这么多年，我踩过不少坑。分享几个典型的：

  • 状态遗漏：我曾经漏掉了 STOPPING 状态，结果正常结束时直接跳 IDLE，电机没有减速，每次结束都「哐」一声。后来加了减速斜坡才解决。
  • 状态转换条件不完整：有一次只考虑了正常转换，没处理解析失败的情况。结果用户输入了一个非法G代码，状态机卡在 PARSING 里出不来。从那以后，我每个状态都加了异常出口。
  • 状态机被多个任务同时调用：RTOS下多个任务访问同一个状态机，不加锁会导致状态错乱。我的做法是：状态机只在一个任务中运行，其他任务通过消息队列发送指令。
  • 看门狗没考虑状态机阻塞：如果某个状态执行时间过长，看门狗会超时复位。我一般在每个状态入口重置看门狗，确保长时间运行的状态不会误触发。

嗯，状态机设计说难不难，说简单也不简单。关键是把每个状态的职责划清楚，把转换条件写完整，把异常路径都考虑到。做到这三点，你的插补状态机就能稳定运行了。

← 上一章 📖 返回目录 下一章 →