STM32移植DengFOC：从无刷电机FOC到云台精确位置控制实战

简介：基于STM32与HAL库的DengFOC闭环位置控制移植包，面向电机控制开发者和嵌入式进阶学习者，主要解决在标准外设库或自制工程中引入FOC位置闭环算法的移植与验证难题，适合正在开展无刷电机位置伺服项目的读者。压缩包共包含183个文件，以头文件、源文件、编译目标文件为主，另有Keil工程配置文件、可烧录的HEX文件、调试用的AXF文件以及IOC初始化配置，各类型分别对应源码阅读、编译构建、固件烧录和引脚初始化等环节，整体仅约1.4MB，便于下载与对照分析。包内基于STM32F4系列HAL库编写，覆盖定时器、I2C、UART、DMA等常用外设驱动，可完整呈现闭环位置控制中电流采样、速度环、位置环的协作流程，帮助理解DengFOC算法向HAL工程移植时需要注意的时钟配置、中断优先级和反馈接口适配等细节。目前该项目已有583人学习，特别适合正在调试FOC位置环或打算快速搭建实验平台的开发者直接参考。

1. 一个云台电机改造：从“会转”到“精确定位”的距离

做桌面级云台那阵子，我手里有一块 F103 开发板和一只云台无刷电机，电机尾部带磁编码器。直接给三相 PWM，电机能转，但想让它停在指定角度时，要么来回抖得像电钻，要么干脆堵转。难点不在“转”，而在“位置闭环”。DengFOC 是一套面向无刷电机和云台电机的 FOC 控制开源方案，把它移植到 STM32+HAL 工程后，配合 AS5600 这类磁编码器，位置环 PID 一接上，电机就能按指令精确停在任意角度，重复精度能做到零点几度。这个项目适合想做云台、机械臂关节、转台，但不想从零手写 Park 变换和 SVPWM 的开发者。

2. 架构与控制链路：位置环、速度环、电流环三层嵌套如何打通

2.1 三级闭环：从目标角度到三相电流的传导路径

在把 DengFOC 跑起来之前，我习惯把 FOC 想成一个“黑匣子”：给个电压矢量，电机就能转。但做位置控制时，这个黑匣子必须拆开看。FOC 位置控制是典型的三环结构——最外层是位置环，输入目标角度，输出目标速度；中间是速度环，输入目标速度，输出 q 轴电压/电流；最内层是电流环，读回相电流算 Id/Iq，输出 Ud/Uq。电压再经逆 Park 变换得到 Uα/Uβ，最后通过 SVPWM 换算成三路 PWM 占空比，送到 TIM1 互补通道驱动三相逆变桥。

DengFOC 的优势在于把这三环拆成了可分别调用的函数。做位置控制时，位置环的输出要接进速度环的输入，速度环再决定 Uq。也就是说，位置控制不是单独一个 PID，而是“位置 PID 包着速度 PID，速度 PID 包着电流 PI”。参数整定必须从内往外调，先电流环再速度环再位置环，内环没稳，外环调得再猛也会抖，这个顺序我后面在二次开发章节还会展开。

2.2 硬件资源映射与软件分层：HAL 库替 DengFOC 干了哪些脏活

DengFOC 原版代码在 Arduino 类环境里跑得顺，但到 STM32 上，平台相关的部分全部要换成 HAL 库。移植的本质是：算法层不动，把“PWM 输出、ADC 采样、编码器读取、延时”四类底层操作换掉。

• PWM：TIM1 高级定时器，三对互补 PWM，中心对齐模式，死区时间在 Break and Dead Time 里配置。PWM 频率我一般选 20kHz，高于人耳听域，云台电机也配合。
• 电流采样：ADC1 注入通道，触发源选 TIM1_TRGO，PWM 每个周期都会触发一次采样，不占 CPU。双电阻采样 A/B 相，C 相软件重构。
• 编码器：AS5600 在 ABZ 模式下输出方波，接 TIM3 编码器模式，硬件四倍频计数，CPU 不用管方向。
• 串口：USART1 打印电角度偏移量、目标/实际位置和 PID 输出，调试全靠它。

