本文是机甲大师机器人控制的系列博客之一。在软件架构设计的基础上，本文设计每个模块中的控制算法。

系列博客：
机甲大师机器人控制（一）：概念与流程
机甲大师机器人控制（二）：功能分析
机甲大师机器人控制（三）：软件架构设计
机甲大师机器人控制（四）：软件单元
机甲大师机器人控制（五）：软件单元测试
机甲大师机器人控制（六）：集成测试
机甲大师机器人控制（七）：购买清单

1 开发阶段

软件单元阶段在软件架构设计阶段之后，同时会参照功能分析阶段中的策略，在Simulink模型的子系统中搭建控制模型。

另外说明一下，由于博主不是专业做控制策略的工程师，所以简化了该阶段的建模过程。在汽车软件开发中，闭环控制、滤波、诊断等技术都非常重要，但是都没有在这个DIY项目中体现出来。如果有做控制策略的同行，非常希望能够多多交流。

2 软件单元设计

本章会根据软件架构设计的内容，在每个子系统中进行细节上的建模，以实现功能分析阶段中提出的4个功能。

2.1 按键锁定模块

按键锁定模块是为了实现功能分析阶段中提出的第1条按键锁定功能，对应的子系统是顶层的使能子系统。该子系统在软件架构设计阶段就已经建立完成。

如上图所示，VeINPR_uint8_PS2MODE变量控制这个子系统的使能。双击进入使能子系统后，其中的每个输出Port需要特别注意。

1）对于4个直流电机调速的参数VeOUTR_int32_XXWheel，Initial Output定义为0，Output when disabled选择为reset，如下图所示。

Initial Output定义为1500，表示初始时刻4个电机转速为0。Output when disabled选择为reset，表示使能子系统失能的时候，输出的电机调速参数会重置（reset）为0，无论怎么动摇杆，电机都保持停止转动。

2）对于2个舵机位置的PWM参数VeOUTR_uint16_SrvXPWM，Initial Output定义为1500，Output when disabled选择为held，如下图所示。

Initial Output定义为1500，表示初始时刻舵机处于中间位置（1500是500和2500的平均值）。Output when disabled选择为held，表示当子系统失能的时候，舵机位置仍然保持不变（held）。

3）对于继电器控制信号，沿用了舵机的PWM参数VeOUTR_uint16_Srv2PWM，Initial Output定义为500，Output when disabled选择为reset，如下图所示。

Initial Output定义为500，表示初始时刻继电器的信号端输入低电平（500对应的电压大概是0.1V）。Output when disabled选择为reset，表示当子系统失能的时候，继电器的信号端输入重置（reset）为低电平。

2.2 电机控制模块

电机控制模块是为了实现功能分析阶段中提出的第2条底盘平面运动功能，对应的子系统是第二层的电机控制子系统。该子系统在软件架构设计阶段就已经建立完成。

如上图所示，子系统的输入是4个摇杆通道对应的信号，输出是4个电机接口函数传入的参数。

双击进入电机控制子系统Motor_Ctrl，其中包含两大块内容，分别是摇杆信号解析（左）和电机参数计算（右），如下图。后面会按照信号流的过程，来解释一下从输入到输出中间做了些什么。

2.2.1 摇杆信号解析

1）4个通道的摇杆信号首先会经过同样的子系统Rocker_Solve，计算出它的比例（Rocker_Ratio）和激活标志（Rocker_Flag），如下图所示。

摇杆比例（Rocker_Ratio）的范围是-1~1的浮点数，包含了摇杆运动的正负方向和相对于极限值的比例；激活标志（Rocker_Flag）的范围是0或1的正整数，表示是否被激活。内部逻辑其实很简单，就是简单地把输入做了个除法和判断。

由于4路通道都要进行这个计算，所以博主把这个模块做成了一个库（Lib），并配置成原子子系统，以便生成代码的时候可以重复调用一个函数。

2）对于输出的摇杆比例（Rocker_Ratio），会根据该通道的来计算出这个比例所对应的四轮参数值。计算方法是根据功能分析阶段中的运动规律图得出。

进入Wheel_Solve子系统，可以看到通过Gain模块，对方向做了一个转换。

这里进行一个说明。左前轮（FL_Wheel）、右后轮（BR_Wheel）与PS2LX_Ratio符号一致，右前轮（FR_Wheel）、左后轮（BL_Wheel）与PS2LX_Ratio符号相反。和运动关系图中的右平移运动相吻合。左平移和右平移相反，但是摇杆符号也是负号，所以也是适用于Wheel_Solve的计算方法。

3）将4个通道解析出来的激活标志（Rocker_Flag）相加，得出当前周期内，总共有多少通道被激活（Rocker_Num）。

2.2.2 电机参数计算

在功能分析阶段中将底盘平面运动分成基本运动和合成运动。由于基本运动是合成运动的一种特殊情况，所以在电机参数计算的时候，直接按照合成运动来计算。电机参数计算后会输出四个电机接口的参数。

如上图所示是对左前轮（FLWheel）的计算过程，分了三步。首先，将四路摇杆通道算出来的对应的左前轮的值相加。接着将计算的和除以被激活的摇杆数量值（Rocker_Num）。最后乘以一个系数CeMTSR_single_PWMratio，来限值PWM的最大值。

这里有两点解释一下。1）相加后再除以数量表示对最终参数影响的平均效果；2）乘以系数是为了限制输出的最大电压，这个系数在数据字典中定义。

2.3 舵机控制模块

电机控制模块是为了实现功能分析阶段中提出的第3条舵机云台二自由度运动功能，对应的子系统是第二层的舵机控制子系统。该子系统在软件架构设计阶段就已经建立完成。

如上图所示，子系统的输入是4个按键对应的信号，输出是2个舵机接口函数传入的第2个参数参数，也就是位置参数。

双击进入Servo_Ctrl子系统，控制逻辑如下图。

if模块先通过两个按键输入，判断舵机应该正转还是反转；接着UnitDelay的结果作为if-Action子系统的输入，if-Action子系统中加上或者减去一个数值，然后输出该周期时舵机应该处于什么位置；最后用Saturation模块对位置信号做限值，1000~2000表示舵机位置从45度到135度。舵机控制模块总体还是很简单的。

2.4 继电器控制模块

继电器控制模块是为了实现功能分析阶段中提出的第4条发射器控制功能，对应的子系统是第二层的水弹枪控制子系统。该子系统在软件架构设计阶段就已经建立完成。

如上图所示，子系统的输入是R1按键对应的信号，输出是第3个舵机接口函数传入的第2个参数参数，因为在软件架构设计阶段中采用舵机接口来控制继电器。

双击进入Gun_Ctrl子系统，控制逻辑如下图。

这个子系统应该是最简单的（虽然其他的也很简单），只用了一个if-else判断输入信号，然后决定输出信号是高电平（19500）还是低电平（500）。

3 总结

博主在本章中简单的介绍了一下自己控制机甲大师的软件单元。事实上，机甲大师的内部控制算法可以有非常多值得研究的技术点。但是本系列博客的重点在于<基于模型设计>这套方法流程，最终走通了这套流程以后，可以回来修改模型并加以验证，不断优化其中的算法。

>>返回个人博客总目录