1 机器人底盘控制器

本项目基于RT-Thread实时操作系统开发了一款机器人底盘控制器系统。系统通过USART接口接收遥控器信号实现手动控制，同时采用基于CANOpen协议的CAN总线与电机控制器通信，实现对驱动电机的精准控制。系统还预留了RS-232接口与ROS上位机通信的能力，为未来扩展提供了可能。整体架构充分利用RT-Thread的实时多任务特性，通过优先级调度确保控制指令的实时响应，为机器人底盘提供了可靠的控制方案。

系统功能架构：

2 硬件资源

MCU: 本系统控制器使用的是自主研发的嵌入式主控单元，采用高性能国产芯片为核心，具有高实时性和强扩展性的特点，可广泛应用于工业控制、机器人、智能装备等领域。

• 主控芯片：N32G455VEL7（国民技术）

  • 主频168MHz

  • 内置512KB Flash和144KB SRAM

• 工作电压：5V DC（内置电源转换）

• 主要通信接口：
  • 串口 ×7（部分支持RS232/RS485）
  • CAN总线 ×2（支持CAN 2.0A/B）
  • RS485 ×2（隔离型）
  • RS232 ×2（标准DB9接口）

外设：时代超群伺服电机、HT-8A航模遥控器、TPS5430DDAR降压模块（24V转5V）、急停物理按键。

3 基础准备

3.1 开发资源

本项目代码可以从PlanRobotShenZhen/Chassis_robot:https://github.com/PlanRobotShenZhen/Chassis_robot 中下载获取。

3.2 软件安装与代码移植

• 安装Keil MDK核心包：可参考下方CSDN博主的安装教程。

  keil5——安装教程附资源包: https://blog.csdn.net/weixin_48100941/article/details/126192218?

• 安装Nationstech设备支持包：

  • 打开下载的资源包，找到并双击 N32G4xx_DFP.x.x.x.pack 自动安装，如下图所示。

• 安装ST-Link驱动：可参考下方CSDN博主的安装教程。

  stlink驱动教程: https://blog.csdn.net/m0_68987050/article/details/146936297?

• 开发库移植步骤过程

  • 打开文件夹找到以下文件并复制。

  • 打开项目工程，新建一个firmware文件夹，把文件复制到此文件夹中。

3.3 RT-Thread源代码移植

本工程的RT-Thread文件已经集成在工程中，如果使用其他芯片作为主控芯片,可以参照官方教程进行文件修改和移植。

官方文档 STM32系列BSP制作教程：https://github.com/RT-Thread/rt-thread/blob/master/bsp/stm32/docs/STM32系列BSP制作教程.md

4 理论基础

4.1 RS-232协议

RS-232 是一种经典的串行通信标准，由美国电子工业协会（EIA）于1962年制定，正式名称为 EIA-232。它定义了数据终端设备与数据通信设备之间的物理接口和通信协议，广泛应用于早期的计算机、工业设备、仪器仪表等领域。

4.1.1 电气特性

RS-232 采用 非平衡传输，电平与TTL/CMOS逻辑不同：

特点：

• 抗干扰能力较弱。
• 通信距离：通常 ≤ 15 米。
• 通信速率：常见 9600 bps ~ 115200 bps。
• 需电平转换：现代设备通常使用 TTL（0V~5V 或 0V~3.3V），需 MAX232、SP3232 等芯片转换电平。

4.1.2 物理层特性

RS-232 使用 DB9（9针）或 DB25（25针）连接器，常见的是 DB9，其引脚定义如下图。

物理连接：

• 最小连接（3线制）：仅需 TXD、RXD、GND（无流控）。
• 全双工（带流控）：增加 RTS/CTS 或 DTR/DSR 进行硬件流控。

4.1.3 数据帧格式

RS-232 采用 异步串行通信，数据以 帧（Frame） 为单位传输：

帧结构：

4.2 CANopen协议理论基础

4.2.1 CANopen简介

