项目简介 :通过遥控器控制机器人向任何方位运动，小陀螺，云台指向，发射子弹，具备多种作战模式，也可将遥控器连接电脑，利用电脑上位机软件控制机器人。
实现过程:
采用sum32+freertos作为控制板，控制机器人整个运动，分为云台任务，底盘任务，发射任务，陀螺仪任务，用户任务校准任务。各个任务之间采用信号量与互斥量进行通讯。
底盘任务CAN通讯控制四个麦克纳姆轮电机，经运动解析实现全方位运动，一般采用速度环pid控制，云台任务主要是CAN通讯控制YAW与PITCH轴电机，采用串级pid实现位置控制，
云台与底盘电机对应多个模式，最主要是相对角(编码器)，绝对角(陀螺仪)。不同电机模式搭配构成了机器人不同的行为模式。
陀螺仪任务采用了DMA+SPI+外部中断方式获取原始数据，也包括温度控制，零漂处理，单位转化等
用户任务是自定义的ui界面，与裁判系统通过uar通讯，将绘制的界面传到上位机。
遥控器操控机器人是通过SBUS的协议传输，利用DMA传输方式节约CPU。
调试总结:在赛场上存在过强的电磁干扰，于是加入了看门狗提高稳定性，在联合调试时遇到一些问题。例如CAN总线的负载，电机抖动，ui无法显示，卡弹等问题
| 任务名称 | FreeRTOS 优先级（数值越大优先级越高） | 执行周期 | 配置理由 |
|---|---|---|---|
| 陀螺仪任务 | 4(MPU9250) | 1ms~2ms | 高频采集传感器数据，保障姿态控制精度，属于硬实时高需求任务 |
| 遥控器接收任务 | 3 | 10ms（匹配 SBUS 帧率） | 是用户指令的核心输入通道，需实时解析 SBUS 协议数据（SBUS 默认 100Hz 更新），避免操控指令延迟 / 丢失 |
| 底盘任务 | 2~3 | 5ms | 全向运动需实时调节电机转速，兼顾控制精度与 CPU 资源负载 |
| 云台任务 | 2 | 5ms | 位置控制需响应指令，但机械响应速度低于底盘，优先级略低避免冲突 |
| 发射任务 | 1 | 10ms | 发射动作节奏较慢，非核心实时需求，优先级低于运动控制类任务 |
| 用户任务 | 0 | 50ms~100ms | UI / 裁判通讯对实时性要求低，高频执行会浪费 CPU 资源 |
| 校准任务 | 0（或仅触发执行） | 非周期性 | 辅助性任务，仅需在低负载时段执行，无需占用实时资源 |
陀螺仪任务
| MPU 引脚 | 连接到 STM32 引脚 |
|---|---|
| SDO（AD0） | SPI1_MISO |
| nCS | GPIO |
| CLK | SPI1_SCK |
| SDI(SDA) | SPI1_MOSI |
使用spi+DMA的方式读取（我的）
SPI
SP是一种四线同步串行接口，它使用两条控制线和两条数据线。在标准的主从式 SPI操作中，MPU-9250 总是作为从设备运行关于主设备而言，串行时钟输出(SCLK)、串行数据输出(SDO)和串行数据输入(SDI)由从设备共同使用每个 SPI从设备都需要从主设备获取自己的芯片选择(CS)线。
在传输开始时，CS端口处于低电平(激活状态)，在传输结束时则恢复到高电平(非激活状态)。每次只有一个CS 线处于激活状态，从而确保在任何给定时间仅选择一个从属设备。未被选中的从属设备的 CS 线保持高电平，导致其 SDO 线处于高阻抗(高阻)状态，从而避免干扰任何处于激活状态的设备。
SPI运行特性
1.数据是以最高有效位(MSB)先传送，最低有效位(LSB)最后传送的方式进行传输的。
2.数据在 SCLK 的上升沿被锁存。
3.数据应在 SCLK 的下降沿进行传输。
4.SCLK 的最大频率为 1MHz。
5.SPI的读写操作需在 16 个或更多的时钟周期内完成(即处理两个或更多的字节)。第一个字节包含SPI地址，而接下来的字节则包含SPI数据。第一个字节的首个位包含读/写位，并指示读取(1)或写入(0)操作。接下来的7位包含寄存器地址。对于多字节的读/写操作，数据为两个或更多的字节
模式3
9250简单解释
MPU-9250具有三个16位的模拟-数字转换器(ADC)，用于将陀螺仪输出进行数字化处理;还有三个16位的ADC，用于将加速度计输出进行数字化处理;以及三个16位的ADC，用于将磁力计输出进行数字化处理。为了实现对快速和缓慢运动的精确跟踪，该部件具有可由用户编程的陀螺仪满量程范围(分别为250、500、士1000和2000秒/度)，可由用户编程的加速度计满量程范围(分别为士28、士4g、土8g和土16g)，以及磁力计满量程范围(4800微特斯拉)。
该设备的所有寄存器进行通信时，均采用400kHz的iiC或1MHz的SPI进行。对于需要更快速通信的应用程序，可以使用 20MHz的 SPI读取传感器和中断寄存器。
4.10 基本的 12C和 SPI串行通信接口
MPU-9250 通过SPL或 12C串行接口与系统处理器进行通信。在与系统处理器通信时，MPU9250总是作为从机。PC从机地址的最低有效位由第9号引脚(ADO)设置。
MPU-9250 设备配备了一条辅助的IPC总线，用于与外部传感器进行通信。该总线有两种工作模式:
主模式:MPU-9250会作为主设备与连接到其上的任何外部传感器进行通信。辅助型集成电路总线
旁路模式:MPU-9250将主IC 总线和辅助IC总线直接连接在一起，使得系统处理器能够直接与任何外部传感器进行通（IIC通信的话，相当于2个从机）
信。
注意:若未使用辅助 fC模式，则应将 AUX DA和 AUX CL保持未连接状态。
辅助 12C 总线运行模式:
主模式:使 MPU-9250能够直接访问外部数字传感器(如磁力计)的数据寄存器。在该模式下，MPU-9250可直接从辅助传感器获取数据，而无需系统应用处理器的干预。
例如，在12C主模式下，MPU-9250 可以被配置为进行批量读取操作，从而从磁力计中返回以下数据:
X磁力计数据(2个字节)
Y磁力计数据(2个字节)
Z方向磁力计数据(2 个字节)
IC主机能够配置为从多达4个辅助传感器中读取最多 24个字节。第五个传感器则可以配置为单字节读写模式。
旁路模式:允许外部系统处理器充当主控制器，并直接与连接到辅助RC总线引脚(AUX_DA和AUX_CL)的外部传感器进行通信。在此模式下，MPU-9250的辅助 PC总线控制逻辑(三方传感器接口模块)将被禁用，而辅助IC引脚AUXDA和AUXCL将连接到主端。C总线通过内部的模拟开关进行传输。
旁路模式适用于配置外部传感器，或者在仅使用外部传感器的情况下使 MPU-9250 保持低功耗状态。在该模式下系统处理器仍可通过rC接口访问 MPU-9250的数据。
通过式模式还可用于直接从主机访问 AK8963振动计。在这种配置下，AK8963的从机地址为OXOC或 12(十进制)
SPI驱动:
https://www.codeleading.com/article/44891883973/
https://www.codeleading.com/article/44891883973/
IIC驱动:
使用MPU9250的MPL库获取欧拉角：
nv_mpu.h的 DEFAULT_MPU_HZ为100 (DMP的最大输出频率为200HZ) 所以输出MPU9250的欧拉角的周期为10ms
问题：
spi异步问题
spi这个样子写不能正常通信（硬件+DMA模式）
cppuint8_t MPU_Read_Byte(uint8_t reg) { uint8_t TXdata[2] = {0}; uint8_t RXdata[2] = {0}; TXdata[0] = reg | 0x80; TXdata[1] = 0xFF; SPi1_CS_low; HAL_SPI_TransmitReceive_DMA(&hspi1, TXdata, RXdata, 2); SPi1_CS_hight; return RXdata[1]; }一、核心致命问题：DMA 异步传输被提前拉高 CS 打断
HAL_SPI_Transmit_DMA(&hspi1, TXdata, 2)是异步函数 ------ 调用后仅 "启动 DMA 传输" 就立刻返回，不会等待数据实际发送完成。此时你马上执行SPi1_CS_hight（拉高片选），相当于：你让快递员（DMA）去送 2 个包裹（寄存器地址 + 数据），刚说完就让快递员关门（拉高 CS），快递员还没出门，包裹根本没送到 MPU6050 手里，写入完全失败。
SPI 协议的硬性要求：片选（CS）必须在整个数据传输周期内保持拉低，直到所有字节发送完成。你的代码中 CS 拉高的时机，早于 DMA 完成传输，MPU6050 会认为 "传输终止"，直接丢弃未接收完的数据。
二、其他关键问题（次要但加剧错误）
- 无 DMA 传输完成判断：没有任何标志 / 等待逻辑确认 DMA 是否真的把 2 个字节发完，即使 DMA 启动失败（比如总线忙），函数也会返回 0（误判成功）。
- 无 DMA 错误处理 ：
HAL_SPI_Transmit_DMA可能返回HAL_ERROR（如 DMA 通道被占用、SPI 状态异常），但你未检查返回值，错误无法被感知。- 冗余变量无意义 ：定义了
RXdata[2]但完全未使用，属于无效代码（虽不影响功能，但增加冗余）。- 返回值逻辑错误：无论 DMA 启动成功 / 失败，都固定返回 0，无法区分 "真成功" 和 "假成功"。
改为：
cppuint8_t MPU_Read_Byte(uint8_t reg) { uint8_t TXdata[2] = {0}; uint8_t RXdata[2] = {0}; //第一个字节包含 SPI地址，接下来的字节包含 SPI数据. //第一个字节的首个位包含读/写位，并指示读取(1)或写入(0)操作 TXdata[0] = reg | 0x80; TXdata[1] = 0xFF; SPi1_CS_low; delay_ms(1); HAL_SPI_TransmitReceive_DMA(&hspi1, TXdata, RXdata, 2); while (HAL_DMA_GetState(&hdma_spi1_rx) != HAL_DMA_STATE_READY); while (HAL_DMA_GetState(&hdma_spi1_tx) != HAL_DMA_STATE_READY); while (__HAL_SPI_GET_FLAG(&hspi1, SPI_FLAG_BSY) != RESET); delay_ms(1); SPi1_CS_hight; return RXdata[1]; }
在 MPU9250 中，"不同模块（主芯片 / 内置磁力计 AK8963）都有 0x00 地址的寄存器"，但 SPI 通信中是通过 **"访问路径 + 模块选择"来区分的 ------ 因为 MPU9250 的主芯片（加速度 / 陀螺）和内置磁力计 AK8963 是两个独立的子模块，SPI 只能直接访问主芯片的寄存器，而访问 AK8963 的寄存器需要通过MPU9250 的内部 I2C 主机中转 。
首先，MPU9250 的内部 I2C 主机模式和旁路模式的区别。当 MPU9250 作为 SPI 从设备，同时要和内部 I2C 主机（连接 AK8963）通信的时候，MPU9250 的内部 I2C 主机是用来控制外部传感器（比如 AK8963）的，这时候如果启用内部 I2C 主机模式，旁路模式应该是关闭的，因为旁路模式是当内部 I2C 主机禁用的时候，让 I2C 引脚作为普通引脚使用。
再理清楚：旁路模式（BYPASS_EN）的作用是，当内部 I2C 主机被禁用时，让 MPU 的 SDA/SDL 引脚作为普通 I2C 引脚（旁路到外部），这样外部 MCU 可以直接通过 I2C 和 AK8963 通信。但用户现在是用 MPU 的内部 I2C 主机来控制 AK8963，所以内部 I2C 主机是启用的（MPU_USER_CTRL_REG 写了 0X20，启用内部 I2C 主机），这时候旁路模式应该关闭，因为内部 I2C 主机需要使用这些引脚来和 AK8963 通信，不需要旁路到外部。
MPU9250内置磁力计，而是通过 I2C 接口连接 AK8963 磁力计芯片，其 I2C 的主机模式和旁路模式均围绕 AK8963 等外部从设备的访问方式设计，两种模式在控制主体、通信流程和适用场景上差异显著，以下是具体详解：
I2C 主机模式 该模式是 MPU9250 的默认 I2C 模式，此时 MPU9250 自身作为 I2C 主机，主动接管对附属的 AK8963 磁力计的通信控制，外部主控 MCU 无法直接与 AK8963 交互，只能和 MPU9250 通信。
- 工作机制：需通过配置 MPU9250 的 USER_CTRL 寄存器（写入 0x20）开启 I2C 主机功能，还可通过 I2C_MST_CTRL 寄存器设置主机通信时钟（如 0x0D 对应 400kHz）。工作时 MPU9250 按照预设时序发起对 AK8963 的读写操作，读取磁力计数据后，会将其暂存到自身内部寄存器中。外部 MCU 只需与 MPU9250 通信，就能一次性获取陀螺仪、加速度计以及磁力计的整合数据。
- 优缺点：优点是能减少外部 MCU 的通信负担，无需 MCU 频繁单独与 AK8963 交互，且数据整合后读取可降低时序冲突风险。缺点是灵活性较低，若需修改 AK8963 的采样率、量程等参数，需先通过 MCU 配置 MPU9250 的从设备控制寄存器，再由 MPU9250 间接转发配置指令，流程较繁琐。
- 适用场景：适合对外部 MCU 资源紧张的场景，比如简单的运动姿态检测设备，无需频繁调整磁力计参数，仅需稳定获取九轴数据。
旁路模式（Bypass Mode） 此模式下 MPU9250 相当于 I2C 信号的 "通路"，会将自身的 I2C 引脚（SDA、SCL）与 AK8963 的对应引脚直接连通，外部 MCU 可绕过 MPU9250，直接与 AK8963 进行 I2C 通信。
- 工作机制：通过配置 MPU9250 的 INT_PIN_CFG 寄存器（如写入 0x02）开启该模式。开启前需先在 USER_CTRL 寄存器中清除 I2C 主控使能位，避免内部主机功能干扰外部通信。模式开启后，MCU 可直接发送 AK8963 的设备地址和寄存器指令，读取磁力计数据或修改其工作参数，此时 MPU9250 仅作为陀螺仪和加速度计的数据来源，与 AK8963 的通信完全由 MCU 主导。
- 优缺点：优点是调试和参数配置灵活，比如调试阶段可单独测试 AK8963 的通信状态，也能直接修改磁力计的采样模式等参数。缺点是增加了 MCU 的工作量，需 MCU 单独管理与 MPU9250、AK8963 的两组 I2C 通信时序，若时序设计不当易出现通信冲突。
- 适用场景：适合需要单独调试磁力计、或需灵活调整 AK8963 工作参数的场景，例如高精度磁场检测设备，需频繁切换磁力计的采样率以适配不同检测场景。
两种模式的核心差异对比
对比维度 I2C 主机模式 旁路模式 控制主体 MPU9250 作为 I2C 主机 外部 MCU 作为 I2C 主机 通信路径 MCU→MPU9250→AK8963 MCU→AK8963（绕过 MPU9250） 开启方式 配置 USER_CTRL 寄存器开启主机功能 配置 INT_PIN_CFG 寄存器，且需关闭内部主机使能 MCU 负担 低，仅需与 MPU9250 通信 高，需单独与 MPU9250 和 AK8963 通信 灵活性 低，间接配置 AK8963 高，直接配置 AK8963 某些版本的MPU9250中，内部I2C主机状态机在上电后可能处于不确定状态。通过以下步骤可以确保状态机正确初始化：
text
步骤1：开启旁路模式 目的：让I2C总线处于已知状态 效果：AUX I2C引脚直接连接到外部，总线被释放 步骤2：启用I2C主机 目的：启动内部I2C控制器 效果：I2C主机开始运行，但总线控制权还未接管 步骤3：关闭旁路模式 目的：让I2C主机接管总线 效果：内部I2C主机现在可以控制AUX I2C引脚
遥控器接收任务
使用和无人机相同的方案
底盘任务
麦克纳姆轮简单介绍：
麦克纳姆轮
https://blog.csdn.net/u014453443/article/details/107228531
https://www.cnblogs.com/FangLai-you/p/10867791.html
https://zhuanlan.zhihu.com/p/629182707https://zhuanlan.zhihu.com/p/629182707
电机使用大疆M2006（电机）+c610电调
| 参数 | 值 | 说明 |
|-----------|--------------|--------------------------------|-----------------------|
| 基本参数 | 型号 | M2006 P36 | 大疆 RoboMaster 系列减速电机 |
| | 电机类型 | 三相永磁直流无刷电机 | 内置永磁体，无电刷结构 |
| | 额定电压 | 24V | 推荐工作电压 |
| | 额定电流 | 3A | 持续工作电流上限 |
| | 额定功率 | 200W | 最大功率输出 |
| | 重量 | 90g | 不含电调 |
| | 尺寸 (外径 × 总长) | 24.4mm × 64.8mm | 紧凑设计 |
| | 减速比 | 36:1 | 行星齿轮减速箱 |
| | 适用温度 | 0~55℃ | 工作温度范围 |
| 性能参数 | 空载转速 | 500rpm | 无负载最大转速 |
| | 持续最大扭矩 | 1N·m | 可持续输出的最大扭矩 |
| | 1N・m 负载转速 | 416rpm | 输出 1N・m 时的最大转速 |
| | 最大输出功率 | 44W | 实际最大输出功率 |
| | 空载电流 | 0.6A | 无负载工作电流 |
| 编码器参数 | 类型 | 增量式霍尔编码器 | 提供精确位置反馈 |
| | 分辨率 | 8192PPR | 每转输出 8192 个脉冲 |
| | 角度精度 | 0.001° | 结合减速比后的角度精度 |
| 电气参数 | 转矩常数 | 0.18N·m/A | 每安培电流产生的扭矩 |
| | 速度常数 | 32.96rpm/V | 每伏特电压产生的转速 |
| | 相电阻 | 461mΩ | 绕组电阻 |
| | 相电感 | 64.22μH | 绕组电感 |
| | 极对数 | 7 | 内部磁场极对数 |
| 控制特性 | 通信方式 | CAN 总线 | 兼容 DJI C610/C620 电调协议 |
| | 标准 ID 范围 | 0x200~0x20F | 控制指令 ID 范围 |
| | 控制模式 | 三闭环系统 | 位置环、速度环、电流环 |
| | 默认模式 | 速度 + 电流双闭环 | 系统默认配置 |
| | 指令范围 | -10000~10000 | 对应 - 10A~+10A 电流 |
| 应用场景 | 主要应用 | 机器人关节、云台系统、自动化设备、RoboMaster 竞赛 | 官方推荐应用领域 |
| 配套电调 | 型号 | C610 | 官方推荐配套电调 |
| | 控制技术 | 32 位 FOC | 磁场定向控制技术 |
| | 最大电流 | 10A | 满足高负载需求 |
| | 连接方式 | 4 针接口 | 连接编码器实现精确反馈 |
| | 调参方式 | RoboMaster Assistant 软件 | 用于参数调整和固 |
8192PPR ：编码器的 "每转脉冲数"PPR 是
Pulses Per Revolution的缩写，指编码器所在的轴转 1 圈时，编码器输出的脉冲信号数量 。M2006 的编码器装在电机本体的轴上，所以电机本体轴转 1 圈，编码器会输出 8192 个脉冲（这是编码器的 "原始分辨率"）。36:1 ：减速箱的 "传动比"这是电机内置行星减速箱的传动比例，意思是：电机本体的轴转 36 圈 ，经过减速箱后，电机的输出轴（连接负载的轴）才转 1 圈。
输出轴角度精度 = 360° ÷（编码器 PPR × 减速比）= 360° ÷（8192×36）≈ 0.00122°
官方手册中提到在同一个CAN总线上，最多可以接入8个电调，通过电调的自动分配他们分别是地址0x201~0x208。
| 数据项 | 数据类型 | 位数 | 最大值（返回原始值） | 最小值（返回原始值） | DATA 组合表达式（单行） | 原始值物理意义 |
|---|---|---|---|---|---|---|
| 转子机械角度 | 13 位无符号整数 | 13 | 8191 | 0 | `DATA[0] << 8|DATA[1] | 0~8191 对应电机转子旋转一圈的机械角度 0°~360°，单个编码值对应约 0.0439°（360°/8192） |
| 转子转速 | 16 位有符号整数 | 16 | 32767 | -32768 | `DATA[2] << 8|data[3] | -32768~32767 为转速数字量编码，单位为 rpm（转 / 分钟），正数表示电机正转，负数表示电机反转 |
电机数据, 0:底盘电机1 2006电机, 1:底盘电机2 2006电机,2:底盘电机3 2006电机,3:底盘电机4 2006电机;
4:yaw云台电机 6020电机; 5:pitch云台电机 6020电机; 6:拨弹电机 2006电机*/
下面是速度环的pid
cpp
typedef struct
{
uint16_t ecd;
int16_t speed_rpm;
int16_t given_current;
uint8_t temperate;
int16_t last_ecd;
} motor_measure_t;
static motor_measure_t motor_chassis[7];
void MX_CAN1_Init(void)
{
hcan1.Instance = CAN1;
hcan1.Init.Prescaler = 3;
hcan1.Init.Mode = CAN_MODE_NORMAL;
hcan1.Init.SyncJumpWidth = CAN_SJW_1TQ;
hcan1.Init.TimeSeg1 = CAN_BS1_10TQ;
hcan1.Init.TimeSeg2 = CAN_BS2_3TQ;
hcan1.Init.TimeTriggeredMode = DISABLE;
hcan1.Init.AutoBusOff = DISABLE;
hcan1.Init.AutoWakeUp = DISABLE;
hcan1.Init.AutoRetransmission = DISABLE;
hcan1.Init.ReceiveFifoLocked = DISABLE;
hcan1.Init.TransmitFifoPriority = DISABLE;
if (HAL_CAN_Init(&hcan1) != HAL_OK)
{
Error_Handler();
}
....省略32为屏蔽模式的配置
}
void pid_abs_limit(float *a,float abs_max)
{
if(*a>abs_max)
*a=abs_max;
if(*a<-abs_max)
*a=-abs_max;
}
float Pid_Caculate( float Mu,float Cu )
{
err= Cu - Mu; // 误差等于：目标值-测量值
total_err = err;
pout = Kp * err; // 比例输出
iout += Ki * err; // 积分输出
pid_abs_limit(&iout,5000); // 积分输出限幅
out = pout + iout; // PID计算总输出
out = out * 0.85f + last_out * 0.15f; // PID输出一阶低通滤波
pid_abs_limit(&out,5000); // PID输出限幅
err_last=err;
return out; // 输出PID计算值
}
void CAN( void )
{
motor11 = Pid_Caculate( motor_chassis[0].speed_rpm, 300 ); //右轮2006电机
motor21 = Pid_Caculate( motor_chassis[1].speed_rpm, -300 ); //左轮2006电机
motor31 = Pid_Caculate( motor_chassis[2].speed_rpm, -300 ); //右轮2006电机
motor41 = Pid_Caculate( motor_chassis[3].speed_rpm, 300 ); //左轮2006电机
CAN_cmd_chassis( motor11,motor21,motor31,motor41 );
}
/**
* @brief 发送电机控制电流(0x201,0x202,0x203,0x204)
* @param[in] motor1: (1) 3508电机控制电流, 范围 [-10000,10000]
* @param[in] motor2: (2) 3508电机控制电流, 范围 [-10000,10000]
* @param[in] motor3: (3) 3508电机控制电流, 范围 [-10000,10000]
* @param[in] motor4: (4) 3508电机控制电流, 范围 [-10000,10000]
* @retval none
*/
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
uint32_t send_mail_box;
chassis_tx_message.StdId = CAN_CHASSIS_ALL_ID;
chassis_tx_message.IDE = CAN_ID_STD;
chassis_tx_message.RTR = CAN_RTR_DATA;
chassis_tx_message.DLC = 0x08;
chassis_can_send_data[0] = motor1 >> 8;
chassis_can_send_data[1] = motor1;
chassis_can_send_data[2] = motor2 >> 8;
chassis_can_send_data[3] = motor2;
chassis_can_send_data[4] = motor3 >> 8;
chassis_can_send_data[5] = motor3;
chassis_can_send_data[6] = motor4 >> 8;
chassis_can_send_data[7] = motor4;
HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}
#define get_motor_measure(ptr, data) \
{ \
(ptr)->last_ecd = (ptr)->ecd; \
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
(ptr)->temperate = (data)[6]; \
}
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
CAN_RxHeaderTypeDef rx_header;
uint8_t rx_data[8];
HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);
switch (rx_header.StdId)
{
case CAN_3508_M1_ID:
case CAN_3508_M2_ID:
case CAN_3508_M3_ID:
case CAN_3508_M4_ID:
case CAN_YAW_MOTOR_ID:
case CAN_PIT_MOTOR_ID:
case CAN_TRIGGER_MOTOR_ID:
{
static uint8_t i = 0;
//get motor id
i = rx_header.StdId - CAN_3508_M1_ID;
get_motor_measure(&motor_chassis[i], rx_data);
break;
}
default:
{
break;
}
}
}
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_CAN1_Init();
MX_CAN2_Init();
/* USER CODE BEGIN 2 */
can_filter_init();
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
CAN( );
HAL_Delay(2);
}
/* USER CODE END 3 */
}
云台任务
4:yaw云台电机 6020电机; 5:pitch云台电机 6020电机; 6:拨弹电机 2006电机*/
下面是速度环的pid
| 参数类别 | 参数名称 | 数值 | 备注 | |
|---|---|---|---|---|
| 基本电气参数 | 额定电压 | DC 24V | 推荐工作电压范围 | |
| 额定电流 | 1.62A | 最大持续电流 | ||
| 额定功率 | 约 200W | 24V×1.62A | ||
| 最大空载转速 | 320rpm | 无负载时最高转速 | ||
| 额定扭矩 | 1.2N·m | 最大持续输出扭矩 | ||
| 额定扭矩下转速 | 132rpm | 输出 1.2N・m 时的最大转速 | ||
| 空载电流 | 78mA | 无负载时的电流消耗 | ||
| 相电阻 | 1.8Ω | 电机绕组电阻 | ||
| 相电感 | 5.78mH | 电机绕组电感 | ||
| 转矩常数 | 741mN·m/A | 每安培电流产生的扭矩 | ||
| 转速常数 | 13.33rpm/V | 每伏特电压产生的转速 | ||
| 最大效率 | 67.85% | 能量转换最高效率点 | ||
| 机械与结构参数 | 电机重量 | 468g | 不含线缆 | |
| 电机外径 | 66.7mm | 定子最大直径 | ||
| 总高度 | 45mm | 电机整体高度 | ||
| 空心轴内径 | 18mm | 贯穿轴内直径 | ||
| 空心轴外径 | 22mm | 贯穿轴外直径 | ||
| 安装孔 (转子) | M3 (深 4mm) | 固定用螺纹孔 | ||
| 安装孔 (定子) | M4 (深 6mm) | 固定用螺纹孔 | ||
| 轴承动载荷 | 3.5kN | 最大径向动态负载 | ||
| 轴承静载荷 | 2.2kN | 最大径向静态负载 | ||
| 极对数 | 10 | 电机内部磁极对数 | ||
| 相数 | 3 | 三相交流电机 | ||
| 工作环境参数 | 工作温度范围 | 0℃~55℃ | 环境温度 | |
| 绕组最高温度 | 125℃ | 超过此温度电机保护机制启动 | ||
| 存储温度 | -20℃~60℃ | 非工作状态存放温度 | ||
| 湿度范围 | 10%~90% | 无凝结 | ||
| 控制信号参数 | CAN 控制模式 | 通信协议 | CAN 总线 (1Mbps) | |
| ID 设置范围 | 0~7 | 通过拨码开关设置 | ||
| 控制指令范围 | 电压模式:-30000~30000电流模式:-16384~16384 | 对应 - 25V~25V对应 - 3A~3A | ||
| 状态反馈频率 | 1kHz | 每秒发送 1000 次数据 | ||
| 反馈内容 | 位置 (0~8191)转速 (rpm)电流 (mA)温度 (℃) | 14 位位置分辨率 | ||
| PWM 控制模式 | 信号频率 | 50Hz | 标准 PWM 频率 | |
| 脉宽范围 | 1000~2000μs | 有效控制信号宽度 | ||
| 默认模式 | 位置控制 | 可通过软件切换为速度控制 | ||
| 速度模式映射 | 1000μs:0rpm1500μs:1500rpm2000μs:1920rpm | 线性映射关系 | ||
| 电机特性参数 | 转矩脉动系数 | 3.32% | 输出扭矩波动幅度 | |
| 定位精度 | 0.05° | 位置控制精度 | ||
| 机械时间常数 | 3ms | 响应速度指标 | ||
| 调速范围 (空载) | 0~320rpm | 速度控制范围 | ||
| 调速范围 (额定转矩) | 0~132rpm | 速度控制范围 | ||
| 堵转扭矩 | 0.86N·m | 电机锁定时最大扭矩 | ||
| 堵转电流 | 0.90A | 电机锁定时电流 | ||
| 指示灯状态 | 绿灯每秒闪 N 次 | 正常工作，ID=N | ||
| 绿灯慢闪 | PWM 通信正常 | |||
| 绿灯常亮 | PWM 信号校准中 | |||
| 橙灯每秒闪 1 次 | 电机高温警告 (>100℃) | |||
| 橙灯每秒闪 2 次 | CAN 总线 ID 冲突 | |||
| 红灯每秒闪 4 次 | 电机温度过高 (>125℃) | |||
| 应用场景 | 主要应用 | RoboMaster 机器人竞赛工业自动化科研教育直接驱动应用 | 适合高扭矩、低转速场景 | |
| 连接方式 | 电源接口 | XT30 | 红 (+)、黑 (-) | |
| CAN 通信接口 | 专用端口 | A (红):CAN_H, B (黑):CAN_L | ||
| PWM 控制接口 | 专用端口 | 白 (PWM/RX), 灰 (TX), 黑 (GND) |
| 数据项 | 数据类型 | 位数 | 最大值（返回原始值） | 最小值（返回原始值） | DATA 组合表达式（单行） | 原始值物理意义 |
|---|---|---|---|---|---|---|
| 转子机械角度 | 13 位无符号整数 | 13 | 8191 | 0 | `DATA[0] << 8|DATA[1] | 0~8191 对应电机转子旋转一圈的机械角度 0°~360°，单个编码值对应约 0.0439°（360°/8192） |
| 转子转速 | 16 位有符号整数 | 16 | 32767 | -32768 | `DATA[2] << 8|data[3] | -32768~32767 为转速数字量编码，单位为 rpm（转 / 分钟），正数表示电机正转，负数表示电机反转 |
| 实际输出电流 | 16 位有符号整数 | 16 | 32767 | -32768 | DATA[4] << 8|datap[5] | -32768~32767 为转矩 / 电流数字量编码，数值大小对应转矩（或相电流）的强弱，正负对应转矩输出方向（通常与转速方向一致 / 相反） |
GM6020有三个环，电流环、位置环、速度环。
https://blog.csdn.net/weixin_73037889/article/details/130696750
位置环
编码器的 "每转脉冲数"PPR 是Pulses Per Revolution的缩写，指编码器所在的轴转 1 圈时，编码器输出的脉冲信号数量。的编码器装在电机本体的轴上，所以电机本体轴转 1 圈，编码器会输出 8192 个脉冲（这是编码器的 "原始分辨率"）。
位置环返回去值范围为：DATA[0] << 8|DATA[1] 8191-0
速度环DATA[2] << 8|data[3]
返回去的`DATA[2] << 8|data[3] 范围在下面的设置，下面的为 -300-300
电流环：-30000-----30000
发射任务
遥控器摇杆 → 拨弹电机电机位置环转→完毕恢复原来的状态
cpp
void launchTaskFuc(void const * argument)
{
/* USER CODE BEGIN launchTaskFuc */
TickType_t xLastWakeTime = xTaskGetTickCount();
BaseType_t err = pdFALSE;
JoyStick_Struct local_joyStick; // 本地缓存遥控器数据
static int16_t pid_output[4] = {0}; // PID输出（电机控制指令）
static int16_t pidSpeed_output[4] = {0}; // 速度环PID输出
/* 无限循环 */
for(;;)
{
err = xQueueReceive(communication, &local_joyStick, portMAX_DELAY);
if(local_joyStick.KEY_Letf1){
// PID计算（底盘电机控制）
pid_output[0] = PID_Calc(&pid_w7, Motor6020[0].angle, 3000); // 1号电机位置环PID
pidSpeed_output[0] = PID_Calc(&pid_w7, Motor6020[0].angle,pid_output[0] ); // 1号电机速度PID
}
else{
pid_output[0] = PID_Calc(&pid_w7, Motor6020[0].angle, 0); // 1号电机位置环PID
pidSpeed_output[0] = PID_Calc(&pid_w7, Motor6020[0].angle,pid_output[0] ); // 1号电机速度PID
}
set_motor_voltage(1, 0, 0, pidSpeed_output[0], 0);
vTaskDelayUntil(&xLastWakeTime, 10);
}
/* USER CODE END launchTaskFuc */
}电机
GM6020电机单环位置环PID
位置环返回去值范围为：DATA[0] << 8|DATA[1] 8191-0
cpp
typedef struct
{
float kp; // 比例系数
float ki; // 积分系数
float kd; // 微分系数
float integral; // 积分项
float err[2];
float min_out; // 输出最小值
float max_out; // 输出最大值
float p_out;
float i_out;
float d_out;
float last_error;
float out;
float taeget_value;
float feedback_value; //测量值
} PID_HandleTypeDef;
typedef struct
{
int16_t speed;
uint16_t angle;
uint8_t temperature;
int16_t current;
int16_t really_angle;
} Motor6020rx;
Motor6020rx Motor6020[4];
PID_HandleTypeDef pid_w1 = {.kp=30.0f, .ki=0.1f, .kd=2.0f, .min_out=-8192, .max_out=8192};
void DomainTASKFuc(void const * argument)
{
static int16_t pid_output[4] = {0}; // PID输出（电机电流指令）
uint16_t num=1024;
uint8_t key=0;
for(;;)
{
key=key_scan(0);
if(key==KEY1_PRES){
num+=1024.0f;
if(num>=8192) num=1024;
}
pid_output[0] = PID_Calc(&pid_w1, Motor6020[0].angle, num);
set_motor_voltage(0,
pid_output[0],
pid_output[1],
pid_output[2],
pid_output[3]); // 设置0号CAN通道电机电压
vTaskDelayUntil(&xLastWakeTime, 5); // 任务周期5ms
}
}
can其他的省略，只贴下面的
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
CAN_RxHeaderTypeDef rx_header;
uint8_t rx_data[8];
if(hcan->Instance == CAN1)
{
HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data); //receive can data
}
if ((rx_header.StdId >= FEEDBACK_ID_BASE)
&& (rx_header.StdId < FEEDBACK_ID_BASE + MOTOR_MAX_NUM)) // judge the can id
{
uint8_t index = rx_header.StdId - FEEDBACK_ID_BASE;
Motor6020[index].angle = ((rx_data[0] << 8) | rx_data[1]);
Motor6020[index].speed = ((rx_data[2] << 8) | rx_data[3]);
Motor6020[index].current = ((rx_data[4] << 8) | rx_data[5]);
Motor6020[index].temperature = rx_data[6];
Motor6020[index].really_angle= Motor6020[index].angle/8192.0f*360.0f;
}
}
float PID_Calc(PID_HandleTypeDef *pid, float feedback, float reference)
{
float error = reference - feedback;
float p_out = pid->kp * error;
pid->integral += pid->ki * error;
pid->integral = constrain(pid->integral, pid->min_out, pid->max_out);
float i_out = pid->integral;
float d_out = pid->kd * (error - pid->last_error);
pid->last_error = error;
float output = p_out + i_out + d_out;
output = constrain(output, pid->min_out, pid->max_out);
return output;
}PID的P调节太小的话会出现各种奇怪的问题
大疆GM6002电机单环位置环PID
GM6020电机单环速度环PID
返回去的`DATA[2] << 8|data[3] 范围在上位机上面设置，我们设置为最为 -300-300
cpp
typedef struct
{
float kp; // 比例系数
float ki; // 积分系数
float kd; // 微分系数
float integral; // 积分项
float err[2];
float min_out; // 输出最小值
float max_out; // 输出最大值
float p_out;
float i_out;
float d_out;
float last_error;
float out;
float taeget_value;
float feedback_value; //测量值
} PID_HandleTypeDef;
typedef struct
{
int16_t speed;
uint16_t angle;
uint8_t temperature;
int16_t current;
int16_t really_angle;
} Motor6020rx;
Motor6020rx Motor6020[4];
PID_HandleTypeDef pid_w1 = {.kp=24.0f, .ki=10.0f, .kd=19.0f,.min_out=-30000, .max_out=30000};
void DomainTASKFuc(void const * argument)
{
static int16_t pid_output[4] = {0}; // PID输出（电机电流指令）
uint16_t num=0;
uint8_t key=0;
for(;;)
{
key=key_scan(0);
if(key==KEY1_PRES){
num+=100.0f;
if(num>300) num=0;
}
pid_output[0] = PID_Calc(&pid_w1, Motor6020[0].speed, num); // 电机1位置环PID（目
set_motor_voltage(0,
pid_output[0],
pid_output[1],
pid_output[2],
pid_output[3]); // 设置0号CAN通道电机电压
vTaskDelayUntil(&xLastWakeTime, 5); // 任务周期5ms
}
}
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
CAN_RxHeaderTypeDef rx_header;
uint8_t rx_data[8];
if(hcan->Instance == CAN1)
{
HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data); //receive can data
}
//标识符:0x204+驱动器 ID
if ((rx_header.StdId >= FEEDBACK_ID_BASE)
&& (rx_header.StdId < FEEDBACK_ID_BASE + MOTOR_MAX_NUM)) // judge the can id
{
// can_cnt ++;
uint8_t index = rx_header.StdId - FEEDBACK_ID_BASE; // get motor index by can_id
Motor6020[index].angle = ((rx_data[0] << 8) | rx_data[1]);
Motor6020[index].speed = ((rx_data[2] << 8) | rx_data[3]);
Motor6020[index].current = ((rx_data[4] << 8) | rx_data[5]);
Motor6020[index].temperature = rx_data[6];
}
}
float PID_Calc(PID_HandleTypeDef *pid, float feedback, float reference)
{
float error = reference - feedback;
float p_out = pid->kp * error;
pid->integral += pid->ki * error;
pid->integral = constrain(pid->integral, pid->min_out, pid->max_out);
float i_out = pid->integral;
float d_out = pid->kd * (error - pid->last_error);
pid->last_error = error;
float output = p_out + i_out + d_out;
output = constrain(output, pid->min_out, pid->max_out);
return output;
}
float constrain(float amt, float low, float high) {
return (amt < low) ? low : (amt > high) ? high : amt;
}大疆GM6002电机单环速度环PID
GM6020电机串级PID
目标位置（num） → 位置环PID → 期望速度 → 速度环PID → 电流指令 → GM6020电机
cpp
typedef struct
{
float kp; // 比例系数
float ki; // 积分系数
float kd; // 微分系数
float integral; // 积分项
float err[2];
float min_out; // 输出最小值
float max_out; // 输出最大值
float p_out;
float i_out;
float d_out;
float last_error;
float out;
float taeget_value;
float feedback_value; //测量值
} PID_HandleTypeDef;
typedef struct
{
int16_t speed;
uint16_t angle;
uint8_t temperature;
int16_t current;
int16_t really_angle;
} Motor6020rx;
Motor6020rx Motor6020[4];
PID_HandleTypeDef pid_Speed1= {.kp=24, .ki=10, .kd=19, .integral=0,.last_error=0,.min_out=-30000, .max_out=30000};
PID_HandleTypeDef pid_w1 = {.kp=0.17, .ki=0.0f, .kd=0.15, .integral=0,.last_error=0,.min_out=-8192, .max_out=8192};
void DomainTASKFuc(void const * argument)
{
static int16_t pid_output[4] = {0}; // PID输出（电机电流指令）
static int16_t pidSpeed_output[4] = {0};
uint16_t num=1024;
uint8_t key=0;
for(;;)
{
key=key_scan(0);
if(key==KEY1_PRES){
num+=1024;
if(num>8192) num=1024;
}
pid_output[0] = PID_Calc(&pid_w1, Motor6020[0].angle, num); //位置环--外环
pidSpeed_output[0]=PID_Calc(&pid_Speed1,Motor6020[0].speed,pid_output[0]); //速度环--内环
set_motor_voltage(0,
pidSpeed_output[0],
pid_output[1],
pid_output[2],
pid_output[3]); // 设置0号CAN通道电机电压
vTaskDelayUntil(&xLastWakeTime, 5); // 任务周期5ms
}
}
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
CAN_RxHeaderTypeDef rx_header;
uint8_t rx_data[8];
if(hcan->Instance == CAN1)
{
HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data); //receive can data
}
//标识符:0x204+驱动器 ID
if ((rx_header.StdId >= FEEDBACK_ID_BASE)
&& (rx_header.StdId < FEEDBACK_ID_BASE + MOTOR_MAX_NUM)) // judge the can id
{
// can_cnt ++;
uint8_t index = rx_header.StdId - FEEDBACK_ID_BASE; // get motor index by can_id
Motor6020[index].angle = ((rx_data[0] << 8) | rx_data[1]);
Motor6020[index].speed = ((rx_data[2] << 8) | rx_data[3]);
Motor6020[index].current = ((rx_data[4] << 8) | rx_data[5]);
Motor6020[index].temperature = rx_data[6];
}
}
float PID_Calc(PID_HandleTypeDef *pid, float feedback, float reference)
{
float error = reference - feedback;
float p_out = pid->kp * error;
pid->integral += pid->ki * error;
pid->integral = constrain(pid->integral, pid->min_out, pid->max_out);
float i_out = pid->integral;
float d_out = pid->kd * (error - pid->last_error);
pid->last_error = error;
float output = p_out + i_out + d_out;
output = constrain(output, pid->min_out, pid->max_out);
return output;
}
float constrain(float amt, float low, float high) {
return (amt < low) ? low : (amt > high) ? high : amt;
}PID给的异常的话，会导致电机的结果出现奇怪的问题
GM6002电机串级PID内环（速度环）外环（位置环）