硬件资源映射可以按下表抄作业，这是我在 F103 上的配置，F407 或国产替代芯片同理：

软件分层上，我习惯把 DengFOC 算法文件当成一个不动的核心层，新建一个 driver 层文件放 HAL 回调：DFOC_SetPWM(ch, duty)、DFOC_ReadCurrent(phase)、DFOC_ReadEncoder()、DFOC_DelayMs(ms)。移植时只改 driver 层，改坏了最多是底层驱动出错，不会把控制算法搞乱。这一步虽小，后面 Debug 能省大量时间。

2.3 极对数、编码器分辨率与角度换算：最容易算错的一组数

云台电机常见极对数 P=7，意思是电机转子转一圈，电角度要走 7 个完整 2π。做位置闭环时，位置环的反馈量用机械角度（编码器读数），而电流环和 SVPWM 必须用电角度。DengFOC 内部的核心换算关系是：

电角度 = fmod(机械角度 × 极对数, 2π)

很多第一次移植的人在这翻车，比如校准出来的电角度偏移量不对，电机上电后锁不住、发热，或者转半圈就卡死。我一般在初始化完编码器后，先用开环电压让电机转起来，同时串口打印计算出的电角度和速度，确认换算关系再进闭环。AS5600 在 ABZ 模式四倍频后是 4096 计数，I2C 模式可以读到 4096 或 16384；如果换了编码器，DengFOC 的读取函数要按自己硬件改。常见做法是维护一个累计圈数变量，配合溢出中断防止计数回转。

注意：如果编码器计数方向跟电机正转方向相反，位置闭环会变成正反馈，一启动就飞车。方向问题要么改 TIM3 的编码器极性，要么在 DFOC_ReadEncoder() 里乘一个方向系数。这个细节后面避坑章节还会专门讲。

3. 移植落地：CubeMX 配置、源码接入与第一次上电

3.1 CubeMX 工程配置：先让外设就位

打开 CubeMX，芯片选型按手头板子来。以 F103 为例，时钟树配到 72MHz，APB1 定时器时钟确认好，后面算 PWM 重载值要用。TIM1 勾选 Channel1/2/3 的 PWM Generation 和互补输出，Pulse 全部填 0。计数器设为 Center-Aligned Mode 2，预分频和重载值按目标频率 20kHz 计算。死区时间在 Break and Dead Time 配置里调，云台电机驱动器建议 1us 起步，太小容易上下桥直通炸管，太大会让电流波形畸变。

ADC1 打开注入通道，选择两路，采样时间先用默认，后面看波形再压。TIM1 的 TRGO 设为 Update Event，接到 ADC 触发。TIM3 配置为 Encoder Mode，计数范围 0xFFFF，不使能预装载。USART1 开 115200。

提示：部分板子的编码器接口和调试口共用 GPIO，进 CubeMX 前先查原理图。AS5600 的 ABZ 模式 A/B 接 TIM3 的 CH1/CH2，Z 轴可以不接，靠初始化时清零计数。

配置完生成 HAL 工程，先编译一次确认没有硬件层面错误，再动代码。

3.2 接入源码：拷文件、改宏、填 HAL 底层函数

把 DengFOC 的 .c/.h 放进 Core/Src 和 Core/Inc。DengFOC.h 顶层是电机参数宏，我按这个格式维护：

// DengFOC_param.h 电机参数配置 #define MOTOR_POLE_PAIRS 7 // 极对数，云台电机常见 7 或 14 #define ENCODER_RESOLUTION 4096.0f // 编码器单圈计数（四倍频后） #define PWM_FREQ_HZ 20000 // 与 CubeMX 配置保持一致 #define VOLTAGE_LIMIT 12.0f // 母线电压，用于占空比换算 #define CURRENT_SENSE_GAIN 66.0f // 电流采样放大倍数，按硬件改