CANopen 是一种建立在 CAN (Controller Area Network) 总线技术之上的、高层通信协议和设备配置文件标准。它诞生于 20 世纪 90 年代初，由 CiA (CAN in Automation) 协会开发和维护，旨在解决原始 CAN 总线（主要定义物理层和数据链路层）缺乏标准化应用层通信机制的问题。

简言之，CAN是物理层和数据链路层，CANOpen是调用CAN进行通行的应用层。

CANopen协议栈采用分层结构，与OSI模型对应关系如下：

CAN总线入门教程can总线教程：https://www.bilibili.com/cheese/play/ss7014

CANOpen入门推荐 周立功教程: https://www.zlg.cn/data/upload/software/Can/CANopen_easy_begin.pdf

4.2.2 CAN总线物理层

CAN总线采用差分信号传输，使用两根线（CAN_H和CAN_L）：

4.2.3 CAN帧结构分析

标准CAN数据帧包含以下字段：

• 帧起始位(SOF)：1位显性位，表示帧的开始

• 仲裁段：

• 控制段：

• 数据段：0-8字节数据

• CRC段：15位CRC校验码 + 1位界定符

• ACK段：1位应答槽 + 1位界定符

• 帧结束(EOF)：7位隐性位

4.3 CANopen通信

4.3.1 通讯对象

1. COB-ID结构

CANopen使用11位标识符(标准CAN帧)，COB-ID由两部分组成：

• 功能码(4位)：定义消息类型
• 节点ID(7位)：标识发送或接收节点

常见功能码：

2. CANopen4种主要的通信对象：

4.3.2 对象字典

对象字典（Object Dictionary, OD）是 CANopen 设备的核心数据结构，用于存储设备的所有配置参数、通信设置和实时数据。

1. 对象字典结构

对象字典采用16位索引和8位子索引的寻址方式：

• 索引(Index)：16位，范围0x0000-0xFFFF
• 子索引(Sub-index)：8位，范围0x00-0xFF

2. 对象字典区域划分

3. 常见对象字典条目示例

4.3.3 NMT协议

NMT协议用于控制网络节点的状态，主要命令包括：

4.4 CANopen设备配置

4.4.1 SDO通信

SDO帧为8字节固定长度，格式如下：

示例1：读取设备类型（索引0x1000）

步骤：主站读取从站（NodeID=1）的设备类型（0x1000:00）。

• 命令字 0x40：读请求（4表示读取，0表示加速传输）

• 命令字 0x43：读响应（4表示读取，3表示数据为4字节）

示例2：写入TPDO1通信参数（索引0x1800）

步骤：主站配置从站（NodeID=2）的TPDO1 COB-ID为0x182。

• 命令字 0x2B：写2字节数据（2表示写入，B表示2字节）

• 命令字 0x60：写成功响应

4.4.2 PDO配置

示例1：TPDO1配置（设备→主站）

功能：周期性发送电机实际速度（0x606C）和状态字（0x6041），触发周期50ms，配置步骤如下。

// 1. 禁用TPDO1（修改前必须先禁用）
{0x23, 0x01, 0x18, 0x01, 0x01, 0x00, 0x00, 0x80}, // COB-ID = 0x180 + NodeID, 禁用
// 2. 设置通信参数（0x1801）
{0x2F, 0x01, 0x18, 0x02, 0xFF, 0x00, 0x00, 0x00}, // 传输类型=0xFF（异步事件触发）
{0x2B, 0x01, 0x18, 0x03, 0x00, 0x00, 0x00, 0x00}, // 禁止时间=0ms
{0x2B, 0x01, 0x18, 0x05, 0x32, 0x00, 0x00, 0x00}, // 事件定时器=50ms（0x32）
// 3. 配置映射参数（0x1A01）
{0x2F, 0x01, 0x1A, 0x00, 0x00, 0x00, 0x00, 0x00}, // 清除映射（子索引0=0）
{0x23, 0x01, 0x1A, 0x01, 0x10, 0x00, 0x41, 0x60}, // 映射状态字（0x6041，16位）
{0x23, 0x01, 0x1A, 0x02, 0x20, 0x00, 0x6C, 0x60}, // 映射实际速度（0x606C，32位）
{0x2F, 0x01, 0x1A, 0x00, 0x02, 0x00, 0x00, 0x00}, // 启用2个映射项（子索引0=2）
// 4. 启用TPDO1
{0x23, 0x01, 0x18, 0x01, 0x01, 0x00, 0x00, 0x00}, // COB-ID = 0x180 + NodeID, 启用

