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

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

文章目录

1 开发阶段
2 软件单元设计
- 2.1 按键锁定模块
- 2.2 电机控制模块
- - 2.2.1 摇杆信号解析
  - 2.2.2 电机参数计算
- 2.3 舵机控制模块
- 2.4 继电器控制模块
3 总结

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 总结

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

>>返回个人博客总目录

机甲大师机器人控制（四）：软件单元相关推荐

机甲大师机器人控制（一）：概念与流程
本文是机甲大师机器人控制的系列博客之一.本文主要介绍了机甲大师机器人以及博主在开发其控制策略时所采用的流程.本系列博客的模型和代码已经上传CSDN资源. 系列博客: 机甲大师机器人控制(一):概念与流 ...
机甲大师机器人控制（三）：软件架构设计
本文是机甲大师机器人控制的系列博客之一.在功能分析的基础上,本文设计软件的架构以及定义输入输出接口. 系列博客: 机甲大师机器人控制(一):概念与流程机甲大师机器人控制(二):功能分析机甲大师机器 ...
机甲大师机器人控制（六）：集成测试
本文是机甲大师机器人控制的系列博客之一.在软件单元测试阶段完成后,进行集成测试.本文内容与功能分析阶段相对应.另外,阅读本文的内容需要一些STM32调试经验. 系列博客: 机甲大师机器人控制(一):概 ...
机甲大师机器人控制（二）：功能分析
本文是机甲大师机器人控制的系列博客之一.本文介绍了博主在开发控制策略的前期对其进行的功能分析并熟悉一些基本原理. 系列博客: 机甲大师机器人控制(一):概念与流程机甲大师机器人控制(二):功能分析 ...
机甲大师机器人控制（七）：购买清单
本文详细介绍博主在DIY机甲大师机器人的过程中所购买的零件和工具. 系列博客: 机甲大师机器人控制(一):概念与流程机甲大师机器人控制(二):功能分析机甲大师机器人控制(三):软件架构设计机甲大 ...
大疆机甲大师教育机器人Python开发：中文命名变量初尝试
简言之, 官方例程中的Twister程序, 略加修改添加了一个中文变量, 运行成功! def start()速度 = 120...chassis_ctrl.set_rotate_speed(速度)gi ...
python 中文变量名_大疆机甲大师Python开发: 中文命名变量初尝试
简言之, 官方例程中的Twister程序, 略加修改添加了一个中文变量, 运行成功! def start() 速度 = 120 ... chassis_ctrl.set_rotate_speed(速度 ...
zpl指令打印中文内容_大疆机甲大师Python开发: 中文命名变量初尝试
简言之, 官方例程中的Twister程序, 略加修改添加了一个中文变量, 运行成功! def start()速度 = 120...chassis_ctrl.set_rotate_speed(速度)gi ...
RoboMaster机甲大师——视觉组——总结、未来期望与比赛经验分享
RoboMaster机甲大师--视觉组--总结与未来期望 FOR THE SIGMA FOR THE GTINDER FOR THE ROBOMASTER 入坑指南各兵种需要的视觉: 步兵-每年必备 ...

机甲大师机器人控制（四）：软件单元