基于stm32f103的HAL库控制RM3508角度环详细教程
·
目录
4.更新CAN接收中断回调函数，里面的角度环相关算法在下面讲解
5.主循环调用，540*19意思是转动角度*电机减速比，实现输出轴转动540度。
即用4096做门槛，8192做修正，float做累加，total_angle永不跳变
一、前言
我的前两篇文章已经实现了STM32F103C8T6对RM3508电机的速度环控制，接下来我来详细讲解一下3508的角度环控制部分。
控制3508角度环的大体流程：
硬件连接-->cubemx配置-->CAN发送-->CAN接收-->速度环-->角度读取-->角度P控制-->完整的PID控制-->调参
没有看过前面两篇文章的，我把文章链接放在下面供大家参考：
stm32f103控制RM3508角度环（一）-CSDN博客
stm32f103控制RM3508角度环（二）-CSDN博客
二、角度环讲解
1.角度环的概念
角度环是电机控制系统中的一个关键环节，主要负责控制电机的旋转角度，使其精确地达到目标位置。它通常作为外环存在，与速度环（内环）协同工作，构成一个典型的双闭环控制系统。
2.角度环的核心任务
角度环的核心任务是计算“应该以多快的速度靠近目标位置”。具体来说：
目标量：目标角度（例如，180度）
反馈量：电机实际旋转角度（通过编码器反馈）
输出量： 目标转速（这个转速作为速度环的输入）
角度环通过PID算法，根据目标角度与实际角度的误差，计算出一个合适的速度，使得电机逐步逼近目标位置。
举个荔枝：你正在开车去学校，突然看见路边的里程碑（反馈），发现在450公里处，你的目标是500公里（误差：500-450=50公里），你的大脑（控制器）会决定：“还有50KM，我需要保持一个较高的速度（比如100Km/h）赶过去”。这个“100Km/h”就是角度环输出给速度环的目标速度。
即离目标越远，它输出的目标速度越大；离目标速度越近，它输出的目标速度越小
3.角度环的工作流程
角度环的工作流程可以分为以下几个步骤：
1.误差计算：
计算目标角度与实际角度的差值（即误差）
2.PID调节:
根据误差，利用比例（P）、积分（I）、微分（D）三个参数，计算出一个转速指令
3.速度环衔接：
将计算出的转速指令作为速度环的输入，由于速度环进一步控制电机的电流，实现精准的速度控制
4.角度环的PID参数调节
角度环的PID参数调节角度环的PID参数调节至关重要，直接影响控制效果：
比例（P）：比例项决定了控制的“力度”。比例系数过大，可能导致系统响应过于激进，出现震荡；比例系数过小，响应则会过于迟缓。
积分（I）：积分项用于消除系统的稳态误差，确保电机最终精确停在目标位置。
微分（D）：微分项用于抑制系统的快速变化和震荡，提高系统的稳定性。
5. 角度环与速度环的关系
角度环和速度环是典型的双闭环控制结构：
角度环（外环）：负责计算目标转速，确保电机逐步逼近目标位置。
速度环（内环）：根据角度环的转速指令，精确控制电机的电流，实现速度控制。
6.角度环总结
角度环是电机控制系统中用于精确控制旋转角度的关键环节。它通过PID控制算法，根据目标角度与实际角度的误差，计算出一个合适的速度指令，交给速度环执行。角度环的PID参数调节和程序实现需要特别注意，以确保系统的稳定性和精确性。角度环和速度环的协同工作，构成了一个完整的电机控制系统。
一般的串级PID的调参主要是调角度环，如果需要电机的爆发力很大，角度环的输出限制不能设置的很大（快准狠）。
若是电机在目标值附近来回震荡很大，则需要在角度环里加大d的值。
角度环的程序需要放在CAN的接收中断里。
三、代码编写
以下代码均在前两篇文章的代码基础上添加，没有看过前两篇文章的建议去补下课
1.定义电机反馈结构体
// 定义电机反馈结构体（存储单个电机的反馈数据）
typedef struct {
int16_t angle; // 角度（原始编码器值，0-8191）
int16_t rpm; // 转速（rpm）
int16_t current; // 实际转矩电流
uint8_t temperature; // 温度（℃）
// 【新增】以下两个变量用于计算连续总角度
int16_t last_angle; // 上一次的原始编码器值，用于判断溢出
float total_angle; // 连续的总角度值（单位：编码器值） = 圈数 * 8192 + angle
} MotorFeedbackTypeDef;
// 为电机1定义一个角度环PID控制器
PID_TypeDef angle_pid_m1; // 使用和速度环相同的PID结构体2.添加若干转换函数
/* USER CODE BEGIN PFP */
// 角度值到编码器值的转换函数
float degree_to_encoder(float degree) {
// RM3508电机一圈是8192个编码器值，对应360度
// 所以：编码器值 = (角度 / 360) * 8192
return (degree / 360.0f) * 8192.0f;
}
// 编码器值到角度值的转换函数（用于调试显示）
float encoder_to_degree(float encoder) {
return (encoder / 8192.0f) * 360.0f;
}
/* USER CODE END PFP */3.添加角度环PID参数
//初始化角度环PID参数 ---================================================================================
angle_pid_m1.Target = 0.0f; // 初始目标角度设为0
angle_pid_m1.Kp = 0.8f; // 【关键】需要调试：这个参数决定“多快”地冲向目标角度。建议从0.5开始试。
angle_pid_m1.Ki = 0.0f; // 角度环通常不需要积分项(I)，先设为0
angle_pid_m1.Kd = 0.0f; // 先设为0
angle_pid_m1.IntegralLimit = 1000.0f;
angle_pid_m1.OutputLimit = 1000.0f; // 【关键】输出限幅：这个值限制了角度环能命令速度环达到的最大速度。
// 例如设为5000，意味着角度环最多命令电机以5000RPM的速度去追角度。
// 这个值越大，电机转得越快，但也容易过冲和振荡。请从2000开始慢慢加大。
angle_pid_m1.Integral = 0.0f;
angle_pid_m1.LastError = 0.0f;
angle_pid_m1.Output = 0.0f;
/*
【参数调试】耐心调试PID：
第一步：将 Kp, Ki, Kd 全部设为0。电机应不转。
第二步：只调P。慢慢增大 Kp（比如从1.0 -> 2.0 -> 5.0 -> 10.0...），观察电机是否开始转动并加速。目标是让电机能稳定在一个接近目标速度（1000RPM）的值，即使有静差（比如只能稳定在800RPM）也没关系。
第三步：加入I。慢慢增大 Ki（比如从0.01 -> 0.05 -> 0.1...），观察电机速度是否逐渐接近并最终稳定在目标速度1000RPM上，消除静差。
第四步：（可选）如果电机在目标速度附近震荡，加入一点点 Kd（比如0.1）来抑制震荡。4.更新CAN接收中断回调函数，里面的角度环相关算法在下面讲解
/* USER CODE BEGIN 4 */
//CAN 接收中断回调函数
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData) == HAL_OK) {
if (RxHeader.StdId >= 0x201 && RxHeader.StdId <= 0x204) {
uint8_t motor_id = RxHeader.StdId - 0x201;
// 1. 保存旧的原始角度值（从结构体中获取）
int16_t old_angle = motor_feedback[motor_id].last_angle;
// 2. 解析数据（大疆标准协议）
motor_feedback[motor_id].angle = (RxData[0] << 8) | RxData[1];
motor_feedback[motor_id].rpm = (RxData[2] << 8) | RxData[3];
motor_feedback[motor_id].current = (RxData[4] << 8) | RxData[5];
motor_feedback[motor_id].temperature = RxData[6];
// 获取新角度值
int16_t new_angle = motor_feedback[motor_id].angle;
// 3. 【核心算法】计算连续总角度（修正版）
// 计算角度变化量（考虑溢出）
int32_t delta = (int32_t)new_angle - (int32_t)old_angle;
// 处理溢出：如果变化量超过半圈（4096），认为发生了溢出
if (delta > 4096) {
// 正向溢出：例如从8191变为10，实际变化是+19，但计算出的delta是-8181
// 修正delta：delta = (new_angle + 8192) - old_angle
delta -= 8192;
} else if (delta < -4096) {
// 反向溢出：例如从10变为8191，实际变化是-19，但计算出的delta是+8181
// 修正delta：delta = (new_angle - 8192) - old_angle
delta += 8192;
}
// 更新总角度（累加变化量）
motor_feedback[motor_id].total_angle += delta;
// 更新上一次的原始角度值，为下一次计算做准备
motor_feedback[motor_id].last_angle = new_angle;
// 可选：添加调试输出
// printf("Motor %d: raw=%d, total=%.1f, delta=%ld\n",
// motor_id+1, new_angle, motor_feedback[motor_id].total_angle, delta);
}
}
}
/**
* @brief PID计算函数
* @param pid: PID结构体指针
* @retval PID输出值
*/
float PID_Calculate(PID_TypeDef *pid) {
// 1. 计算误差
pid->Error = pid->Target - pid->Current;
// 2. 比例项
float p_out = pid->Kp * pid->Error;
// 3. 积分项 (并限幅)
pid->Integral += pid->Error;
// 积分限幅
if (pid->Integral > pid->IntegralLimit) pid->Integral = pid->IntegralLimit;
if (pid->Integral < -pid->IntegralLimit) pid->Integral = -pid->IntegralLimit;
float i_out = pid->Ki * pid->Integral;
// 4. 微分项
pid->Derivative = pid->Error - pid->LastError;
float d_out = pid->Kd * pid->Derivative;
pid->LastError = pid->Error; // 更新上一次误差
// 5. 计算总输出
pid->Output = p_out + i_out + d_out;
// 6. 输出限幅
if (pid->Output > pid->OutputLimit) pid->Output = pid->OutputLimit;
if (pid->Output < -pid->OutputLimit) pid->Output = -pid->OutputLimit;
return pid->Output;
}
/* USER CODE END 4 */
5.主循环调用，540*19意思是转动角度*电机减速比，实现输出轴转动540度。
while (1)
{
//=========================纯开环模式，不加PID===================================================
// CAN_Send_Motor_Command(400, 0, 0, 0); // 发送指令：电机1电流1000，其他电机0
//========================仅速度环模式，记得调好参数================================================================
// // 1. 设置目标速度 (例如：让电机1以 1000 RPM 的速度转动)
// speed_pid_m1.Target = 1000.0f;
// // 2. 更新当前速度 (从反馈数据中获取)
// // motor_feedback[0] 对应ID为1的电机 (因为 motor_id = StdId - 0x201)
// speed_pid_m1.Current = motor_feedback[0].rpm;
// // 3. 进行PID计算，得到需要发送的电流值
// int16_t output_current = (int16_t)PID_Calculate(&speed_pid_m1);
// // 4. 发送电流指令
// CAN_Send_Motor_Command(output_current, 0, 0, 0);
// // 5. 控制周期延时，保持控制频率稳定 (例如1kHz)
// HAL_Delay(1);
//=========================角度环与速度环的串级PID控制==============================================================================
// // 【双环控制核心逻辑】
// --- 第1层：角度环计算 ---
// eg.1. 设置目标总角度 (例如：让电机1转动 18000°)
// 注意：目标单位是“编码器值”，18000 代表大约2.2圈 (18000 / 8192 ≈ 2.2)
// --- 第1层：角度环计算 ---
// 1. 设置目标角度（单位：度）
float target_angle_degree = 540.0f*19; // 例如：让电机转到90度位置
// 2. 将角度值转换为编码器值
angle_pid_m1.Target = degree_to_encoder(target_angle_degree);
// 3. 更新当前总角度（从中断计算中获取）
angle_pid_m1.Current = motor_feedback[0].total_angle;
// 4. 进行角度PID计算
// 输出的是为了达到目标角度，建议电机旋转的速度（RPM）
float target_speed_from_angle = PID_Calculate(&angle_pid_m1);
// --- 第2层：速度环计算 ---
// 5. 将角度环计算的"建议速度"设置为速度环的目标
speed_pid_m1.Target = target_speed_from_angle;
// 6. 更新当前速度
speed_pid_m1.Current = motor_feedback[0].rpm;
// 7. 进行速度PID计算
// 输出的是为了达到目标速度，需要给电机提供的电流值
int16_t output_current = (int16_t)PID_Calculate(&speed_pid_m1);
// 8. 发送最终的电流指令
CAN_Send_Motor_Command(output_current, 0, 0, 0);
// 控制周期延时，保持控制频率稳定 (1kHz)
HAL_Delay(1);
}
/* USER CODE END WHILE */6.代码整体框架：
/* USER CODE BEGIN Header */
/**
* All rights reserved. 26th电控考核试题
******************************************************************************
*//* ------------------------------------------------------------------------------
* 愿佛祖保佑代码无BUG：
* _oo0oo_
* o8888888o
* 888" . "88
* (| -_- |)
* 0\ = /0
* ___/`---'\___
* .' \\| |// '.
* / \\||| : |||// \
* / _||||| -:- |||||- \
* | | \\\ - /// | |
* | \_| ''\---/'' |_/ |
* \ .-\__ '-' ___/-. /
* ___'. .' /--.--\ `. .'___
* ."" '< `.___\_<|>_/___.' >' "".
* | | : `- \`.;`\ _ /`;.`/ - ` : | |
* \ \ `_. \_ __\ /__ _/ .-` / /
* =====`-.____`.___ \_____/___.-`___.-'=====
* `=---='
* ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
* 佛祖保佑 永无BUG
* ------------------------------------------------------------------------------
* user：！！！ZCT！！！，代码非最终版本，有锅大家一起修！！！！
*硬件连接 (非常重要！)
* STM32F103C8T6的CAN引脚是：
*
*CAN_RX -> PA11 (必须)
*
*CAN_TX -> PA12 (必须)
*
*TJA1050模块：
*
*TXD 连接 STM32的 PA12 (CAN_TX)
*
*RXD 连接 STM32的 PA11 (CAN_RX)
*
*VCC 接 5V 或 3.3V (请根据模块手册决定)
*
* GND 共地
*
* CANH 和 CANL 连接到RM3508的CAN总线上（务必使用双绞线，末端最好接一个120欧姆的终端电阻）
*
* This software is licensed under terms that can be found in the LICENSE file
*
//关于硬件调试*/
/*若实际测试中电机不转，可按以下步骤排查：
用万用表测 TJA1050 的 VCC 和 GND，确认供电正常（无掉电、无短路）；
用示波器测 STM32 PA12（CAN_TX）和 TJA1050 CANH 引脚，确认有 CAN 信号输出（发送指令时波形变化）；
检查MX_CAN_Init()的波特率配置，确保为 1Mbps；
确认 RM3508 的电源供电正常（12V/24V 电压稳定，无过流保护）
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "can.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
// 定义电机反馈结构体（存储单个电机的反馈数据）
typedef struct {
int16_t angle; // 角度（原始编码器值，0-8191）
int16_t rpm; // 转速（rpm）
int16_t current; // 实际转矩电流
uint8_t temperature; // 温度（℃）
// 【新增】以下两个变量用于计算连续总角度
int16_t last_angle; // 上一次的原始编码器值，用于判断溢出
float total_angle; // 连续的总角度值（单位：编码器值） = 圈数 * 8192 + angle
} MotorFeedbackTypeDef;
// 定义4个电机的反馈数组（对应1~4号电机）
MotorFeedbackTypeDef motor_feedback[4] = {0}; // 全局变量，初始化为0
CAN_TxHeaderTypeDef TxHeader;// CAN发送报文头结构体（存储ID、数据长度等信息）
uint8_t TxData[8]; // CAN发送数据缓冲区（标准CAN帧最多8字节）
uint32_t TxMailbox; // CAN发送邮箱（用于存储发送状态）
CAN_RxHeaderTypeDef RxHeader;// CAN接收报文头结构体
uint8_t RxData[8];// CAN接收数据缓冲区
// 定义PID结构体
typedef struct {
float Target; // 目标值（此处为目标速度）
float Current; // 当前值（此处为反馈的实际速度）
float Error; // 当前误差
float LastError; // 上一次误差
float Integral; // 积分项
float Derivative; // 微分项
float Kp; // 比例系数
float Ki; // 积分系数
float Kd; // 微分系数
float Output; // PID输出值
float IntegralLimit; // 积分限幅
float OutputLimit; // 输出限幅
} PID_TypeDef;
// 为电机1定义一个速度环PID控制器
PID_TypeDef speed_pid_m1;
// 为电机1定义一个角度环PID控制器
PID_TypeDef angle_pid_m1; // 使用和速度环相同的PID结构体
//// 新增全局变量：用于计算电机1的连续多圈总角度
//int16_t last_angle_raw_m1 = 0; // 上一次的原始编码器值
//float total_angle_m1 = 0.0f; // 计算出的连续总角度值
//int32_t rounds_m1 = 0; // 圈数计数
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
float PID_Calculate(PID_TypeDef *pid);
void CAN_Send_Motor_Command(int16_t current1, int16_t current2, int16_t current3, int16_t current4) {
// 1. 填充报文头
TxHeader.StdId = 0x200; // 标准ID：电机控制指令通常用0x200（大疆协议约定）
TxHeader.ExtId = 0; // 扩展ID：不使用（设为0）
TxHeader.RTR = CAN_RTR_DATA; // 帧类型：数据帧（非远程请求帧）
TxHeader.IDE = CAN_ID_STD; // 标识符类型：标准ID（11位）
TxHeader.DLC = 8; // 数据长度：8字节（满长度）
TxHeader.TransmitGlobalTime = DISABLE; // 不包含发送时间戳
// 2. 填充数据（大疆电机的数据格式）
// 注意字节序：小端模式（低字节在前，高字节在后）
TxData[0] = current1 >> 8; // 电流1的高8位（16位整数拆分）
TxData[1] = current1 & 0xFF; // 电流1的低8位
TxData[2] = current2 >> 8; // 电流2的高8位
TxData[3] = current2 & 0xFF; // 电流2的低8位
TxData[4] = current3 >> 8; // 电流3的高8位
TxData[5] = current3 & 0xFF; // 电流3的低8位
TxData[6] = current4 >> 8; // 电流4的高8位
TxData[7] = current4 & 0xFF; // 电流4的低8位
// 3. 发送：增加3次重试，避免一次失败卡死
uint8_t retry = 3; // 重试次数
while (retry-- > 0) {
if (HAL_CAN_AddTxMessage(&hcan, &TxHeader, TxData, &TxMailbox) == HAL_OK) {
return; // 发送成功，退出函数
}
HAL_Delay(1); // 重试间隔1ms，给CAN邮箱释放时间
}
}
/* USER CODE BEGIN PFP */
// 角度值到编码器值的转换函数
float degree_to_encoder(float degree) {
// RM3508电机一圈是8192个编码器值，对应360度
// 所以：编码器值 = (角度 / 360) * 8192
return (degree / 360.0f) * 8192.0f;
}
// 编码器值到角度值的转换函数（用于调试显示）
float encoder_to_degree(float encoder) {
return (encoder / 8192.0f) * 360.0f;
}
/* USER CODE END PFP */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
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_CAN_Init();
/* USER CODE BEGIN 2 */
// 首先，发送零电流指令持续一段时间来解锁电机
uint32_t unlock_time = HAL_GetTick();
while (HAL_GetTick() - unlock_time < 200) { // 持续发送200ms的零电流
CAN_Send_Motor_Command(0, 0, 0, 0);
HAL_Delay(1); // 稍微延迟一下，不要堵死循环
}
/*相关PID参数仅供参考，需要进行参数调试*/
// 初始化速度环PID参数======================================================================
speed_pid_m1.Target = 0.0f; // 初始目标速度设为0
speed_pid_m1.Kp = 5.0f; // 需要调试：比例系数，从小到大试
speed_pid_m1.Ki = 0.1f; // 需要调试：积分系数，补足静差
speed_pid_m1.Kd = 0.0f; // 需要调试：微分系数，先设为0
speed_pid_m1.IntegralLimit = 1000.0f; // 积分限幅，防止积分饱和
speed_pid_m1.OutputLimit = 16384.0f; // 输出限幅，最大电流值
speed_pid_m1.Integral = 0.0f;
speed_pid_m1.LastError = 0.0f;
speed_pid_m1.Output = 0.0f;
//初始化角度环PID参数 ---================================================================================
angle_pid_m1.Target = 0.0f; // 初始目标角度设为0
angle_pid_m1.Kp = 0.8f; // 【关键】需要调试：这个参数决定“多快”地冲向目标角度。建议从0.5开始试。
angle_pid_m1.Ki = 0.0f; // 角度环通常不需要积分项(I)，先设为0
angle_pid_m1.Kd = 0.0f; // 先设为0
angle_pid_m1.IntegralLimit = 1000.0f;
angle_pid_m1.OutputLimit = 1000.0f; // 【关键】输出限幅：这个值限制了角度环能命令速度环达到的最大速度。
// 例如设为5000，意味着角度环最多命令电机以5000RPM的速度去追角度。
// 这个值越大，电机转得越快，但也容易过冲和振荡。请从2000开始慢慢加大。
angle_pid_m1.Integral = 0.0f;
angle_pid_m1.LastError = 0.0f;
angle_pid_m1.Output = 0.0f;
/*
【参数调试】耐心调试PID：
第一步：将 Kp, Ki, Kd 全部设为0。电机应不转。
第二步：只调P。慢慢增大 Kp（比如从1.0 -> 2.0 -> 5.0 -> 10.0...），观察电机是否开始转动并加速。目标是让电机能稳定在一个接近目标速度（1000RPM）的值，即使有静差（比如只能稳定在800RPM）也没关系。
第三步：加入I。慢慢增大 Ki（比如从0.01 -> 0.05 -> 0.1...），观察电机速度是否逐渐接近并最终稳定在目标速度1000RPM上，消除静差。
第四步：（可选）如果电机在目标速度附近震荡，加入一点点 Kd（比如0.1）来抑制震荡。
*/
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
//=========================纯开环模式，不加PID===================================================
// CAN_Send_Motor_Command(400, 0, 0, 0); // 发送指令：电机1电流1000，其他电机0
//========================仅速度环模式，记得调好参数================================================================
// // 1. 设置目标速度 (例如：让电机1以 1000 RPM 的速度转动)
// speed_pid_m1.Target = 1000.0f;
// // 2. 更新当前速度 (从反馈数据中获取)
// // motor_feedback[0] 对应ID为1的电机 (因为 motor_id = StdId - 0x201)
// speed_pid_m1.Current = motor_feedback[0].rpm;
// // 3. 进行PID计算，得到需要发送的电流值
// int16_t output_current = (int16_t)PID_Calculate(&speed_pid_m1);
// // 4. 发送电流指令
// CAN_Send_Motor_Command(output_current, 0, 0, 0);
// // 5. 控制周期延时，保持控制频率稳定 (例如1kHz)
// HAL_Delay(1);
//=========================角度环与速度环的串级PID控制==============================================================================
// // 【双环控制核心逻辑】
// --- 第1层：角度环计算 ---
// eg.1. 设置目标总角度 (例如：让电机1转动 18000°)
// 注意：目标单位是“编码器值”，18000 代表大约2.2圈 (18000 / 8192 ≈ 2.2)
// --- 第1层：角度环计算 ---
// 1. 设置目标角度（单位：度）
float target_angle_degree = 540.0f*19; // 例如：让电机转到90度位置
// 2. 将角度值转换为编码器值
angle_pid_m1.Target = degree_to_encoder(target_angle_degree);
// 3. 更新当前总角度（从中断计算中获取）
angle_pid_m1.Current = motor_feedback[0].total_angle;
// 4. 进行角度PID计算
// 输出的是为了达到目标角度，建议电机旋转的速度（RPM）
float target_speed_from_angle = PID_Calculate(&angle_pid_m1);
// --- 第2层：速度环计算 ---
// 5. 将角度环计算的"建议速度"设置为速度环的目标
speed_pid_m1.Target = target_speed_from_angle;
// 6. 更新当前速度
speed_pid_m1.Current = motor_feedback[0].rpm;
// 7. 进行速度PID计算
// 输出的是为了达到目标速度，需要给电机提供的电流值
int16_t output_current = (int16_t)PID_Calculate(&speed_pid_m1);
// 8. 发送最终的电流指令
CAN_Send_Motor_Command(output_current, 0, 0, 0);
// 控制周期延时，保持控制频率稳定 (1kHz)
HAL_Delay(1);
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}
}
/* USER CODE BEGIN 4 */
//CAN 接收中断回调函数
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData) == HAL_OK) {
if (RxHeader.StdId >= 0x201 && RxHeader.StdId <= 0x204) {
uint8_t motor_id = RxHeader.StdId - 0x201;
// 1. 保存旧的原始角度值（从结构体中获取）
int16_t old_angle = motor_feedback[motor_id].last_angle;
// 2. 解析数据（大疆标准协议）
motor_feedback[motor_id].angle = (RxData[0] << 8) | RxData[1];
motor_feedback[motor_id].rpm = (RxData[2] << 8) | RxData[3];
motor_feedback[motor_id].current = (RxData[4] << 8) | RxData[5];
motor_feedback[motor_id].temperature = RxData[6];
// 获取新角度值
int16_t new_angle = motor_feedback[motor_id].angle;
// 3. 【核心算法】计算连续总角度（修正版）
// 计算角度变化量（考虑溢出）
int32_t delta = (int32_t)new_angle - (int32_t)old_angle;
// 处理溢出：如果变化量超过半圈（4096），认为发生了溢出
if (delta > 4096) {
// 正向溢出：例如从8191变为10，实际变化是+19，但计算出的delta是-8181
// 修正delta：delta = (new_angle + 8192) - old_angle
delta -= 8192;
} else if (delta < -4096) {
// 反向溢出：例如从10变为8191，实际变化是-19，但计算出的delta是+8181
// 修正delta：delta = (new_angle - 8192) - old_angle
delta += 8192;
}
// 更新总角度（累加变化量）
motor_feedback[motor_id].total_angle += delta;
// 更新上一次的原始角度值，为下一次计算做准备
motor_feedback[motor_id].last_angle = new_angle;
// 可选：添加调试输出
// printf("Motor %d: raw=%d, total=%.1f, delta=%ld\n",
// motor_id+1, new_angle, motor_feedback[motor_id].total_angle, delta);
}
}
}
/**
* @brief PID计算函数
* @param pid: PID结构体指针
* @retval PID输出值
*/
float PID_Calculate(PID_TypeDef *pid) {
// 1. 计算误差
pid->Error = pid->Target - pid->Current;
// 2. 比例项
float p_out = pid->Kp * pid->Error;
// 3. 积分项 (并限幅)
pid->Integral += pid->Error;
// 积分限幅
if (pid->Integral > pid->IntegralLimit) pid->Integral = pid->IntegralLimit;
if (pid->Integral < -pid->IntegralLimit) pid->Integral = -pid->IntegralLimit;
float i_out = pid->Ki * pid->Integral;
// 4. 微分项
pid->Derivative = pid->Error - pid->LastError;
float d_out = pid->Kd * pid->Derivative;
pid->LastError = pid->Error; // 更新上一次误差
// 5. 计算总输出
pid->Output = p_out + i_out + d_out;
// 6. 输出限幅
if (pid->Output > pid->OutputLimit) pid->Output = pid->OutputLimit;
if (pid->Output < -pid->OutputLimit) pid->Output = -pid->OutputLimit;
return pid->Output;
}
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
四、如何知道3508转了多少度？
1.RM3508编码器与8192的由来
RM3508内部有一个绝对值编码器，它一圈会输出8192个数字（0-8191）
物理意义:8192=360度
换算关系：1个数值=360/8192=0.0439度
在CAN帧中解析出的angle= (RxData[0] << 8) | RxData[1]，angle这个值就代表电机在当前这一圈内的绝对位置（0-8191）
2.核心难点问题：溢出与多圈处理
编码器只能报告一圈内的位置（0-8191）.当电机转过一圈时，这个值会从8191突变成0，难以实现精准的角度控制。
加入算法进行处理：
检测相邻两次角度值的差值。如果这个差值过大，超过半圈（4096），就认为是发生了一圈的溢出，并相应地增加或减少圈数。
3.算法目标与核心
1.目标：
从“单圈”跳变的raw_angle中还原出“连续的多圈总角度”
2.核心：
4096阈值法：正常的采样周期内，电机不可能转超过半圈（4096）。
如果delta>+4096，一定是正向溢出（如8191-10）；
如果delta<-4096，一定是反向向溢出（如10-8191）；
即用4096做门槛，8192做修正，float做累加，total_angle永不跳变
4.补充部分
RM3508的编码器特性：
1.单圈分辨率:8192(0-8191)
2.每转一圈，数值从8191跳变到0，如果直接用angle做PID反馈，每次经过零点就会抖动一次。
五、总结与感谢
这已经是stm32f103控制RM3508角度环最后一篇文章，很感谢有这次机会可以来分享本人的一些小经验，感觉又对整套系统有了更深刻的理解。相信你只要是根据我的三篇文章一步步的去做，即使是小白也能实现最终的控制效果。这一篇文章字数较多，代码后续还需要完善，手打过程中难免会出现一些错误，还请大家多多担待并大胆指出，作者会积极的改正自己的错误之处。
最后，感谢大家的观看与指导，本人自身技术有限，还有待提高，希望大佬们不吝赐教！
下面推荐一下同僚的文章，写的非常优秀：
基于大疆RM3508电机的串级PID（角度环＋速度环）_can通讯pid角度环和速度环控制电机转角度-CSDN博客
智能硬件社区聚焦AI智能硬件技术生态，汇聚嵌入式AI、物联网硬件开发者，打造交流分享平台，同步全国赛事资讯、开展 OPC 核心人才招募，助力技术落地与开发者成长。
更多推荐
78
0- 0
已为社区贡献3条内容
所有评论(0)