简单理解：SimpleFOC开环控制电机
·
1. 基础配置与流程文件（优先级最高）
user/main.c
- 系统初始化流程（GPIO、串口、定时器）
- 关键参数设置（电源电压、电压限制、极对数）
- 主循环逻辑（电机控制调用、串口命令处理）
- 开环控制的使用示例
user/MyProject.h
- 硬件引脚定义（电机使能引脚）
- 头文件引用关系
- 关键宏定义
2. 核心控制算法文件
SimpleFOC/BLDCMotor.c（开环控制核心）
move()：控制模式切换（速度 / 位置开环）velocityOpenloop()：开环速度控制算法angleOpenloop()：开环位置控制算法setPhaseVoltage()：SVPWM（空间矢量脉宽调制）实现
SimpleFOC/FOCMotor.c（FOC 控制基础）
- 电机状态变量定义（角度、速度、电压）
electricalAngle()：电气角度计算函数- 控制模式枚举定义
3. 工具函数文件
SimpleFOC/foc_utils.c（FOC 工具函数）
_normalizeAngle()：角度归一化（0-2π）_electricalAngle()：机械角度转电气角度- 三角函数近似实现（优化性能）
- 其他数学工具函数
4. 硬件驱动文件
Hardware/timer.c
TIM2_PWM_Init()：PWM 输出初始化（控制电机三相）TIM4_1ms_Init()：系统定时器初始化（时间基准）
Hardware/usart.c
uart_init()：串口初始化（115200 波特率）- 串口中断处理（接收命令）
- 数据发送函数
5. 辅助理解文件（为闭环控制做准备）
SimpleFOC/lowpass_filter.c
- 低通滤波器算法
- 滤波参数调整
SimpleFOC/pid.c
- PID 算法实现
- 控制器参数调整
main.c
#include "stm32f10x.h" // 引入STM32F10x系列芯片核心外设库（寄存器/函数定义）
#include <stdlib.h> // 引入标准库头文件（包含atof()字符串转浮点函数）
#include "MyProject.h" // 引入项目自定义头文件（含PWM、中断、电机控制宏/函数声明）
/************************************************
SimpleMotor开发板
开环速度控制和开环位置控制 演示
=================================================
本程序仅供学习，引用代码请标明出处
使用教程：https://blog.csdn.net/loop222/article/details/119220638
创建日期：20230208
作 者：loop222 @郑州
************************************************/
/******************************************************************************/
#define LED_blink GPIOC->ODR^=(1<<13) // 定义LED闪烁宏：直接操作寄存器翻转PC13电平（比库函数更快）
/******************************************************************************/
float target; // 全局浮点变量：电机目标值（速度控制=rad/s，位置控制=rad）
/******************************************************************************/
void commander_run(void); // 声明串口指令解析函数（先声明后调用，避免编译报错）
/******************************************************************************/
void GPIO_Config(void) // GPIO端口初始化配置函数（LED+电机使能引脚）
{
GPIO_InitTypeDef GPIO_InitStructure; // 定义GPIO初始化结构体（存放引脚配置参数）
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOC|RCC_APB2Periph_AFIO, ENABLE); // 使能GPIOA/B/C和AFIO时钟（AFIO用于引脚重映射）
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE); // 禁用JTAG/SWD调试接口，释放PB3/PB4/PB10等引脚
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13; // 选择PC13引脚（板载LED）
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; // 配置为通用推挽输出模式（输出高低电平）
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz; // 配置引脚输出速度为10MHz（LED无需高速）
GPIO_Init(GPIOC, &GPIO_InitStructure); // 根据结构体参数初始化GPIOC的13引脚
GPIO_ResetBits(GPIOC,GPIO_Pin_13); // 将PC13置低电平，上电点亮LED
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; // 选择PB9引脚（电机1的使能控制端）
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; // 配置为通用推挽输出模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz; // 配置引脚输出速度为10MHz
GPIO_Init(GPIOB, &GPIO_InitStructure); // 根据结构体参数初始化GPIOB的9引脚
GPIO_ResetBits(GPIOB,GPIO_Pin_9); // 将PB9置低电平，解除电机使能锁定（低电平有效）
}
/******************************************************************************/
//开环控制最重要的参数就是voltage_limit
//1、电机抖动转不起来把voltage_limit设置的大一点，
//2、电机发热严重的把voltage_limit设置的小一点，
//3、串口发送指令“T10”，后面要有回车换行符
//4、电机能转就表示一切正常，可以学习下一章了，开环不是电机控制的常态，不要纠结太久。
int main(void) // 主函数：程序入口，所有代码从这里开始执行
{
GPIO_Config(); // 调用GPIO初始化函数，配置LED和电机使能引脚
uart_init(115200); // 初始化串口1，波特率115200（用于指令接收和打印）
TIM2_PWM_Init(); // 初始化TIM2定时器的PWM输出（驱动电机的核心PWM信号）
TIM4_1ms_Init(); // 初始化TIM4定时器，配置为1ms触发一次中断（interrupt per 1ms）
delay_ms(1000); // 软件延时1000毫秒，等待系统和电机驱动电路稳定
voltage_power_supply=12; // 设置电源供电电压为12V（匹配实际硬件供电）
voltage_limit=2.5; // 设置电机驱动电压限制为2.5V（最大值<12/1.732=6.9V；大功率电机0.5-1V，小功率1-3V）
velocity_limit=20; // 设置电机最大速度限制为20rad/s（angleOpenloop()开环位置函数会用到）
controller=Type_velocity_openloop; // 设置控制器类型为“开环速度控制”（可改为Type_angle_openloop开环位置控制）
pole_pairs=7; // 设置电机极对数为7（必须匹配实际电机参数，否则转速计算错误）
M1_Enable; // 宏定义：使能电机1（需在MyProject.h中定义，如GPIO_SetBits(GPIOB,GPIO_Pin_9)）
printf("Motor ready.\r\n"); // 串口打印“Motor ready.”，提示电机初始化完成
systick_CountMode(); // 切换SysTick定时器为计数模式（切换后不能再调用delay_us()/delay_ms()）
target = 6.28; // 设置电机初始目标速度：6.28rad/s（≈1圈/秒，2π rad=1整圈）
while(1) // 死循环：电机主控制逻辑，持续运行
{
if(time1_cntr>=200) // 判断TIM4中断计数变量是否达到200（200*1ms=0.2秒）
{
time1_cntr=0; // 计数变量清零，重新开始计时
LED_blink; // 执行LED闪烁（翻转PC13电平，可视化程序运行状态）
}
move(target); // 调用电机运动函数：按target目标值驱动电机（开环控制核心）
commander_run(); // 调用串口指令解析函数：处理上位机发送的控制指令
}
}
/******************************************************************************/
void commander_run(void) // 串口指令解析函数：解析并执行上位机发送的指令
{
if((USART_RX_STA&0x8000)!=0) // 判断串口接收是否完成（0x8000是接收完成标志位，USART_RX_STA在usart.c中定义）
{
switch(USART_RX_BUF[0]) // 根据接收缓冲区第一个字符判断指令类型
{
case 'H': // 指令类型'H'：测试指令
printf("Hello World!\r\n"); // 串口打印“Hello World!”，验证串口通信正常
break; // 跳出switch分支
case 'T': // 指令类型'T'：设置电机目标值（如T6.28表示目标速度6.28rad/s）
target=atof((const char *)(USART_RX_BUF+1)); // 将接收缓冲区第2个字符开始的字符串转为浮点数，赋值给target
printf("RX=%.4f\r\n", target); // 串口打印接收到的目标值（保留4位小数）
break; // 跳出switch分支
}
USART_RX_STA=0; // 清零串口接收状态标志，准备接收下一条指令
}
}
/******************************************************************************/main.c总结
- 程序核心逻辑：初始化硬件→配置电机参数→开环驱动电机→通过串口动态修改目标值，同时用 LED 闪烁指示程序运行状态；
- 关键注释点：
- 定时器计数变量
time1_cntr的 200 对应 0.2 秒，是定时逻辑的核心； atof()函数实现串口指令的字符串到浮点数值的转换，是上位机控制的关键；systick_CountMode()切换后禁用延时函数，需注意主循环中无阻塞延时。
- 定时器计数变量
MyProject.h
#ifndef MYPROJECT_H // 防止头文件重复包含：检查MYPROJECT_H是否未定义
#define MYPROJECT_H // 定义MYPROJECT_H，标记头文件已包含
/* Includes ------------------------------------------------------------------*/
#include "stm32f10x_it.h" // 引入STM32中断服务函数头文件（含中断向量、中断声明）
#include "usart.h" // 引入串口初始化/操作头文件（含uart_init、printf等）
#include "delay.h" // 引入延时函数头文件（含delay_ms、delay_us等）
#include "timer.h" // 引入定时器初始化/操作头文件（含TIM2_PWM_Init、TIM4_1ms_Init等）
#include "foc_utils.h" // 引入FOC（磁场定向控制）工具函数头文件（含角度/速度计算、PI调节等）
#include "FOCMotor.h" // 引入FOC电机控制核心头文件（含FOC电机结构体、控制函数）
#include "BLDCmotor.h" // 引入无刷直流电机（BLDC）控制头文件（含开环/闭环驱动函数）
#define M1_Enable GPIO_SetBits(GPIOB,GPIO_Pin_9); // 定义电机1使能宏：将PB9置高电平（硬件上高电平使能电机）
#define M1_Disable GPIO_ResetBits(GPIOB,GPIO_Pin_9); // 定义电机1禁用宏：将PB9置低电平（硬件上低电平解除使能）
#endif // 结束#ifndef条件编译（与开头#ifndef配对）MyProject.h总结
- 核心功能：这份头文件是项目的 “总入口”，整合了硬件驱动（串口、定时器）、中断、FOC 电机控制的所有依赖，同时定义了电机使能的快捷宏；
- 关键规范：保护宏
MYPROJECT_H是头文件的必备写法，避免重复包含； - 工程意义：通过头文件统一管理依赖和宏定义，降低代码耦合度，方便后续维护（比如更换电机使能引脚时，仅需修改宏定义）。
BLDCMotor.c
#include "MyProject.h" // 引入项目总头文件（包含硬件驱动、宏定义、函数声明）
/************************************************
main中调用的接口函数都在当前文件中
=================================================
本程序仅供学习，引用代码请标明出处
使用教程：https://blog.csdn.net/loop222/article/details/119220638
创建日期：20210801
作 者：loop222 @郑州
************************************************/
/******************************************************************************/
float voltage_power_supply; // 全局变量：电源供电电压（V）
float voltage_limit; // 全局变量：电机驱动电压限制（V）
int pole_pairs; // 全局变量：电机极对数（需匹配实际电机）
unsigned long open_loop_timestamp; // 全局变量：开环控制时间戳（记录上次计算的SysTick值）
float velocity_limit; // 全局变量：电机速度限制（rad/s，开环位置控制用）
/******************************************************************************/
float velocityOpenloop(float target_velocity); // 声明开环速度控制函数
float angleOpenloop(float target_angle); // 声明开环位置控制函数
/******************************************************************************/
void move(float new_target) // 电机运动控制核心函数：根据控制器类型执行开环控制
{
switch(controller) // 根据控制器类型（速度/位置开环）分支执行
{
case Type_velocity_openloop: // 分支1：开环速度控制模式
// velocity control in open loop
shaft_velocity_sp = new_target; // 将目标值赋值给速度设定值（shaft_velocity_sp：轴速度设定值）
voltage.q = velocityOpenloop(shaft_velocity_sp); // 执行开环速度控制，返回q轴电压赋值给voltage.q
voltage.d = 0; // d轴电压设为0（开环控制仅用q轴）
break; // 跳出当前分支
case Type_angle_openloop: // 分支2：开环位置控制模式
// angle control in open loop
shaft_angle_sp = new_target; // 将目标值赋值给角度设定值（shaft_angle_sp：轴角度设定值）
voltage.q = angleOpenloop(shaft_angle_sp); // 执行开环位置控制，返回q轴电压赋值给voltage.q
voltage.d = 0; // d轴电压设为0（开环控制仅用q轴）
break; // 跳出当前分支
}
}
/******************************************************************************/
void setPhaseVoltage(float Uq, float Ud, float angle_el) // SVPWM核心函数：将d/q轴电压转换为三相PWM占空比
{
float Uout; // 局部变量：归一化后的输出电压幅值
uint32_t sector; // 局部变量：SVPWM扇区（1-6）
float T0,T1,T2; // 局部变量：SVPWM基本矢量作用时间
float Ta,Tb,Tc; // 局部变量：三相（A/B/C）PWM占空比（0~1）
if(Ud) // 若d轴电压非0（d/q轴均有电压）
{// _sqrt是sqrt的近似函数（误差3-4%）
Uout = _sqrt(Ud*Ud + Uq*Uq) / voltage_power_supply; // 计算归一化输出电压（除以电源电压，范围0~1）
// 电角度归一化到0~2π（仅使用_sin/_cos近似函数时需要）
angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud)); // 计算合成电压的电角度
}
else // 仅q轴有电压（开环控制场景）
{// 无需atan2和sqrt，简化计算
Uout = Uq / voltage_power_supply; // 归一化q轴电压（范围0~1）
// 电角度归一化到0~2π（仅使用_sin/_cos近似函数时需要）
angle_el = _normalizeAngle(angle_el + _PI_2); // 补偿π/2弧度（q轴超前d轴90°）
}
sector = (angle_el / _PI_3) + 1; // 计算当前电角度所在的SVPWM扇区（_PI_3=π/3=60°）
T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout; // 计算矢量T1的作用时间（_SQRT3=√3）
T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uout; // 计算矢量T2的作用时间
T0 = 1 - T1 - T2; // 计算零矢量作用时间（T0+T1+T2=1，归一化时间）
// 根据扇区计算三相PWM占空比（Ta/Tb/Tc对应A/B/C相）
switch(sector)
{
case 1: // 扇区1
Ta = T1 + T2 + T0/2;
Tb = T2 + T0/2;
Tc = T0/2;
break;
case 2: // 扇区2
Ta = T1 + T0/2;
Tb = T1 + T2 + T0/2;
Tc = T0/2;
break;
case 3: // 扇区3
Ta = T0/2;
Tb = T1 + T2 + T0/2;
Tc = T2 + T0/2;
break;
case 4: // 扇区4
Ta = T0/2;
Tb = T1+ T0/2;
Tc = T1 + T2 + T0/2;
break;
case 5: // 扇区5
Ta = T2 + T0/2;
Tb = T0/2;
Tc = T1 + T2 + T0/2;
break;
case 6: // 扇区6
Ta = T1 + T2 + T0/2;
Tb = T0/2;
Tc = T1 + T0/2;
break;
default: // 异常扇区（容错处理）
Ta = 0;
Tb = 0;
Tc = 0;
}
// 设置TIM2的3路PWM比较值（占空比×PWM周期），驱动电机三相
TIM_SetCompare1(TIM2,Ta*PWM_Period); // 设置TIM2_CH1（A相）PWM占空比
TIM_SetCompare2(TIM2,Tb*PWM_Period); // 设置TIM2_CH2（B相）PWM占空比
TIM_SetCompare3(TIM2,Tc*PWM_Period); // 设置TIM2_CH3（C相）PWM占空比
}
/******************************************************************************/
float velocityOpenloop(float target_velocity) // 开环速度控制函数：根据目标速度计算q轴电压并驱动电机
{
unsigned long now_us; // 局部变量：当前SysTick计数值（近似微秒）
float Ts,Uq; // Ts：两次调用的时间间隔（s）；Uq：输出q轴电压
now_us = SysTick->VAL; // 读取SysTick当前计数值（替代_micros()函数）
// 计算时间间隔Ts（SysTick为24位递减计数器，9MHz时钟→每计数1≈1/9μs）
if(now_us<open_loop_timestamp) // 若SysTick计数值溢出（递减到0后重新计数）
Ts = (float)(open_loop_timestamp - now_us)/9*1e-6; // 未溢出：直接计算差值，转换为秒
else // 溢出：计算循环差值
Ts = (float)(0xFFFFFF - now_us + open_loop_timestamp)/9*1e-6;
open_loop_timestamp=now_us; // 保存当前时间戳，供下次计算
// 容错处理：Ts为0或过大时，设为1ms（避免计算异常）
if(Ts == 0 || Ts > 0.5) Ts = 1e-3;
// 根据目标速度和时间间隔，计算当前轴角度（积分：角度=速度×时间）
shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts);
Uq = voltage_limit; // q轴电压设为电压限制值（开环控制直接用最大允许电压）
// 设置q轴电压和电角度，驱动电机（d轴=0，电角度=机械角度×极对数）
setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs));
return Uq; // 返回输出的q轴电压
}
/******************************************************************************/
float angleOpenloop(float target_angle) // 开环位置控制函数：根据目标角度计算q轴电压并驱动电机
{
unsigned long now_us; // 局部变量：当前SysTick计数值（近似微秒）
float Ts,Uq; // Ts：两次调用的时间间隔（s）；Uq：输出q轴电压
now_us = SysTick->VAL; // 读取SysTick当前计数值（替代_micros()函数）
// 计算时间间隔Ts（SysTick为24位递减计数器，9MHz时钟→每计数1≈1/9μs）
if(now_us<open_loop_timestamp) // 若SysTick计数值溢出
Ts = (float)(open_loop_timestamp - now_us)/9*1e-6; // 未溢出：直接计算差值
else // 溢出：计算循环差值
Ts = (float)(0xFFFFFF - now_us + open_loop_timestamp)/9*1e-6;
open_loop_timestamp = now_us; // 保存当前时间戳，供下次计算
// 容错处理：Ts为0或过大时，设为1ms
if(Ts == 0 || Ts > 0.5) Ts = 1e-3;
// 计算当前角度到目标角度的步进值（限制最大速度为velocity_limit）
if(fabs( target_angle - shaft_angle ) > velocity_limit*Ts) // 若剩余角度大于单次最大步进
{// 按最大速度步进（_sign返回符号，控制正/反转）
shaft_angle += _sign(target_angle - shaft_angle) * velocity_limit * Ts;
//shaft_velocity = velocity_limit; // 可选：记录当前速度
}
else // 剩余角度小于单次最大步进，直接到达目标角度
{
shaft_angle = target_angle;
//shaft_velocity = 0; // 可选：速度清零
}
Uq = voltage_limit; // q轴电压设为电压限制值（开环控制直接用最大允许电压）
// 设置q轴电压和电角度，驱动电机（d轴=0，电角度=机械角度×极对数）
setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs));
return Uq; // 返回输出的q轴电压
}
/******************************************************************************/BLDCMotor.c总结
- 核心功能：这份代码实现 SimpleFOC 的开环速度 / 位置控制，核心是
move()函数分发控制模式，velocityOpenloop()/angleOpenloop()计算目标角度，setPhaseVoltage()实现 SVPWM 驱动； - 关键注释点：
- SysTick 计数值用于计算时间间隔，是开环控制 “积分” 的核心；
- 电压归一化（除以电源电压）是 SVPWM 的标准操作，确保占空比在 0~1 范围内；
- 电角度 = 机械角度 × 极对数，是 FOC 控制的基础转换关系；
- 工程特点：开环控制无反馈，仅靠电压和时间计算角度，优点是简单，缺点是精度低、易丢步，适合电机调试阶段使用。
BLDCMotor.h
#ifndef BLDCMotor_H // 头文件保护宏：检查BLDCMotor_H是否未定义，防止重复包含
#define BLDCMotor_H // 定义BLDCMotor_H宏，标记该头文件已被包含
/******************************************************************************/
/**
* Direction structure // 注释：电机旋转方向枚举结构体
*/
typedef enum // 定义枚举类型：电机旋转方向（顺时针/逆时针/未知）
{
CW = 1, //clockwise // 枚举值：CW=1，代表顺时针旋转（Clockwise）
CCW = -1, // counter clockwise // 枚举值：CCW=-1，代表逆时针旋转（Counter Clockwise）
UNKNOWN = 0 //not yet known or invalid state // 枚举值：UNKNOWN=0，代表方向未知/无效状态
} Direction; // 枚举类型名：Direction（旋转方向类型）
/******************************************************************************/
extern long sensor_direction; // 声明全局变量：传感器检测到的电机旋转方向（取值为CW/CCW/UNKNOWN）
extern float voltage_power_supply; // 声明全局变量：电机电源供电电压（单位V，匹配实际硬件供电）
extern float voltage_limit; // 声明全局变量：电机驱动电压限制（单位V，防止过压烧毁电机）
extern float voltage_sensor_align; // 声明全局变量：传感器校准电压（单位V，电机电气零点校准时使用）
extern int pole_pairs; // 声明全局变量：电机极对数（需匹配实际电机参数，如7对极）
extern unsigned long open_loop_timestamp; // 声明全局变量：开环控制时间戳（记录SysTick值，计算时间间隔）
extern float velocity_limit; // 声明全局变量：电机速度限制（单位rad/s，开环位置控制时限制最大转速）
extern float current_limit; // 声明全局变量：电机电流限制（单位A，防止过流烧毁驱动板/电机）
/******************************************************************************/
void Motor_init(void); // 声明函数：电机初始化（硬件初始化、参数配置等）
void Motor_initFOC(void); // 声明函数：FOC算法初始化（校准零电角度、传感器零点等）
void loopFOC(void); // 声明函数：FOC主循环（计算电角度、SVPWM输出等核心逻辑）
void move(float new_target); // 声明函数：电机运动控制（根据目标值执行开环/闭环控制）
void setPhaseVoltage(float Uq, float Ud, float angle_el); // 声明函数：SVPWM核心（d/q轴电压转三相PWM占空比）
//void setPwm(float Ua, float Ub, float Uc); // 注释掉的函数：直接设置三相PWM占空比（备用接口）
/******************************************************************************/
#endif // 结束头文件保护宏（与开头#ifndef配对）
BLDCMotor.h总结
- 核心功能：这份头文件定义了 BLDC 电机 FOC 控制的方向枚举、保护参数、核心控制函数，是整个无刷电机控制程序的 “功能清单”；
- 关键注释点：
- 头文件保护宏避免重复包含，是 C 语言模块化编程的规范；
Direction枚举用 1/-1 表示正 / 反转，算法中可直接通过乘法补偿方向（如shaft_angle *= sensor_direction）；- 所有
extern变量需在BLDCMotor.c中定义，函数需在BLDCMotor.c中实现；
- 工程意义：通过头文件统一管理电机的保护参数（电压 / 电流限制）和核心函数，后续修改电机参数、替换控制算法时，只需改动.c 文件的实现，无需修改调用处。
FOCMotor.c
#include "MyProject.h" // 引入项目总头文件（包含硬件驱动、宏定义、函数声明）
/******************************************************************************/
float shaft_angle;//!< current motor angle // 全局变量：电机轴机械角度（单位rad，当前实际角度）
float electrical_angle; // 全局变量：电机电角度（单位rad，=机械角度×极对数-零电角度）
float shaft_velocity; // 全局变量：电机轴机械角速度（单位rad/s，当前实际转速）
float current_sp; // 全局变量：电流设定值（单位A，转矩控制模式下使用）
float shaft_velocity_sp; // 全局变量：电机轴速度设定值（单位rad/s，速度控制模式目标值）
float shaft_angle_sp; // 全局变量：电机轴角度设定值（单位rad，位置控制模式目标值）
DQVoltage_s voltage; // 全局结构体变量：d/q轴电压（包含voltage.d和voltage.q两个成员）
DQCurrent_s current; // 全局结构体变量：d/q轴电流（包含current.d和current.q两个成员）
TorqueControlType torque_controller; // 全局变量：转矩控制类型（如TORQUE_CONTROL_VOLTAGE电压控制、TORQUE_CONTROL_CURRENT电流控制）
MotionControlType controller; // 全局变量：运动控制类型（如Type_velocity_openloop开环速度、Type_angle_openloop开环位置）
float sensor_offset=0; // 全局变量：传感器角度偏移量（单位rad，校准传感器零点用）
float zero_electric_angle; // 全局变量：电机零电角度（单位rad，校准电机电气零点用）
/******************************************************************************/
float electricalAngle(void) // 电角度计算核心函数：将机械角度转换为校准后的电角度
{
// 返回归一化后的电角度：(机械角度+传感器偏移)×极对数 - 零电角度，结果限制在0~2π
return _normalizeAngle((shaft_angle + sensor_offset) * pole_pairs - zero_electric_angle);
}
/******************************************************************************/-
电角度计算的核心逻辑：公式：
电角度 = (机械角度 + 传感器偏移) × 极对数 - 零电角度- 第一步：
shaft_angle + sensor_offset→ 校准后的机械角度； - 第二步：×
pole_pairs→ 未校准的电角度（机械角度 × 极对数 = 电角度）； - 第三步：-
zero_electric_angle→ 校准后的最终电角度； - 最后：
_normalizeAngle()→ 归一化到 0~2π，保证角度范围有效。
- 第一步：
FOCMotor.c总结
- 核心功能：这份代码定义了 SimpleFOC 控制中最核心的状态变量（角度、速度、电压 / 电流）和校准参数，以及机械角度转电角度的关键函数；
- 关键注释点：
- 机械角度与电角度的转换关系是 FOC 的基础（电角度 = 机械角度 × 极对数）；
- 校准参数（
sensor_offset/zero_electric_angle）是保证 FOC 控制精度的关键； - 不同控制模式（速度 / 位置 / 转矩）对应不同的设定值变量（
shaft_velocity_sp/shaft_angle_sp/current_sp）；
- 工程意义：这些变量是连接 “用户设定值” 和 “电机实际状态” 的桥梁，后续的闭环控制、PWM 输出都依赖这些变量的更新。
FOCMotor.h
#ifndef FOCMOTOR_H // 头文件保护宏：检查FOCMOTOR_H是否未定义，防止重复包含
#define FOCMOTOR_H // 定义FOCMOTOR_H宏，标记该头文件已被包含
#include "foc_utils.h" // 引入FOC工具函数头文件（含角度归一化、三角函数近似、电角度转换等核心工具）
/******************************************************************************/
/**
* Motiron control type // 注：原注释拼写错误（Motiron→Motion），运动控制模式
*/
typedef enum // 定义枚举类型：电机运动控制模式（FOC核心控制模式选择）
{
Type_torque,//!< Torque control // 枚举值：转矩控制模式（直接控制电机输出转矩）
Type_velocity,//!< Velocity motion control // 枚举值：速度闭环控制（精准控制电机转速）
Type_angle,//!< Position/angle motion control // 枚举值：角度/位置闭环控制（精准控制电机轴位置）
Type_velocity_openloop, // 枚举值：速度开环控制（无反馈，仅算法推算转速，调试用）
Type_angle_openloop // 枚举值：角度开环控制（无反馈，仅算法推算位置，调试用）
} MotionControlType; // 枚举类型名：MotionControlType（运动控制模式类型）
/**
* Motiron control type // 注：原注释拼写错误（Motiron→Motion），实际为转矩控制类型
*/
typedef enum // 定义枚举类型：电机转矩控制类型（转矩的不同实现方式）
{
Type_voltage, //!< Torque control using voltage // 枚举值：电压型转矩控制（开环常用，直接输出电压）
Type_dc_current, //!< Torque control using DC current (one current magnitude) // 枚举值：直流电流型转矩控制（控制单路电流幅值）
Type_foc_current //!< torque control using dq currents // 枚举值：FOC电流型转矩控制（闭环常用，控制d/q轴电流）
} TorqueControlType; // 枚举类型名：TorqueControlType（转矩控制类型）
extern TorqueControlType torque_controller; // 声明全局变量：转矩控制类型（在.c文件中定义，跨文件使用）
extern MotionControlType controller; // 声明全局变量：运动控制模式（在.c文件中定义，跨文件使用）
/******************************************************************************/
extern float shaft_angle;//!< current motor angle // 声明全局变量：电机轴机械角度（rad，当前实际角度）
extern float electrical_angle; // 声明全局变量：电机电角度（rad，=机械角度×极对数，FOC核心参数）
extern float shaft_velocity; // 声明全局变量：电机轴机械角速度（rad/s，当前实际转速）
extern float current_sp; // 声明全局变量：电流设定值（A，转矩控制模式下的目标电流）
extern float shaft_velocity_sp; // 声明全局变量：电机轴速度设定值（rad/s，速度控制的目标转速）
extern float shaft_angle_sp; // 声明全局变量：电机轴角度设定值（rad，位置控制的目标角度）
extern DQVoltage_s voltage; // 声明全局结构体变量：d/q轴电压（含d/q轴电压值，FOC核心）
extern DQCurrent_s current; // 声明全局结构体变量：d/q轴电流（含d/q轴电流值，闭环控制用）
extern float sensor_offset; // 声明全局变量：传感器角度偏移量（rad，校准传感器零点）
extern float zero_electric_angle; // 声明全局变量：电机零电角度（rad，校准电机电气零点）
/******************************************************************************/
float shaftAngle(void); // 声明函数：读取电机轴机械角度（返回值：rad）
float shaftVelocity(void); // 声明函数：读取电机轴机械角速度（返回值：rad/s）
float electricalAngle(void); // 声明函数：计算/读取电机电角度（返回值：rad）
/******************************************************************************/
#endif // 结束头文件保护宏（与开头#ifndef配对）FOCMotor.h总结
- 核心功能：这份头文件是 FOC 电机控制的 “接口层”，定义了所有核心控制模式、状态变量和函数接口，是整个 SimpleFOC 程序的核心头文件；
- 关键注释点：
- 头文件保护宏是 C 语言规范写法，避免重复包含导致编译错误；
- 枚举类型区分了 “运动控制模式” 和 “转矩控制类型”，是选择电机控制逻辑的核心；
extern声明的变量 / 函数实现了跨文件复用，是模块化编程的基础；
- 工程意义：通过头文件统一管理 FOC 控制的核心接口，后续修改控制模式、参数时，只需修改.c 文件的实现，无需改动调用处。
foc_utils.c
#include "MyProject.h" // 包含项目头文件
/***************************************************************************/
// 使用整型数组替代浮点数组以节省空间
// 每360度采样4x200个点（每90度200个点）
// 节省一半存储空间（int占2字节，float占4字节）
// 正弦值放大10000倍存储
const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,1024,1103,1181,1260,1338,1416,1494,1572,1650,1728,1806,1883,1961,2038,2115,2192,2269,2346,2423,2499,2575,2652,2728,2804,2879,2955,3030,3105,3180,3255,3329,3404,3478,3552,3625,3699,3772,3845,3918,3990,4063,4135,4206,4278,4349,4420,4491,4561,4631,4701,4770,4840,4909,4977,5046,5113,5181,5249,5316,5382,5449,5515,5580,5646,5711,5775,5839,5903,5967,6030,6093,6155,6217,6279,6340,6401,6461,6521,6581,6640,6699,6758,6815,6873,6930,6987,7043,7099,7154,7209,7264,7318,7371,7424,7477,7529,7581,7632,7683,7733,7783,7832,7881,7930,7977,8025,8072,8118,8164,8209,8254,8298,8342,8385,8428,8470,8512,8553,8594,8634,8673,8712,8751,8789,8826,8863,8899,8935,8970,9005,9039,9072,9105,9138,9169,9201,9231,9261,9291,9320,9348,9376,9403,9429,9455,9481,9506,9530,9554,9577,9599,9621,9642,9663,9683,9702,9721,9739,9757,9774,9790,9806,9821,9836,9850,9863,9876,9888,9899,9910,9920,9930,9939,9947,9955,9962,9969,9975,9980,9985,9989,9992,9995,9997,9999,10000,10000};
/***************************************************************************/
// 使用固定大小数组近似计算正弦函数
// 执行时间：~40us（浮点数组） / ~50us（整型数组）
// 精度：±0.005
// 输入角度需在[0, 2PI]范围内
float _sin(float a){
if(a < _PI_2){ // 第一象限：[0, π/2)
//return sine_array[(int)(199.0*( a / (_PI/2.0)))]; // 原版映射计算
//return sine_array[(int)(126.6873* a)]; // 浮点数组优化版
return 0.0001*sine_array[_round(126.6873* a)]; // 整型数组优化版（缩放10000倍后需还原）
}else if(a < _PI){ // 第二象限：[π/2, π)
// return sine_array[(int)(199.0*(1.0 - (a-_PI/2.0) / (_PI/2.0)))]; // 原版
//return sine_array[398 - (int)(126.6873*a)]; // 浮点数组优化版
return 0.0001*sine_array[398 - _round(126.6873*a)]; // 整型数组优化版（对称映射）
}else if(a < _3PI_2){ // 第三象限：[π, 3π/2)
// return -sine_array[(int)(199.0*((a - _PI) / (_PI/2.0)))]; // 原版
//return -sine_array[-398 + (int)(126.6873*a)]; // 浮点数组优化版
return -0.0001*sine_array[-398 + _round(126.6873*a)]; // 整型数组优化版（负值对称映射）
} else { // 第四象限：[3π/2, 2π)
// return -sine_array[(int)(199.0*(1.0 - (a - 3*_PI/2) / (_PI/2.0)))]; // 原版
//return -sine_array[796 - (int)(126.6873*a)]; // 浮点数组优化版
return -0.0001*sine_array[796 - _round(126.6873*a)]; // 整型数组优化版（负值对称映射）
}
}
/***************************************************************************/
// 使用固定大小数组近似计算余弦函数
// 执行时间：~55us（浮点数组） / ~56us（整型数组）
// 精度：±0.005
// 输入角度需在[0, 2PI]范围内
float _cos(float a){
float a_sin = a + _PI_2; // 利用cos(a) = sin(a + π/2)转换
a_sin = a_sin > _2PI ? a_sin - _2PI : a_sin; // 角度归一化到[0, 2π)
return _sin(a_sin); // 调用正弦函数
}
/***************************************************************************/
// 将弧度角归一化到[0, 2PI]范围内
float _normalizeAngle(float angle){
float a = fmod(angle, _2PI); // 取模运算
return a >= 0 ? a : (a + _2PI); // 处理负角度
}
/***************************************************************************/
// 计算电机电角度
float _electricalAngle(float shaft_angle, int pole_pairs){
return (shaft_angle * pole_pairs); // 电角度 = 机械角度 × 极对数
}
/***************************************************************************/
// 平方根近似计算函数（使用快速平方根倒数算法）
// 参考：https://reprap.org/forum/read.php?147,219210
// 参考：https://en.wikipedia.org/wiki/Fast_inverse_square_root
float _sqrtApprox(float number){ // 低精度快速计算
long i;
float y;
// float x;
// const float f = 1.5F; // 更高精度版本参数
// x = number * 0.5F; // 更高精度版本计算
y = number; // 初始猜测值
i = * ( long * ) &y; // 将浮点数按位解释为整数
i = 0x5f375a86 - ( i >> 1 ); // 魔法数计算（牛顿迭代初始值）
y = * ( float * ) &i; // 将整数按位解释回浮点数
// y = y * ( f - ( x * y * y ) ); // 更高精度版本迭代计算
return number * y; // 返回平方根近似值（√x = x * 1/√x）
}
/***************************************************************************/foc_utils.c总结
核心优化策略
-
查表法计算三角函数：使用200个点的整型数组存储放大10000倍的正弦值，节省50%存储空间（int替代float）
-
分段映射优化：将角度按象限划分，通过对称性和线性映射实现快速查表
-
快速平方根算法：基于著名的"Fast Inverse Square Root"算法进行近似计算
性能指标
-
正弦计算：~50μs，精度±0.005
-
余弦计算：~56μs（通过正弦函数转换）
-
平方根计算：使用位运算和牛顿迭代法快速近似
功能模块
-
正弦/余弦函数：支持0-2π范围内的角度输入，四象限正确处理
-
角度归一化：将任意弧度角规范到[0,2π]区间
-
电角度计算：用于电机控制（电角度=机械角度×极对数）
-
平方根近似：牺牲精度换取计算速度，适用于实时系统
应用场景
特别适用于资源受限的嵌入式系统（如电机驱动、机器人控制），在保证计算精度的前提下显著提升运算效率。
foc_utils.h
#ifndef FOCUTILS_LIB_H // 防止头文件重复包含
#define FOCUTILS_LIB_H
#include <math.h> // 包含数学函数库
/******************************************************************************/
// 常用工具宏定义
// 符号函数：返回数值的符号（-1, 0, 1）
#define _sign(a) ( ( (a) < 0 ) ? -1 : ( (a) > 0 ) )
// 四舍五入函数：将浮点数转换为最接近的整数
#define _round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
// 数值约束函数：限制数值在指定范围内
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
// 平方根函数别名：指向近似计算函数
#define _sqrt(a) (_sqrtApprox(a))
// 判断变量是否已设置
#define _isset(a) ( (a) != (NOT_SET) )
// 常用数学常量定义
#define _2_SQRT3 1.15470053838 // 2/√3
#define _SQRT3 1.73205080757 // √3
#define _1_SQRT3 0.57735026919 // 1/√3
#define _SQRT3_2 0.86602540378 // √3/2
#define _SQRT2 1.41421356237 // √2
#define _120_D2R 2.09439510239 // 120度转弧度
#define _PI 3.14159265359 // π
#define _PI_2 1.57079632679 // π/2
#define _PI_3 1.0471975512 // π/3
#define _2PI 6.28318530718 // 2π
#define _3PI_2 4.71238898038 // 3π/2
#define _PI_6 0.52359877559 // π/6
/******************************************************************************/
// 数据结构定义
// dq轴电流结构体（用于电机FOC控制）
typedef struct
{
float d; // 直轴电流
float q; // 交轴电流
} DQCurrent_s;
// 三相电流结构体
typedef struct
{
float a; // A相电流
float b; // B相电流
float c; // C相电流
} PhaseCurrent_s;
// dq轴电压结构体
typedef struct
{
float d; // 直轴电压
float q; // 交轴电压
} DQVoltage_s;
/******************************************************************************/
// 函数声明
float _sin(float a); // 正弦近似计算
float _cos(float a); // 余弦近似计算
float _normalizeAngle(float angle); // 角度归一化
float _electricalAngle(float shaft_angle, int pole_pairs); // 电角度计算
float _sqrtApprox(float number); // 平方根近似计算
/******************************************************************************/
#endif // 结束头文件保护foc_utils.h总结
这是电机FOC控制的核心工具库，包含：
-
优化版数学函数（查表法三角函数、快速平方根）
-
FOC专用数据结构（dq轴电流/电压、三相电流）
-
常用工具宏（符号、约束、四舍五入）
-
电机控制专用常量（√3、π等）
专为嵌入式电机驱动器设计，在保证精度的同时大幅提升计算效率。
timer.c
#include "stm32f10x.h" // STM32F10x系列微控制器头文件
#include "timer.h" // 自定义定时器头文件
/***************************************************************************/
void TIM2_PWM_Init(void) // 初始化TIM2为3路PWM输出
{
GPIO_InitTypeDef GPIO_InitStructure; // GPIO初始化结构体
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure; // 定时器时基初始化结构体
TIM_OCInitTypeDef TIM_OCInitStructure; // 定时器输出比较初始化结构体
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); // 使能GPIOA时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); // 使能TIM2时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2; // 配置PA0、PA1、PA2引脚
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; // 复用推挽输出模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; // 50MHz输出速度
GPIO_Init(GPIOA, &GPIO_InitStructure); // 初始化GPIOA
TIM_DeInit(TIM2); // 复位TIM2定时器
TIM_TimeBaseInitStructure.TIM_Prescaler = 0; // 预分频器为0（不分频）
TIM_TimeBaseInitStructure.TIM_Period = 1440 - 1; // 自动重装载值，设置PWM频率为25kHz
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_CenterAligned1; // 中央对齐模式1
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1; // 时钟分频为1
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure); // 初始化TIM2时基
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; // PWM模式1
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; // 使能输出
TIM_OCInitStructure.TIM_Pulse = 0; // 初始占空比为0
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; // 输出极性为高电平有效
TIM_OC1Init(TIM2, &TIM_OCInitStructure); // 初始化通道1
TIM_OC2Init(TIM2, &TIM_OCInitStructure); // 初始化通道2
TIM_OC3Init(TIM2, &TIM_OCInitStructure); // 初始化通道3
TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable); // 使能通道1预装载
TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable); // 使能通道2预装载
TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable); // 使能通道3预装载
TIM_ARRPreloadConfig(TIM2, ENABLE); // 使能自动重装载预装载
TIM_Cmd(TIM2, ENABLE); // 使能TIM2定时器
}
/***************************************************************************/
void TIM4_1ms_Init(void) // 初始化TIM4为1ms定时中断
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure; // 定时器时基初始化结构体
NVIC_InitTypeDef NVIC_InitStructure; // NVIC中断初始化结构体
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); // 使能TIM4时钟
NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn; // TIM4中断通道
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; // 使能中断
NVIC_Init(&NVIC_InitStructure); // 初始化NVIC
TIM_TimeBaseInitStructure.TIM_Period = 1000 - 1; // 自动重装载值，设置1ms中断
TIM_TimeBaseInitStructure.TIM_Prescaler = 72 - 1; // 72分频，时钟频率为1MHz
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up; // 向上计数模式
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1; // 时钟分频为1
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseInitStructure); // 初始化TIM4时基
TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); // 使能更新中断
TIM_Cmd(TIM4, ENABLE); // 使能TIM4定时器
}
/***************************************************************************/
/***************************************************************************/timer.c总结
这是STM32F10x的定时器配置代码，包含两个功能：
-
TIM2：配置为3路25kHz中心对齐PWM输出（PA0/PA1/PA2），用于电机驱动
-
TIM4：配置为1ms定时中断，用于系统定时任务
采用中央对齐PWM模式，适合电机FOC控制，可减少谐波干扰。
timer.h
#ifndef STM32_TIMER_H // 防止头文件重复包含
#define STM32_TIMER_H
/******************************************************************************/
#define PWM_Period 1440 // 定义PWM周期值（对应TIM2的ARR寄存器）
/******************************************************************************/
void TIM2_PWM_Init(void); // 声明TIM2 PWM初始化函数
void TIM4_1ms_Init(void); // 声明TIM4 1ms定时中断初始化函数
/******************************************************************************/
#endif // 结束头文件保护timer.h总结
这是STM32定时器模块的头文件，定义了PWM周期常量（1440）并声明了两个定时器初始化函数：TIM2用于PWM输出，TIM4用于1ms定时中断。
lowpass_filter.c
#include "MyProject.h" // 包含项目主头文件
/******************************************************************************/
#define DEF_CURR_FILTER_Tf 0.005 // 默认电流滤波器时间常数
#define DEF_VEL_FILTER_Tf 0.005 // 默认速度滤波器时间常数
/******************************************************************************/
//unsigned long lpf_vel_timestamp; // 被注释掉的时间戳变量
float y_vel_prev=0; // 速度低通滤波器上一次输出值
float y_current_q_prev=0; // Q轴电流低通滤波器上一次输出值
float y_current_d_prev=0; // D轴电流低通滤波器上一次输出值
/******************************************************************************/
/*
float LPF_velocity(float x) // 被注释掉的高精度速度低通滤波器
{
unsigned long now_us;
float Ts, alpha, y;
now_us = SysTick->VAL;
if(now_us<lpf_vel_timestamp)Ts = (float)(lpf_vel_timestamp - now_us)/9 * 1e-6f;
else
Ts = (float)(0xFFFFFF - now_us + lpf_vel_timestamp)/9 * 1e-6f;
lpf_vel_timestamp = now_us;
if(Ts == 0 || Ts > 0.5) Ts = 1e-3f;
alpha = DEF_VEL_FILTER_Tf/(DEF_VEL_FILTER_Tf + Ts);
y = alpha*y_prev + (1.0f - alpha)*x;
y_prev = y;
return y;
}
*/
float LPF_velocity(float x) // 简化版速度低通滤波器
{
float y = 0.9*y_vel_prev + 0.1*x; // 一阶低通滤波：y(n) = 0.9*y(n-1) + 0.1*x(n)
y_vel_prev = y; // 保存当前输出值供下次使用
return y;
}
/******************************************************************************/
float LPF_current_q(float x) // Q轴电流低通滤波器
{
float y = 0.9*y_current_q_prev + 0.1*x; // 一阶低通滤波公式
y_current_q_prev = y; // 保存当前输出值
return y;
}
/******************************************************************************/
float LPF_current_d(float x) // D轴电流低通滤波器
{
float y = 0.9*y_current_d_prev + 0.1*x; // 一阶低通滤波公式
y_current_d_prev = y; // 保存当前输出值
return y;
}
/******************************************************************************/lowpass_filter.c总结
这是电机控制系统的低通滤波器模块，包含三个简化的一阶低通滤波器：
-
速度滤波器：滤除电机转速信号中的高频噪声
-
Q轴电流滤波器：滤除交轴电流噪声
-
D轴电流滤波器：滤除直轴电流噪声
采用固定系数（0.9/0.1）的简化算法替代自适应时间常数计算，降低计算复杂度，适合嵌入式实时控制。
lowpass_filter.h
#ifndef LOWPASS_FILTER_H // 防止头文件重复包含
#define LOWPASS_FILTER_H
/******************************************************************************/
/******************************************************************************/
float LPF_velocity(float x); // 声明速度低通滤波器函数
float LPF_current_q(float x); // 声明Q轴电流低通滤波器函数
float LPF_current_d(float x); // 声明D轴电流低通滤波器函数
/******************************************************************************/
#endif // 结束头文件保护lowpass_filter.h总结
这是低通滤波器模块的头文件，声明了三个用于电机控制的滤波函数：
-
速度滤波器：平滑电机转速信号
-
Q轴电流滤波器：滤除交轴电流噪声
-
D轴电流滤波器：滤除直轴电流噪声
该模块为FOC控制提供信号调理功能，确保控制系统的稳定性。
pid.c
#include "MyProject.h" // 引入项目头文件，包含必要的宏定义、函数声明等
/
本程序仅供学习，未经作者许可，不得用于其它任何用途
PID参数初始化、角度PID、速度PID、电流PID
使用教程：https://blog.csdn.net/loop222/article/details/119220638
创建日期：20210801
/
/**/
float pid_vel_P, pid_ang_P, pid_cur_P; // 定义PID比例参数：速度P、角度P、电流P
float pid_vel_I, pid_ang_D, pid_cur_I; // 定义PID积分/微分参数：速度I、角度D、电流I
float integral_vel_prev, integral_cur_q_prev, integral_cur_d_prev; // 积分项历史值：速度、q轴电流、d轴电流
float error_vel_prev, error_ang_prev, error_cur_q_prev, error_cur_d_prev; // 误差历史值：速度、角度、q轴电流、d轴电流
float output_vel_ramp,output_cur_ramp; // 输出斜坡限制：速度输出变化率、电流输出变化率（限制输出加速度）
float output_vel_prev,output_cur_q_prev, output_cur_d_prev; // 输出历史值：速度、q轴电流、d轴电流
unsigned long pid_vel_timestamp, pid_ang_timestamp, pid_cur_q_timestamp, pid_cur_d_timestamp; // PID计算时间戳：记录各PID上次执行时间
/**/
void PID_init(void) // PID参数初始化函数
{
pid_vel_P=0.1; //0.2 // 速度PID比例系数初始化
pid_vel_I=2; //20 // 速度PID积分系数初始化
output_vel_ramp=100; // output derivative limit [volts/second] 速度输出变化率限制（V/s）
integral_vel_prev=0; // 速度积分项初始化为0
error_vel_prev=0; // 速度误差初始化为0
output_vel_prev=0; // 速度输出初始化为0
pid_vel_timestamp=SysTick->VAL; // 初始化速度PID时间戳为系统滴答定时器当前值
pid_ang_P=10; // 角度PID比例系数初始化
pid_ang_D=0.5; // 角度PID微分系数初始化
error_ang_prev=0; // 角度误差初始化为0
pid_ang_timestamp=SysTick->VAL; // 初始化角度PID时间戳为系统滴答定时器当前值
pid_cur_P=2; // 电流PID比例系数初始化
pid_cur_I=1; // 电流PID积分系数初始化
output_cur_ramp=10; // 电流输出变化率限制（V/s）
integral_cur_q_prev=0; // q轴电流积分项初始化为0
error_cur_q_prev=0; // q轴电流误差初始化为0
output_cur_q_prev=0; // q轴电流输出初始化为0
pid_cur_q_timestamp=SysTick->VAL; // 初始化q轴电流PID时间戳
integral_cur_d_prev=0; // d轴电流积分项初始化为0
error_cur_d_prev=0; // d轴电流误差初始化为0
output_cur_d_prev=0; // d轴电流输出初始化为0
pid_cur_d_timestamp=SysTick->VAL; // 初始化d轴电流PID时间戳
}
/**/
//just P&I is enough,no need D 仅需PI控制，无需D（微分）
float PID_velocity(float error) // 速度PID计算函数，输入：速度误差，输出：速度控制量
{
unsigned long now_us; // 定义当前时间变量
float Ts; // 定义采样周期（s）
float proportional,integral,output; // 定义比例项、积分项、最终输出
float output_rate; // 定义输出变化率
now_us = SysTick->VAL; // 获取系统滴答定时器当前值
// 计算采样周期Ts（微秒转秒，SysTick时钟频率9MHz，故除以9）
if(now_us<pid_vel_timestamp)Ts = (float)(pid_vel_timestamp - now_us)/9*1e-6f; // 定时器未溢出
else
Ts = (float)(0xFFFFFF - now_us + pid_vel_timestamp)/9*1e-6f; // 定时器溢出时的计算
pid_vel_timestamp = now_us; // 更新速度PID时间戳为当前值
if(Ts == 0 || Ts > 0.5) Ts = 1e-3f; // 异常值处理：Ts为0或过大时，默认1ms
// u(s) = (P + I/s + Ds)e(s) PID连续域公式
// Discrete implementations 离散化实现
// proportional part 比例项计算
// u_p = P *e(k)
proportional = pid_vel_P * error; // 比例项 = 比例系数 * 当前误差
// Tustin transform of the integral part 积分项的双线性变换（Tustin）离散化
// u_ik = u_ik_1 + I*Ts/2*(ek + ek_1)
integral = integral_vel_prev + pid_vel_I*Ts*0.5*(error + error_vel_prev); // 积分项计算
// antiwindup - limit the output 积分抗饱和：限制积分项范围
integral = _constrain(integral, -voltage_limit, voltage_limit); // 积分项限幅到电压限制范围
// sum all the components 比例+积分得到输出
output = proportional + integral;
// antiwindup - limit the output variable 输出抗饱和：限制最终输出范围
output = _constrain(output, -voltage_limit, voltage_limit);
// limit the acceleration by ramping the output 输出斜坡限制：限制输出变化率（防止加速度过大）
output_rate = (output - output_vel_prev)/Ts; // 计算输出变化率
if(output_rate > output_vel_ramp)output = output_vel_prev + output_vel_ramp*Ts; // 超过上限则限幅
else if(output_rate < -output_vel_ramp)output = output_vel_prev - output_vel_ramp*Ts; // 低于下限则限幅
// saving for the next pass 保存当前值，供下一次计算使用
integral_vel_prev = integral; // 更新积分项历史值
output_vel_prev = output; // 更新输出历史值
error_vel_prev = error; // 更新误差历史值
return output; // 返回速度PID输出
}
/**/
//P&D for angle_PID 角度PID仅用PD控制
float PID_angle(float error) // 角度PID计算函数，输入：角度误差，输出：角度控制量
{
unsigned long now_us; // 定义当前时间变量
float Ts; // 定义采样周期（s）
float proportional,derivative,output; // 定义比例项、微分项、最终输出
//float output_rate; // 注释：未使用的输出变化率变量
now_us = SysTick->VAL; // 获取系统滴答定时器当前值
// 计算采样周期Ts（处理定时器溢出）
if(now_us<pid_ang_timestamp)Ts = (float)(pid_ang_timestamp - now_us)/9*1e-6f;
else
Ts = (float)(0xFFFFFF - now_us + pid_ang_timestamp)/9*1e-6f;
pid_ang_timestamp = now_us; // 更新角度PID时间戳
if(Ts == 0 || Ts > 0.5) Ts = 1e-3f; // 异常值处理，默认1ms
// u(s) = (P + I/s + Ds)e(s) PID连续域公式
// Discrete implementations 离散化实现
// proportional part 比例项计算
// u_p = P *e(k)
proportional = pid_ang_P * error; // 比例项 = 比例系数 * 当前误差
// u_dk = D(ek - ek_1)/Ts 微分项离散化公式
derivative = pid_ang_D*(error - error_ang_prev)/Ts; // 微分项 = 微分系数*(当前误差-历史误差)/采样周期
output = proportional + derivative; // 比例+微分得到输出
output = _constrain(output, -velocity_limit, velocity_limit); // 输出限幅到速度限制范围
// limit the acceleration by ramping the output 注释：未启用的输出斜坡限制
// output_rate = (output - output_ang_prev)/Ts;
// if(output_rate > output_ang_ramp)output = output_ang_prev + output_ang_ramp*Ts;
// else if(output_rate < -output_ang_ramp)output = output_ang_prev - output_ang_ramp*Ts;
// saving for the next pass 保存历史值
// output_ang_prev = output; // 注释：未使用的输出历史值更新
error_ang_prev = error; // 更新角度误差历史值
return output; // 返回角度PID输出
}
/**/
//P&I for current_PID 电流PID仅用PI控制（q轴）
float PID_current_q(float error) // q轴电流PID计算函数，输入：q轴电流误差，输出：q轴电流控制量
{
unsigned long now_us; // 定义当前时间变量
float Ts; // 定义采样周期（s）
float proportional,integral,output; // 定义比例项、积分项、最终输出
float output_rate; // 定义输出变化率
now_us = SysTick->VAL; // 获取系统滴答定时器当前值
// 计算采样周期Ts（处理溢出）
if(now_us<pid_cur_q_timestamp)Ts = (float)(pid_cur_q_timestamp - now_us)/9*1e-6f;
else
Ts = (float)(0xFFFFFF - now_us + pid_cur_q_timestamp)/9*1e-6f;
pid_cur_q_timestamp = now_us; // 更新q轴电流PID时间戳
if(Ts == 0 || Ts > 0.5) Ts = 1e-3f; // 异常值处理，默认1ms
// u(s) = (P + I/s + Ds)e(s) PID连续域公式
// Discrete implementations 离散化实现
// proportional part 比例项计算
// u_p = P *e(k)
proportional = pid_cur_P * error; // 比例项 = 比例系数 * 当前误差
// Tustin transform of the integral part 积分项双线性变换离散化
// u_ik = u_ik_1 + I*Ts/2*(ek + ek_1)
integral = integral_cur_q_prev + pid_cur_I*Ts*0.5*(error + error_cur_q_prev); // 积分项计算
// antiwindup - limit the output 积分抗饱和：限幅到电源电压范围
integral = _constrain(integral, -voltage_power_supply, voltage_power_supply);
// sum all the components 比例+积分得到输出
output = proportional + integral;
// antiwindup - limit the output variable 输出抗饱和：限幅到电压限制范围
output = _constrain(output, -voltage_limit, voltage_limit);
// limit the acceleration by ramping the output 输出斜坡限制：限制变化率
output_rate = (output - output_cur_q_prev)/Ts; // 计算输出变化率
if(output_rate > output_cur_ramp)output = output_cur_q_prev + output_cur_ramp*Ts; // 上限限幅
else if(output_rate < -output_cur_ramp)output = output_cur_q_prev - output_cur_ramp*Ts; // 下限限幅
// saving for the next pass 保存历史值
integral_cur_q_prev = integral; // 更新q轴积分项历史值
output_cur_q_prev = output; // 更新q轴输出历史值
error_cur_q_prev = error; // 更新q轴误差历史值
return output; // 返回q轴电流PID输出
}
/**/
//P&I for current_PID 电流PID仅用PI控制（d轴）
float PID_current_d(float error) // d轴电流PID计算函数，输入：d轴电流误差，输出：d轴电流控制量
{
unsigned long now_us; // 定义当前时间变量
float Ts; // 定义采样周期（s）
float proportional,integral,output; // 定义比例项、积分项、最终输出
float output_rate; // 定义输出变化率
now_us = SysTick->VAL; // 获取系统滴答定时器当前值
// 计算采样周期Ts（处理溢出）
if(now_us<pid_cur_d_timestamp)Ts = (float)(pid_cur_d_timestamp - now_us)/9*1e-6f;
else
Ts = (float)(0xFFFFFF - now_us + pid_cur_d_timestamp)/9*1e-6f;
pid_cur_d_timestamp = now_us; // 更新d轴电流PID时间戳
if(Ts == 0 || Ts > 0.5) Ts = 1e-3f; // 异常值处理，默认1ms
// u(s) = (P + I/s + Ds)e(s) PID连续域公式
// Discrete implementations 离散化实现
// proportional part 比例项计算
// u_p = P *e(k)
proportional = pid_cur_P * error; // 比例项 = 比例系数 * 当前误差
// Tustin transform of the integral part 积分项双线性变换离散化
// u_ik = u_ik_1 + I*Ts/2*(ek + ek_1)
integral = integral_cur_d_prev + pid_cur_I*Ts*0.5*(error + error_cur_d_prev); // 积分项计算
// antiwindup - limit the output 积分抗饱和：限幅到电源电压范围
integral = _constrain(integral, -voltage_power_supply, voltage_power_supply);
// sum all the components 比例+积分得到输出
output = proportional + integral;
// antiwindup - limit the output variable 输出抗饱和：限幅到电压限制范围
output = _constrain(output, -voltage_limit, voltage_limit);
// limit the acceleration by ramping the output 输出斜坡限制：限制变化率
output_rate = (output - output_cur_d_prev)/Ts; // 计算输出变化率
if(output_rate > output_cur_ramp)output = output_cur_d_prev + output_cur_ramp*Ts; // 上限限幅
else if(output_rate < -output_cur_ramp)output = output_cur_d_prev - output_cur_ramp*Ts; // 下限限幅
// saving for the next pass 保存历史值
integral_cur_d_prev = integral; // 更新d轴积分项历史值
output_cur_d_prev = output; // 更新d轴输出历史值
error_cur_d_prev = error; // 更新d轴误差历史值
return output; // 返回d轴电流PID输出
}
/**/pid.c总结
- 功能定位：这是一套用于电机控制的三环 PID（角度环、速度环、电流环） 实现代码，采用分层控制逻辑（角度环输出作为速度环输入，速度环输出作为电流环输入）。
- 算法特点：
- 角度环：仅用 PD 控制（无积分，避免静态误差但防止积分饱和），输出限幅到速度范围；
- 速度环 / 电流环（q/d 轴）：仅用 PI 控制（无微分，减少高频噪声），积分项采用 Tustin 双线性变换离散化，且做了积分抗饱和和输出斜坡限制（防止输出突变）；
- 采样周期：基于 SysTick 定时器动态计算，处理定时器溢出异常，默认采样周期 1ms。
- 工程细节：所有 PID 参数、历史值（误差、积分、输出）均为全局变量，初始化函数统一配置参数和初始值，各 PID 函数独立计算且包含异常值处理，保证控制稳定性。
pid.h
#ifndef PID_H // 头文件保护：检查PID_H宏是否未定义，防止重复包含
#define PID_H // 定义PID_H宏，标记头文件已包含
/******************************************************************************/
void PID_init(void); // PID控制器初始化函数：初始化各PID参数（比例/积分/微分、限幅等）
float PID_velocity(float error); // 速度环PID计算函数：输入速度误差，输出速度环调节值
float PID_angle(float error); // 角度环PID计算函数：输入角度误差，输出角度环调节值
float PID_current_q(float error); // q轴电流环PID计算函数：输入q轴电流误差，输出q轴电压调节值（控制转矩）
float PID_current_d(float error); // d轴电流环PID计算函数：输入d轴电流误差，输出d轴电压调节值（控制励磁，通常Id=0）
/******************************************************************************/
#endif // 头文件保护：结束#ifndef的条件编译块pid.h总结
- 头文件保护宏（
PID_H）的注释明确了其 “防止重复包含” 的核心作用； - 每个 PID 函数注释都清晰标注了所属控制环、输入输出含义，贴合 STM32+SimpleFOC 的实际应用场景；
- d/q 轴电流环注释特别区分了两者的控制目标（q 轴控转矩、d 轴控励磁），符合 FOC 的核心逻辑。
有“AI”的1024 = 2048，欢迎大家加入2048 AI社区
更多推荐
10
0- 0
已为社区贡献12条内容
所有评论(0)