参数说明：VOLTAGE_LIMIT 直接影响 DengFOC 内部从电压到占空比的换算。母线电压不稳定，低速就会一顿一顿。CURRENT_SENSE_GAIN 来自电流采样电路的放大倍数，具体值看硬件原理图，量不准的话电流环基本没法用。

底层适配层的核心函数长这样：

// driver_stm32.c HAL 底层适配层 void DFOC_SetPWM(uint8_t ch, float duty) { // duty 范围 0~1，换算成比较寄存器的值 uint32_t arr = __HAL_TIM_GET_AUTORELOAD(&htim1); switch(ch) { case 0: __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, (uint32_t)(duty * arr)); break; case 1: __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, (uint32_t)(duty * arr)); break; case 2: __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, (uint32_t)(duty * arr)); break; default: break; } } uint16_t DFOC_ReadEncoder(void) { return (uint16_t)__HAL_TIM_GET_COUNTER(&htim3); // 编码器计数 }

逻辑说明：DFOC_SetPWM 把归一化占空比写入 TIM1 的三路比较寄存器，HAL 的 SET_COMPARE 宏要求 channel 参数是编译期常量，case 里写死是标准做法。DFOC_ReadEncoder 直接读 TIM3 计数器，编码器硬件四倍频后的值就是机械角度的原始分辨率。

3.3 开环验证与电角度校准：让电机先“听话”地转

接入编码器和 PWM 后，不急着写位置闭环。先跑开环电角度扫描：把电角度从 0 递增到 2π，同时给一个固定电压，电机应该平滑转动。如果电机锁死、发热或者抖，先查三相 PWM 相序对不对，交换任意两相 PWM 输出再试。这种问题跟相序错位有直接关系，下面什么闭环都白搭。我一般把电角度步进设成 0.01rad，电压设 2V，转顺了再慢慢加。

开环正常后，做一次电角度零点校准。DengFOC 的思路是：给一个固定电角度的电压，转子会锁到某个机械位置，这时编码器读数对应的电角度就是偏移量。校准函数在下一章给完整代码。校准完，把打印出来的偏移值保存到全局变量。注意断电再开机编码器计数会复位，偏移值没变的话可以直接写死在宏里；但换电机或者重装编码器后必须重新校准。

上电瞬间如果电机猛抽一下，多半是进入闭环前占空比不为 0。养成习惯：主循环进闭环前先调 DFOC_SetPWM(0,0)，等几十毫秒，再使能位置环。这个细节能少烧好几块驱动板。

4. 核心代码拆解：电角度零点校准与位置环级联实现

4.1 电角度偏移校准：让闭环找到“零位”

这一步是 FOC 位置控制里最容易被跳过、又最影响成败的环节。校准的目的是告诉算法：编码器读到的哪个机械角度，对应电角度的 0 点。

// 电角度零点校准：把转子锁到固定位置，记录编码器读数 #include <math.h> #define _PI 3.141592653589793f float zero_electric_angle = 0.0f; void DFOC_CalibrateZeroElectricAngle(void) { // 1. 施加固定电角度 90 度，电压 3V，转子会锁到最近的磁势位置 DFOC_setPhaseVoltage(3.0f, 0.0f, _PI / 2.0f); HAL_Delay(1200); // 2. 稳定后读取编码器原始值，换算成机械角（弧度） uint16_t raw = DFOC_ReadEncoder(); float mech_angle = (float)raw / ENCODER_RESOLUTION * 2.0f * _PI; // 3. 机械角乘以极对数并取模 2PI，得到电角度偏移 zero_electric_angle = fmodf(mech_angle * MOTOR_POLE_PAIRS, 2.0f * _PI); if (zero_electric_angle < 0.0f) zero_electric_angle += 2.0f * _PI; // 4. 关断输出，稍等再进入闭环 DFOC_setPhaseVoltage(0.0f, 0.0f, 0.0f); HAL_Delay(200); printf("Zero Angle Offset: %.3f rad\r\n", zero_electric_angle); }