发送的CAN帧示例（假设NodeID=1）：

• COB-ID: 0x181（TPDO1默认ID）
• 数据: [状态字低字节, 状态字高字节, 速度低字节1, 速度低字节2, 速度高字节1, 速度高字节2] （例如：0x00 0x47 0x00 0x00 0x01 0x00 表示状态字=0x4700，速度=65536 rpm）

示例2： RPDO1配置（主站→设备）

功能：接收目标位置（0x607A）和控制字（0x6040），用于运动控制，配置步骤如下：

// 1. 禁用RPDO1
{0x23, 0x01, 0x14, 0x01, 0x01, 0x00, 0x00, 0x80}, // COB-ID = 0x200 + NodeID, 禁用
// 2. 设置通信参数（0x1401）
{0x2F, 0x01, 0x14, 0x02, 0x01, 0x00, 0x00, 0x00}, // 传输类型=0x01（同步周期触发，每1个SYNC）
// 3. 配置映射参数（0x1601）
{0x2F, 0x01, 0x16, 0x00, 0x00, 0x00, 0x00, 0x00}, // 清除映射
{0x23, 0x01, 0x16, 0x01, 0x10, 0x00, 0x40, 0x60}, // 映射控制字（0x6040，16位）
{0x23, 0x01, 0x16, 0x02, 0x20, 0x00, 0x7A, 0x60}, // 映射目标位置（0x607A，32位）
{0x2F, 0x01, 0x16, 0x00, 0x02, 0x00, 0x00, 0x00}, // 启用2个映射项
// 4. 启用RPDO1
{0x23, 0x01, 0x14, 0x01, 0x01, 0x00, 0x00, 0x00}, // COB-ID = 0x200 + NodeID, 启用

接收的CAN帧示例（假设NodeID=1）：

• COB-ID: 0x201（RPDO1默认ID）
• 数据: [控制字低字节, 控制字高字节, 位置低字节1, 位置低字节2, 位置高字节1, 位置高字节2] （例如：0x06 0x00 0x00 0x00 0x01 0x00 表示控制字=0x0006，目标位置=65536 pulses）

4.5 Modbus-RTU

4.5.1 通信特点

Modbus-RTU（Remote Terminal Unit）是一种基于主从架构的串行通信协议，采用二进制编码，具有以下特点：

• 一主机多从机：只有一个主设备（Master），多个从设备（Slave）。
• 主设备发起请求：只有主设备能主动发送请求，从设备被动响应。
• 单工通信：同一时间只能有一个数据在传输（半双工）。
  • 单播模式：主设备发送请求 → 指定从设备响应 → 主设备接收响应。
  • 广播模式：主设备发送请求（地址为0）→ 所有从设备执行操作，但不返回响应。

4.5.2 数据帧结构

Modbus-RTU 的报文帧由应用数据单元（ADU）组成，包括：

RTU字节格式：

• 1起始位 + 8数据位（先传最低有效位） + 1奇偶校验位（可选） + 1停止位（无校验时为2停止位）。

PDU类型：

4.5.3 数据类型

示例1：读取单个寄存器

请求： 读取从站地址为1的设备，起始地址为0x0000（保持寄存器40001）的1个寄存器

[从站地址] [功能码] [起始地址高] [起始地址低] [数量高] [数量低] [CRC低] [CRC高]
0x01      0x03     0x00        0x00     0x00    0x01    0x84    0x0A

响应： 返回的寄存器值为0x1234

[从站地址] [功能码] [字节计数] [数据高] [数据低] [CRC低] [CRC高]
0x01      0x03     0x02     0x12    0x34    0x79    0xCB

