机器人的控制除了算法之外，都还需要一个状态机，实现机器人的启停、模式切换等。对轮子超过3个的轮式机器人来说，所有控制模式对应的机器人物理状态都十分接近，即使发生错误切换，也不会造成特别大的危险。但足式机器人状态特别多，包括但不限于卧倒、站立、行走、摔倒等。单一的控制算法只能运行在特定的状态下，如果在摔倒的机器人上跑一个奔跑的算法，那机器人很可能就Boom了。
如果状态机设计好了，下一个问题就是怎么用遥控器切换状态。遥控器的按钮一定比机器人需要的状态多，如果没有，那就换一个通道更多的遥控器。但是如果简单的设计成一个键对应一个状态，那就会很难用，因此还需要一个符合直觉的操作逻辑，除非操作手能够把状态机跑在自己的脑子里。
鉴于自己经常记不住自己设计的状态机，也被吐槽过逻辑过于复杂，所以就写篇文章记录一下现有的和自己设计的一些状态机和操作指令设计思路。当然，我自己设计的状态机和操作逻辑主要追求是“有趣”，并不是完全合理。
宇树官方控制器
上图是Go1启停部分的逻辑。由于周末在家手头没有狗，凭记忆和说明书想不起运动状态的控制逻辑，就只能把启停部分大概画出来。总共有锁定、运动、阻尼三种模式。[1]
锁定模式我猜是固定角度的位置控制，即目标位置恒定，kp较大，kd为0的PD控制。锁定模式分站立和卧倒两种。L2+A是锁定模式的核心操作，其他状态按L2+A都会进入锁定模式，而锁定模式下按L2+A是切换站立和卧倒。锁定模式是一个相对安全可控的模式，机器人开机启动之后就是锁定站立状态。这个模式也可以用来一个人临时搬机器人，只要内存不被宇宙射线打飞，还是相对安全的。
运动模式下可以通过摇杆控制机器人行走或者晃动，或者通过按键展示特定动作。运动模式是一个比较危险的状态，所以被设计成只能从锁定站立状态下按START进入。
阻尼模式我猜是目标速度为0的速度控制，即目标速度恒定为0，kp为0，kd较大的PD控制。这是一个安全模式，所有状态下都可以通过L2+B进入，特别是紧急状态。阻尼模式机器人处在被动状态，躺下后和关机时状态一样。
所以整体的控制逻辑就是开机后按START开始运动，出现状况时按L2+B急停，安全之后按L2+A站起来，START重新恢复运动。关机时，静止状态下L2+A锁定卧倒，再L2+B趴下，然后断电。如果不心疼地板和狗的话，直接L2+B趴下断电也行。
自用Go1调试模式状态机
理论上保护架或者吊车才是安全的调试方式。保护架悬挂启动也是宇树官方推荐的调试启动方式。但是这些方式都不够优雅，也不方便一个人操作。为了方便调试，我参考赛车的序列式变速箱，自己写了状态机。
状态机总共大阻尼、小阻尼、过渡、锁定、测试五种状态。大阻尼小阻尼模仿官方的阻尼模式设计，分别使用了不同的阻尼数值。大阻尼模式保留了L2+B急停的逻辑，所有模式下L2+B都会跳转到大阻尼，数值比较大是为了摔倒的时候阻力大一些。小阻尼模式阻尼的数值小，是为了人工摆放机器人位置的时候省力一些，也是为了降低噪音。过渡模式的目的是实现站立、趴下的功能，实现方式是设定一个站立的目标位置，然后渐变kp，kp从0增大到目标值即起立过程，而kp从工作的数值逐渐减小到0即趴下的过程。锁定模式，即目标位置恒定的控制，从其他模式切换到锁定时，把位置指令固定在前一状态的最后一个指令。最后的测试模式，即运行需要测试的控制器，也就是最危险的模式。
状态切换的逻辑模仿了序列式变速箱，大阻尼、小阻尼、过渡、锁定、测试五种模式，除了大阻尼是安全模式外，其他模式只能依次切换。“降档”逻辑通过R1实现，在测试模式下，按R1切换到锁定模式，再按卧倒，3秒后自动触发小阻尼模式，再按变成大阻尼，越来越安全，一直按就行了。“升档”逻辑出于安全起见，通过R2和START共同实现，在比较安全的阻尼模式下，通过R2从大阻尼变成小阻尼，进入工作模式，需要按START升到起立模式，3秒站稳之后，再按START才能进入测试模式。R2到START的变化，以及过渡态的3秒，都是为了避免连按出错。调试状态程序启动时，出于阻尼模式和工作模式的中间态，也遵循一样的逻辑，按START升档，按R1降档。
当然这个状态机和宇树的官方状态机没有太大的区别，只是按键逻辑有一些不一样。具体来讲，官方的状态机里面，L2+A同时对应了站起来和卧倒两个功能，操作者需要确认L2+A的结果之后，才能进行下一步有效的操作。而这里的序列式逻辑下，每个按键自己的逻辑都是一致的，操作者按多按少，都没有什么影响。在宇树的官方控制器里面，关机操作需要先L2+A，在L2+B，而在这个方法下，只需要狂按R1就能安全趴下。启动的时候，官方控制器需要先L2+A，确保站起来，再START。L2+A不能多按。而我的方案可以先狂按R2，R2无效之后，再狂按START，就可以基本安全启动了。
自用多步态切换状态机
之前步态切换的文章，一个控制器实现了十几种步态之间的两两切换。[2]普通手柄显然不能以人类能理解的方式实现这个切换，所以用了天地飞的遥控器。天地飞有SA-SH 8个拨杆，这些拨杆像开关一样，可以停留在2-3个位置。其中SA-SD有3档，SE-SH有2档。
那个控制器上，我用SA-SD四分拨杆，模拟了一个有8个状态的H档。简单期间，我们把每个拨杆上中下的位置分别定义为-1, 0, 1. 有且仅有一个拨杆非0状态为有效指令，即8个状态中的一个，其他都是无效指令。8个有效指令就是(-1, 0, 0, 0), (1, 0, 0, 0), (0, -1, 0, 0), (0, 1, 0, 0) … 当状态机读取到无效指令时，停留在上一个状态；读到有效指令时，切换到该指令对应的状态。那么实际操作起来，如果要从1挡切换到4挡，只需要先把SA从-1拨到0，变成无效指令，状态机保持在1挡；再把SB从0拨到1，进入有效指令，状态机切换到4挡。当然由于无效指令太多了，有好多种拨法。
理论上这4个拨杆总共有81个状态，但是为了实现两两切换，以及好操作，这81个状态只有8个有效状态，剩下72个都被当做无效指令，用于过渡。虽然浪费，但是这个逻辑像极了手动挡换挡，有趣就行。