逻辑说明：固定电角度 90 度，本质是给电机一个确定的磁场方向，转子会锁到最近的换向位置。编码器此时读到的机械角，乘以极对数后就是电角度零点。以后每次算电角度都要先减掉这个偏移量，再取模 2π。参数说明：3.0V 是锁定电压，太小锁不住，太大会让转子在锁定点附近过冲；90 度只是习惯值，任意固定角度都可以，但必须和后续算法约定一致。12.0 这个延时给足 1 秒以上，电机机械响应需要时间。

4.2 位置环 + 速度环级联：目标角度到 Uq 电压

位置控制的核心是两个 PID 串在一起。位置环输出的是目标速度，速度环输出的是 Uq 电压。下面是我在 STM32 上跑通的简化版，每 1ms 在主循环调用一次：

// 位置环：输入目标位置(弧度)，输出目标速度(rad/s) #define POS_KP 12.0f // 位置环比例，越大越硬但容易震荡 #define POS_KI 0.05f // 积分项，消除静态误差 #define POS_KD 0.8f // 微分项，抑制超调 #define VEL_LIMIT 20.0f // 最大转速 rad/s static float pos_error_prev, pos_integral; float DFOC_PositionLoop(float target_pos, float current_pos) { // 1. 计算角度误差，并做最短路径处理（绕 2PI 边界） float error = target_pos - current_pos; if (error > _PI) error -= 2.0f * _PI; if (error < -_PI) error += 2.0f * _PI; // 2. 积分限幅，防止长时间堵转导致积分饱和 pos_integral += error; if (pos_integral > 10.0f) pos_integral = 10.0f; if (pos_integral < -10.0f) pos_integral = -10.0f; // 3. 微分项用误差变化率，这里对误差做了一阶低通，减少编码器噪声 float derivative = (error - pos_error_prev) * 0.5f; // 4. 合成输出 = 目标速度，交给速度环 float target_vel = POS_KP * error + POS_KI * pos_integral + POS_KD * derivative; if (target_vel > VEL_LIMIT) target_vel = VEL_LIMIT; if (target_vel < -VEL_LIMIT) target_vel = -VEL_LIMIT; pos_error_prev = error; return target_vel; } // 速度环：输入目标速度，输出 Uq 电压，再进 FOC float DFOC_SpeedLoop(float target_vel, float current_vel) { float vel_error = target_vel - current_vel; return PID_Speed(&speed_pid, vel_error); // speed_pid 用同样的 PID 结构体 } // 主循环：位置环 + 速度环 + FOC 一步到位 void DFOC_PositionControl(float target_pos, float current_pos, float elec_angle) { float vel_target = DFOC_PositionLoop(target_pos, current_pos); float vel_now = (current_pos - last_pos) / DT_SEC; // 编码器差分测速 float uq = DFOC_SpeedLoop(vel_target, vel_now); DFOC_setPhaseVoltage(uq, 0.0f, elec_angle); last_pos = current_pos; }

逻辑说明：位置环的误差先做最短路径修正，否则角度在 0 和 2π 边界跳变时，PID 会误判误差接近 2π 而产生剧烈反转。积分限幅 10.0 是经验值，堵转时误差持续累积，不加限幅就会积分饱和，松开后要好几秒才能回来。微分项乘 0.5 相当于一个简陋低通，编码器量化噪声在差分时会放大，这是实际调参中抖动的主要来源。速度环的输出直接进 DFOC_setPhaseVoltage，第一个参数是 Uq，第二个 0 是 Ud（表贴式电机一般不需要 d 轴电压），第三个是当前电角度。

参数说明：POS_KP=12 适合云台这类轻负载，负载越重这个值越低；POS_KI 的 0.05 主要消除重力的静态误差，水平转台可以设 0；POS_KD 是压震荡用的，编码器噪声大时反而要减小甚至设 0。速度环 PID 的整定经验值在最后二次开发章节给。