示例2：读取多个寄存器

请求： 读取从站地址为2的设备，起始地址为0x0003（保持寄存器40004）的3个寄存器

[从站地址] [功能码] [起始地址高] [起始地址低] [数量高] [数量低] [CRC低] [CRC高]
0x02      0x03     0x00        0x03       0x00   0x03   0x05   0xE8

响应： 返回的3个寄存器值分别为0x5678、0x9ABC、0xDEF0

[从站地址] [功能码] [字节计数] [数据1高] [数据1低] [数据2高] [数据2低] [数据3高] [数据3低] [CRC低] [CRC高]
0x02      0x03     0x06       0x56     0x78     0x9A     0xBC     0xDE     0xF0     0x12    0x34

4.5.4 通信过程

4.6 运动学算法

4.6.1 公式转换

1. 轮胎半径 → 轮胎周长：其中 C 为周长，r 为轮胎半径。 $C=2*pi*r$

2. 电机转速 → 线速度：其中 v为线速度（m/s），ω 为角速度（rad/s），N为电机转速（r/min），r为轮胎半径（m）。 $v=w*r=(\frac {2*pi*N}{60})*r$

3. 线速度 → 电机转速：由线速度 v（m/s）反推电机转速 N（r/min）。 $N=(\frac {60}{2*pi*r})*v$

4. 速度反馈单位转换（r/min → p/s）：其中 f为脉冲频率（pulses/min, p/s），N为转速（r/min），P为每转脉冲数。 假设编码器每转产生 P个脉冲： $f=\frac {N*P}{60}$

5. 角度 → 弧度：其中 θd为角度，θr为弧度。 $\theta_r=(\frac {pi}{180})*\theta_d$

4.6.2 运动学模型

在相邻时刻，机器人底盘的位姿变化可以通过圆弧运动模型进行描述。设θ1表示底盘绕瞬时转动中心转过的圆弧角，θ3代表机器人航向角的变化量。该运动模型中，l为左右驱动轮的轮距，d表示右轮相对于左轮多行驶的弧长距离，r为底盘运动的瞬时转弯半径。左右轮的线速度分别记为vr和vl，ω表示底盘绕转动中心的角速度。

不考虑轮子打滑，左右轮的线速度近似等于其轨迹的圆周运动线速度: $v_l=w*R_l$

$vr=w*R_r$

基于相邻时刻机器人位姿的几何关系分析，可建立航向角变化量θ1的计算模型。如图所示，将两个时刻的机器人位姿进行空间叠加后，通过几何推导可得航向角变化量θ3满足以下关系式： $\theta_3=\theta_2=\theta_1$ 从运动学角度可以证明，机器人底盘航向角的变化量与其绕运动轨迹圆心的旋转角度保持严格一致。这一结论可通过圆周运动实验验证：当机器人沿圆形轨迹运行一周返回起点时，其航向角累计变化360°，同时绕圆心旋转的角度同样为360°，两者完全吻合。在三个关键角度参数中，θ2的计算最为简便。考虑到相邻采样时刻的时间间隔Δt极短，此时航向角变化量θ2趋近于无穷小量，因此可建立如下线性近似关系式： $\theta_2 \approx sin(\theta) = \frac{d}{l} = \frac{(v_r - v_l) * \Delta _t}{l}$ 所以可以得到机器人绕圆心运动的角速度ω,它也是机器人航向角变化的速度: $\omega=\frac{\theta_2}{\Delta _t}=\frac{(v_r-v_l)}{l}$

$v=\frac {(v_r+v_l)}{2}$

其中：l 是机器人底盘的轴距（即两对轮子之间的距离),vl和vr 分别是左轮和右轮的线速度。

5 软件代码教程

5.1 RT_Thread系统初始化

首先在main.c的主函数中调用 rt_hw_interrupt_disable()函数禁用所有中断，确保系统初始化过程不会被中断打断，接着调用rtthread_startup()函数开始RT-Thread的启动流程。