4.3 SVPWM：从 DQ 电压到三相占空比的最后一跳

DFOC_setPhaseVoltage 内部是标准 FOC 流程：逆 Park 变换、SVPWM、更新比较寄存器。逆 Park 变换的公式约定必须和编码器方向、电角度零点对齐：

// 逆 Park：dq 轴电压 → αβ 轴电压 float u_alpha = ud * cosf(elec_angle) - uq * sinf(elec_angle); float u_beta = ud * sinf(elec_angle) + uq * cosf(elec_angle);

SVPWM 的扇区判断是核心之一，这段代码只展示判断逻辑，完整的作用时间计算通常用查表法：

#define SQRT3 1.7320508f uint8_t DFOC_Sector(float u_alpha, float u_beta) { if (u_beta >= 0) { if (u_alpha >= 0) return (u_beta > SQRT3 * u_alpha) ? 2 : 1; else return (u_beta > -SQRT3 * u_alpha) ? 3 : 4; } else { if (u_alpha >= 0) return (-u_beta > SQRT3 * u_alpha) ? 5 : 6; else return (-u_beta > -SQRT3 * u_alpha) ? 4 : 5; } }

逻辑说明：SVPWM 把 αβ 平面分成 6 个 60 度扇区，每个扇区用相邻两个基础电压矢量合成目标矢量。扇区判断只是第一步，后面要根据扇区查表计算两个基础矢量的作用时间 T1、T2，再映射到三相比较寄存器。这里判断用的是电压分量比值和符号，完整换算在 DengFOC 源码里有现成实现，直接套用即可。参数说明：u_alpha、u_beta 是电压矢量在 αβ 轴的投影，单位伏特；SQRT3 是根号 3，来源于六边形电压矢量边界的几何关系。如果电机转向反了，优先检查逆 Park 的符号约定和编码器方向，而不是去改扇区表。

5. 避坑指南：移植 DengFOC 最容易翻车的五个细节

5.1 校准时电机不停震荡锁不住

现象：执行电角度零点校准时，转子在某个位置附近来回摆动，始终稳定不下来，打印出来的偏移量每次都不一样。

原因：锁定电压太小，转子被齿槽转矩和阻尼拉扯；更隐蔽的原因是极对数填错，电角度周期和机械角度周期对不上，转子找不到稳定的锁定点。

解决：把锁定电压从 3.0V 提到 4V 左右，等待时间加到 2 秒。如果还不行，用开环扫描确认极对数：让电角度缓慢走一个完整的 2π，观察转子转过的机械角，机械角等于 360° 除以极对数，实测和参数对不上就改宏。

5.2 位置闭环到位后持续小幅抖动

现象：电机到位后静音看编码器，数值在目标位置附近 ±1 计数来回跳，电机发出细碎的电流声。位置环 KP 调大后更明显，调小又停不到位。

原因：位置环 P 太强，编码器量化误差被放大成速度指令的抖动；速度环带宽不够，跟不上位置环给出的高频小幅指令。

解决：先给编码器读数加一阶低通，截止频率 100Hz 左右；再降位置环 KP，同时加一点 KD。我常用“到位死区”根治：误差绝对值小于 0.01rad 时，直接把目标速度置 0，让电机进入保持状态。注意死区太大会降低定位精度，0.01rad 在 4096 线编码器下大概是 0.3 度，视觉上感觉不到。

5.3 相电流波形毛刺多，电机明显发烫

现象：用示波器看采样电阻上的电流波形，满屏毛刺；电机空载跑 5 分钟就烫手，力矩输出却明显偏小。

原因：ADC 采样窗口和 PWM 边沿重叠了。三相桥上下管切换瞬间有剧烈的 dv/dt，采样点正好落在开关噪声上。另一个常见原因是采样电阻的放大倍数和 DengFOC 里的 CURRENT_SENSE_GAIN 不一致。