int main(void)
{
    /* 首先关闭中断，确保初始化过程不被中断 */
    rt_hw_interrupt_disable();
    /* 启动RT-Thread RTOS */
    rtthread_startup();
}

RT-Thread实时操作系统的启动流程。

5.2 硬件初始化

5.2.1 UART5初始化

UART5初始化函数，作为航模信号接收，进行机器人底盘的控制。 在usartx.c中对其参数配置为：100k波特率，8位数据位，2位停止位，偶校验（EVEN)，无控流，25个字节。

5.2.2 USART3初始化

为了拓展实现上位机对机器人底盘的控制以及反馈机器人信息，项目通过usart3实现这一功能。同时为了不占用CPU资源，使用了DMA转运的方式。

5.2.3 CAN初始化

本系统采用 CANopen 协议 实现机器人底盘的运动控制，该协议构建于 CAN 总线 物理层之上，通过标准化的通信机制完成对驱动电机的精准控制。在user_can.c中，首先对CAN1控制器进行初始化，包括GPIO、过滤器、CAN寄存器等初始化。

can.h

#define  CAN_STD_ID_H_MASK_DONT_CARE   0x0000
#define  CAN_STD_ID_L_MASK_DONT_CARE   0x0000

为了可以接受任何ID的消息，需要将上面这两个宏定义了CAN过滤器的掩码值设置为0x0000，在ID/掩码过滤模式下，这表示对应位的值在过滤时将被完全忽略。

CAN的波特率通常配置为1M，这可由参数baud决定。

5.2.4 电机初始化

电机初始化需遵循CANopen协议状态机流程： ① 通过NMT命令设置预操作状态 ② 动态配置PDO通信参数（COB-ID、传输模式、映射条目） ③ 发送NMT启动报文（0x01）切换至操作状态

• 节点状态设置

• PDO参数配置

uint8_t Init_sdo[][8]={ //主站(单片机)，恢复从站的初始值 
        // 读参数映射（TPDO0）
        {0x23,0x00,0x18,0x01,v[0],0x01,0x00,0x80},      //失能Tpdo0
        {0x2B,0x00,0x18,0x05,v[2],0x00,0x00,0x00},        //定时器触发时间 20/50ms(刷新率为50/20Hz)    
        {0x2F,0x00,0x1A,0x00,0x00,0x00,0x00,0x00},      //清除映射
        {0x23,0x00,0x1A,0x01,0x10,0x00,0x41,0x60},      //6041  状态字
        {0x23,0x00,0x1A,0x02,0x20,0x00,0x6C,0x60},      //606C  速度反馈
        {0x23,0x00,0x1A,0x03,0x10,0x00,0x77,0x60},      //6077  实际转矩
        {0x23,0x00,0x18,0x01,v[0],0x01,0x00,0x00},         //设置PDO的COB-ID
        {0x2F,0x00,0x18,0x02,0xFF,0x00,0x00,0x00},        //定时器触发
        {0x2F,0x00,0x1A,0x00,0x03,0x00,0x00,0x00},        //映射对象子索引个数
        // 读参数映射（TPDO1）
        {0x23,0x01,0x18,0x01,v[0],0x02,0x00,0x80},      //失能Tpdo1
        {0x2B,0x01,0x18,0x05,v[2],0x00,0x00,0x00},        //定时器触发时间 50ms(刷新率为20Hz)
        {0x2F,0x01,0x1A,0x00,0x00,0x00,0x00,0x00},      //清除映射
        {0x23,0x01,0x1A,0x01,0x10,0x00,0x41,0x60},      //6041  状态字
        {0x23,0x01,0x1A,0x02,0x10,0x00,0x3F,0x60},      //603F  错误代码
        {0x23,0x01,0x1A,0x03,0x20,0x01,0x03,0x10},      //1003  故障信息    
        {0x23,0x01,0x18,0x01,v[0],0x02,0x00,0x00},         //设置PDO的COB-ID
        {0x2F,0x01,0x18,0x02,0xFF,0x00,0x00,0x00},        //定时器触发
        {0x2F,0x01,0x1A,0x00,0x03,0x00,0x00,0x00},        //映射对象子索引个数    
        // 写参数映射（RPDO0）
        {0x23,0x00,0x14,0x01,v[1],0x02,0x00,0x80},      //失能Rpdo0
        {0x2F,0x00,0x14,0x02,0xFE,0x00,0x00,0x00},        //②Hex1400_02，传输形式：0xFE：事件触发；0xFF：定时器触发
        {0x2F,0x00,0x16,0x00,0x00,0x00,0x00,0x00},      //清除映射
        {0x23,0x00,0x16,0x01,0x10,0x00,0x40,0x60},      //6040 控制字
        {0x23,0x00,0x16,0x02,0x20,0x00,v[3],0x60},      //60FF/7A 速度、位置模式
        {0x2F,0x00,0x16,0x00,0x02,0x00,0x00,0x00},       //映射对象子索引个数
        {0x23,0x00,0x14,0x01,v[1],0x02,0x00,0x00},      // 设置PDO的COB-ID            
    };
    Add_Sdo_Linked_List(id, Init_sdo, sizeof(Init_sdo)/sizeof(Init_sdo[0]));//添加到发送链表中

5.3 应用层

5.3.1 核心控制(balance.c)

• 核心控制任务创建

• 各控制模式设置电机速度

• 遥控器控制下的线速度设置，上位机控制同理。

//  输入对应遥控器的通道值
short LinearVelocityGet(void){
    uint16_t VelocityCoefficient = (myabs(pdu[GearPosition_value] - pdu[rc_min_value]) < CHANNEL_VALUE_ERROR) ? pdu[linear_low] : (myabs(pdu[GearPosition_value] - pdu[rc_base_value]) < CHANNEL_VALUE_ERROR) ? pdu[linear_middle] : pdu[linear_high];
    float VelocityTemp = pdu[robot_forward_direction] * (pdu[Forward_value] - pdu[rc_base_value]) / (float)pdu[rc_gears_difference];

    switch(pdu[car_type]){
        case Akm_Car:        // 阿克曼
        case FourWheel_Car:    // 室外差速四驱车
        case Diff_Car:        // 差速车(圆形底盘)
        case TwoWheel_Car:     // 室内差速二驱车
        case Tank_Car:       // 坦克车
        case RC_Car:           // RC车
            return (myabs(pdu[Enable_value] - pdu[rc_max_value]) < CHANNEL_VALUE_ERROR) ?//使能拨杆
                   (short)(VelocityCoefficient * VelocityTemp) : 0;
        default:
            return 0;
    }
}

• 遥控器控制下的角速度设置，上位机控制同理。

//  输入对应遥控器的通道值
short AngularVelocityGet(short linearValue){
    uint16_t YawCoefficient = (pdu[GearPosition_value] == pdu[rc_min_value]) ? pdu[angular_low] : //角速度系数
                              (pdu[GearPosition_value] == pdu[rc_base_value]) ? pdu[angular_middle] : pdu[angular_high];
    float YawTemp = (pdu[robot_forward_direction] * (pdu[Turn_value] - pdu[rc_base_value]) / (float)pdu[rc_gears_difference]); //角速度与转向通道值正方向相反，归一法(-1,1)

    switch(pdu[car_type])
    {
        case Akm_Car:        // 阿克曼
            pdu[target_angle] = round(YawTemp * DEGREE_MAX); //前轮转角
            Servo_pulse = YawTemp * MYHALF * ENCODER_LINES * CORRECTION_FACTOR * pdu[motor1_reduction_ratio];//脉冲
            return (myabs(pdu[Enable_value] - pdu[rc_max_value]) < CHANNEL_VALUE_ERROR) ?
                   (short)(linearValue * YawTemp / AKM_RAW_FACTOR) : 0; // Z轴角速度
        case Diff_Car:        // 差速车(圆形底盘)
        case FourWheel_Car:    // 室外差速四驱车
        case TwoWheel_Car:     // 室内差速二驱车
        case Tank_Car :       // 坦克车
            return (myabs(pdu[Enable_value] - pdu[rc_max_value]) < CHANNEL_VALUE_ERROR) ?
                   (short)(YawCoefficient * YawTemp / PI * 4) : 0; // Z轴角速度
        default:
            return 0;
    }
}