解决：中心对齐模式下，电流纹波在载波谷底最小，把 ADC 触发点对齐到 TIM1 计数器的谷底，而不是顶点。硬件上加一级 RC 低通，截止频率放在几十 kHz 级别，能滤掉大部分开关毛刺。最后用万用表测采样电阻两端电压，反推实际增益，把宏改成真实值。

5.4 闭环一启动电机就往一个方向冲

现象：使能位置闭环的瞬间，电机不是冲向目标位置，而是朝反方向加速，直到堵转；开环手动转却完全正常。

原因：编码器计数方向与电机正转方向不一致。位置环的误差被编码器方向“反着读”，正反馈直接飞车。这跟 PID 参数无关，是符号问题。

解决：开环跑一遍，确认电机正转时编码器计数值是递增还是递减。递减的话，在 DFOC_ReadEncoder() 里 return 0xFFFF - count，或者在 TIM3 编码器配置里交换 A/B 相。改完之后重新校准电角度偏移，因为编码器方向变了，零点也要重算。

5.5 PWM 频率和死区设置不合理导致发热和噪音

现象：PWM 频率设到 40kHz 后，驱动板 MOS 管烫得不敢摸；降到 8kHz，电机啸叫刺耳，电流波形也开始畸变。

原因：MOS 管开关损耗随频率近似线性上升，频率太高驱动板扛不住；频率太低时电流纹波变大，电机本体和采样电路都会出现音频噪声。

解决：云台电机和常见的低压驱动板，20kHz~30kHz 是比较稳的区间。死区时间用示波器看上下桥互补 PWM 的交叉点，有直通尖峰就加大，有平顶缺口就减小，一般 0.5us~2us 够用。改完频率记得同步改 DengFOC 的 PWM_FREQ_HZ 宏，因为内部计算电压矢量作用时间要用到这个值。

6. 二次开发：从单轴位置控制到多轴联动的改造路径

拿到一份能跑的 DengFOC 移植工程，别急着改代码，先按“电流环 → 速度环 → 位置环”的顺序做参数整定，这是我在这个项目上最想强调的一点。电流环用固定负载测试，Uq 给一个小阶跃，观察电流响应，PI 调到电流能跟上但不超调；速度环用手拨电机，看速度反馈是否平滑；最后才调位置环 KP。换电机、换供电电压都要重新走一遍，参数写在 Flash 里，上电读出来。

6.1 模块化改造：把“轴”封装成结构体

单轴调通后，要上多轴联动（比如机械臂），最直接的做法是把位置环状态量收进一个结构体，每个轴实例一份：

typedef struct { PID_t pos_pid; PID_t vel_pid; float zero_electric_angle; float target_pos; float current_pos; uint8_t axis_id; } FOC_Axis_t;

改造点在于：原来位置环、速度环里的 static 局部变量全部挪进结构体，函数签名改成 FOC_Axis_t 指针。多轴共用一套算法函数，只是数据区独立。这一步做完，后续加第三、第四个轴就是实例化结构体的事。

6.2 串口指令调参与上位机联调

位置控制从“写死目标角”到“外部给指令”，需要一个简单的串口协议。我一般用 ASCII 行协议，格式类似 set_angle 1.57 这种，用 strtok 解析，同时把实际位置和 PID 输出回传。调参不用重新编译下载，串口发 set_kp 12.5 就能在线改，省掉大量烧录时间。如果想再进一步，可以用 PC 端串口助手定时发目标角度，观察电机跟随曲线的滞后和超调，用来验证位置环带宽。

整定参数时的一个细节：位置环的 KP 和速度环的带宽是耦合的，位置环给的目标速度超过速度环响应能力，就会出现明显超调跟手现象。我后来的习惯是：每次换电机，都强制走一遍“开环旋转 → 电角度校准 → 电流环阶跃 → 速度环拨动 → 位置环阶跃”的流程，再进多轴联动，一步都不跳。这套流程替我省了无数个调参数的下午，希望帮到你。

本文还有配套的精品资源，点击获取