5.3.2 解析数据(remote.c)

• 数据解析任务创建

主要负责接收和处理来自不同源的控制指令，并管理电机的使能状态。

• 航模解析

获取通道值，这里用的是SBUS协议，参考网址：https://os.mbed.com/users/Digixx/notebook/futaba-s-bus-controlled-by-mbed/

• 航模控制权的切换

当上位机通信超时（>5秒）时，系统控制权将自动移交至预设的备用控制模式，确保系统持续稳定运行。

• 接收上位机数据

开启DMA通道接收来自上位机发送的控制数据，并将数据存在Receive_Data[]。只要接收到数据，控制权转给上位机。

• 电机使能

根据状态字判断电机的状态。

5.3.3 电机刷新任务(motor.c)

• 创建电机刷新任务

实时接收电机反馈的信息，包括状态字和控制字。

• 获取电机反馈的信息

解析CAN总线上的TPDO报文，提取电机运行状态信息，如脉冲频率、力矩、错误代码和故障信息等。

5.3.4 CAN任务(user_can.c)

• 创建CAN_PDO任务

用于周期性的发送PDO和SDO数据。

• 发送SDO数据

从预先配置的SDO数据链表中提取待发送数据（该链表已在motor.c文件的WANZER_PDO_Config()函数中初始化），将其封装为CAN报文格式，并通过指定的CAN通信模块发送。主要的作用是映射

• 发送PDO数据(CAN_PDOSend())

负责将电机控制命令实时发送给各个伺服驱动器，这里主要发送的是电机的目标速度，用于控制机器人底盘的运动。

6 运动学算法代码教程

6.1 公式转换

//公式转换
void InitMotorParameters(void)
{
    //轮胎半径->轮胎周长, 单位：m    公式：C=2 * PI * R
    TireCircumference = 2 * PI * pdu[wheel_radius] * MAGNIFIC_10000x_DOWN;
    //电机转速->线速度, 单位：m/s    公式： C / REDUCTION_RATE / 60
    MotorSpeedToLineSpeed = TireCircumference / pdu[motor1_reduction_ratio] / MINTOSEC;
    //线速度->电机转速, 单位：r/min    公式： REDUCTION_RATE * 60 / C
    LineSpeedToMotorSpeed = 1 / MotorSpeedToLineSpeed;
    //速度反馈单位转换0.1r/min->p/s(ZLAC),     公式： 0.1 / 60 * ENCODER_LINES
    MotorSpeedZLACToPulse = MAGNIFIC_10x_DOWN / MINTOSEC * ENCODER_LINES;
    //速度反馈单位转换p/s->0.1r/min(WANZE), 单位：r/min    公式： REDUCTION_RATE * 60 / C
    MotorPulseWANZEToMotorSpeed = 1 / MotorSpeedZLACToPulse;
    //角度最大值->弧度最大值，单位：0.1 degree -> radian
    Z_Radian_Max = pdu[max_angle] * MAGNIFIC_10x_DOWN * ANGtoRAD;
}

6.2 运动学模型代码

线速度换算

 msl = pdu[motor2_rpm_feedback];//电机2转速（左）
 msr = pdu[motor3_rpm_feedback];//电机3转速（右）

linear_speed_feedbacktmp= (short)(((short)msl + ((short)(0xFFFF - msr + 1) & 0xFFFF)) / 2 * MAGNIFIC_10x_DOWN * MotorSpeedToLineSpeed * MAGNIFIC_1000x_UP);

角速度换算

yaw_speed_feedbacktmp= -(short)((float)(pdu[wheel_radius] * (msl + msr) * MAGNIFIC_10x_DOWN * PI /30 *  MAGNIFIC_1000x_UP) / (float)pdu[car_tread] );

7 上位机控制示例

• 上位机下发数据帧格式

示例：