直流无刷电机控制（FOC）：电流模式
目录
概述
1 系统框架结构
1.1 硬件模块介绍
1.2 硬件实物图
1.3 引脚接口定义
2 代码实现
2.1 软件架构
2.2 电流检测函数
3 电流环功能实现
3.1 代码实现
3.2 测试代码实现
4 测试
概述
本文主要介绍基于DengFOC的库函数，实现直流无刷电机控制（FOC）的电流环控制。笔者详细介绍了电流环采样电流时用到的函数。DengFOC的软件架构,电流环模式实现的原理等内容。
源代码下载地址：
dengfoctestdemo资源-CSDN文库
1 系统框架结构
1.1 硬件模块介绍
系统硬件框架结构功能如下：
电机模块： 2208直流无刷电机（数量：1）
编码器： 选用AS5600编码器，用于获取当前电机转动的角度，其通过I2C接口与MCU进行通信
驱动板（FOC）： 通过接收到MCU传送的PWM芯片，直接作用到MOS管上，以驱动直流无刷电机工作。
主控模块：使用ESP32 ( ESP32-WROOM-32E)
电流检测功能： 使用MCU ADC功能检测INA240的输出电压值
1.2 硬件实物图
1.3 引脚接口定义
1） 电机驱动板与电机之间的接口
2）编码器与MCU接口
3）电流检测
2 代码实现
2.1 软件架构
2.2 电流检测函数
1）源代码介绍
代码12行： 定义参考电压值
代码13行： ADC的最大值（12bit ）
代码16： 计算比率
代码25~26行： 定义ADC的端口
代码28行： 分流电阻
代码29行： 电流放大倍数
代码31行： 电流放大的比率
代码55~59行： 获取ADC的count值
代码61~66行： 配置ADC的端口模式
代码69~91行： 获取ADC的均值
代码93~99行: 读电流参数初始化
代码104~109行： 获取电流值
2）源代码文件
编写InlineCurrent.c文件的内容
#include <Arduino.h>
#include "InlineCurrent.h"
// - shunt_resistor - 分流电阻值
// - gain - 电流检测运算放大器增益
// - phA - A 相 adc 引脚
// - phB - B 相 adc 引脚
// - phC - C 相 adc 引脚（可选）
#define _ADC_VOLTAGE 3.3f //ADC 电压
#define _ADC_RESOLUTION 4095.0f //ADC 分辨率
// ADC 计数到电压转换比率求解
#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
#define NOT_SET -12345.0
#define _isset(a) ( (a) != (NOT_SET) )
CurrSense::CurrSense(int Mot_Num)
{
if(Mot_Num==0)
{
pinA = 39;
pinB = 36;
//int pinC;
_shunt_resistor = 0.01;
amp_gain = 50;
volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps
// gains for each phase
gain_a = volts_to_amps_ratio*-1;
gain_b = volts_to_amps_ratio*-1;
gain_c = volts_to_amps_ratio;
}
if(Mot_Num==1)
{
pinA = 35;
pinB = 34;
//int pinC;
_shunt_resistor = 0.01;
amp_gain = 50;
volts_to_amps_ratio = 1.0f /_shunt_resistor / amp_gain; // volts to amps
// gains for each phase
gain_a = volts_to_amps_ratio*-1;
gain_b = volts_to_amps_ratio*-1;
gain_c = volts_to_amps_ratio;
}
}
float CurrSense::readADCVoltageInline(const int pinA)
{
uint32_t raw_adc = analogRead(pinA);
return raw_adc * _ADC_CONV;
}
void CurrSense::configureADCInline(const int pinA,const int pinB, const int pinC)
{
pinMode(pinA, INPUT);
pinMode(pinB, INPUT);
if( _isset(pinC) )
pinMode(pinC, INPUT);
}
// 查找 ADC 零偏移量的函数
void CurrSense::calibrateOffsets()
{
const int calibration_rounds = 1000;
// 查找0电流时候的电压
offset_ia = 0;
offset_ib = 0;
offset_ic = 0;
// 读数1000次
for (int i = 0; i < calibration_rounds; i++)
{
offset_ia += readADCVoltageInline(pinA);
offset_ib += readADCVoltageInline(pinB);
if(_isset(pinC)) offset_ic += readADCVoltageInline(pinC);
delay(1);
}
// 求平均，得到误差
offset_ia = offset_ia / calibration_rounds;
offset_ib = offset_ib / calibration_rounds;
if(_isset(pinC))
offset_ic = offset_ic / calibration_rounds;
}
void CurrSense::init()
{
// 配置函数
configureADCInline(pinA,pinB,pinC);
// 校准
calibrateOffsets();
}
// 读取全部三相电流
void CurrSense::getPhaseCurrents()
{
current_a = (readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps
current_b = (readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps
current_c = (!_isset(pinC)) ? 0 : (readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps
}
编写InlineCurrent.h文件的内容
#include <Arduino.h>
class CurrSense
{
public:
CurrSense(int Mot_Num);
float readADCVoltageInline(const int pinA);
void configureADCInline(const int pinA,const int pinB, const int pinC);
void calibrateOffsets();
void init();
void getPhaseCurrents();
float current_a,current_b,current_c;
int pinA;
int pinB;
int pinC;
float offset_ia;
float offset_ib;
float offset_ic;
float _shunt_resistor;
float amp_gain;
float volts_to_amps_ratio;
float gain_a;
float gain_b;
float gain_c;
private:
int _Mot_Num;
};
3 电流环功能实现
3.1 代码实现
代码29行： 设置电流环的PID参数
代码36行： 创建电流环的Obj
代码57~63行： 设置电流环参数PID函数
代码271~278行： 获取电流数据函数
代码283~293行： 获取IQ电流值
代码295~300行： 电流滤波函数
dengFOC.c 源文件内容：
#include <Arduino.h>
#include "AS5600.h"
#include "lowpass_filter.h"
#include "pid.h"
#include "InlineCurrent.h" //引入在线电流检测
#define _1_SQRT3 0.57735026919f
#define _2_SQRT3 1.15470053838f
//低通滤波初始化
LowPassFilter M0_Vel_Flt = LowPassFilter(0.01); // Tf = 10ms //M0速度环
LowPassFilter M0_Curr_Flt = LowPassFilter(0.05); // Tf = 5ms //M0电流环
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
float voltage_power_supply;
float Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0;
#define _3PI_2 4.71238898038f
float zero_electric_angle=0;
int PP=1,DIR=1;
int pwmA = 32;
int pwmB = 33;
int pwmC = 25;
//PID
PIDController vel_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = voltage_power_supply/2};
PIDController angle_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = 100};
PIDController current_loop_M0 = PIDController{.P = 1.2, .I = 0, .D = 0, .ramp = 100000, .limit = 12.6};
//AS5600
Sensor_AS5600 S0=Sensor_AS5600(0);
TwoWire S0_I2C = TwoWire(0);
//初始化电流闭环
CurrSense CS_M0= CurrSense(0);
//=================PID 设置函数=================
//速度PID
void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp) //M0角度环PID设置
{
vel_loop_M0.P=P;
vel_loop_M0.I=I;
vel_loop_M0.D=D;
vel_loop_M0.output_ramp=ramp;
}
//角度PID
void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp) //M0角度环PID设置
{
angle_loop_M0.P=P;
angle_loop_M0.I=I;
angle_loop_M0.D=D;
angle_loop_M0.output_ramp=ramp;
}
void DFOC_M0_SET_CURRENT_PID(float P,float I,float D,float ramp) //M0电流环PID设置
{
current_loop_M0.P=P;
current_loop_M0.I=I;
current_loop_M0.D=D;
current_loop_M0.output_ramp=ramp;
}
//M0速度PID接口
float DFOC_M0_VEL_PID(float error) //M0速度环
{
return vel_loop_M0(error);
}
//M0角度PID接口
float DFOC_M0_ANGLE_PID(float error)
{
return angle_loop_M0(error);
}
//初始变量及函数定义
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
//宏定义实现的一个约束函数,用于限制一个值的范围。
//具体来说，该宏定义的名称为 _constrain，接受三个参数 amt、low 和 high，分别表示要限制的值、最小值和最大值。该宏定义的实现使用了三元运算符，根据 amt 是否小于 low 或大于 high，返回其中的最大或最小值，或者返回原值。
//换句话说，如果 amt 小于 low，则返回 low；如果 amt 大于 high，则返回 high；否则返回 amt。这样，_constrain(amt, low, high) 就会将 amt 约束在 [low, high] 的范围内。1
// 归一化角度到 [0,2PI]
float _normalizeAngle(float angle)
{
float a = fmod(angle, 2*PI); //取余运算可以用于归一化，列出特殊值例子算便知
return a >= 0 ? a : (a + 2*PI);
//三目运算符。格式：condition ? expr1 : expr2
//其中，condition 是要求值的条件表达式，如果条件成立，则返回 expr1 的值，否则返回 expr2 的值。可以将三目运算符视为 if-else 语句的简化形式。
//fmod 函数的余数的符号与除数相同。因此，当 angle 的值为负数时，余数的符号将与 _2PI 的符号相反。也就是说，如果 angle 的值小于 0 且 _2PI 的值为正数，则 fmod(angle, _2PI) 的余数将为负数。
//例如，当 angle 的值为 -PI/2，_2PI 的值为 2PI 时，fmod(angle, _2PI) 将返回一个负数。在这种情况下，可以通过将负数的余数加上 _2PI 来将角度归一化到 [0, 2PI] 的范围内，以确保角度的值始终为正数。
}
// 设置PWM到控制器输出
void setPwm(float Ua, float Ub, float Uc)
{
// 限制上限
Ua = _constrain(Ua, 0.0f, voltage_power_supply);
Ub = _constrain(Ub, 0.0f, voltage_power_supply);
Uc = _constrain(Uc, 0.0f, voltage_power_supply);
// 计算占空比
// 限制占空比从0到1
float dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
float dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
float dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );
//写入PWM到PWM 0 1 2 通道
ledcWrite(0, dc_a*255);
ledcWrite(1, dc_b*255);
ledcWrite(2, dc_c*255);
}
void setTorque(float Uq,float angle_el)
{
S0.Sensor_update(); //更新传感器数值
Uq=_constrain(Uq,-(voltage_power_supply)/2,(voltage_power_supply)/2);
float Ud=0;
angle_el = _normalizeAngle(angle_el);
// 帕克逆变换
Ualpha = -Uq*sin(angle_el);
Ubeta = Uq*cos(angle_el);
// 克拉克逆变换
Ua = Ualpha + voltage_power_supply/2;
Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2;
Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2;
setPwm(Ua,Ub,Uc);
}
void DFOC_Vbus(float power_supply)
{
voltage_power_supply=power_supply;
pinMode(pwmA, OUTPUT);
pinMode(pwmB, OUTPUT);
pinMode(pwmC, OUTPUT);
ledcSetup(0, 30000, 8); //pwm频道, 频率, 精度
ledcSetup(1, 30000, 8); //pwm频道, 频率, 精度
ledcSetup(2, 30000, 8); //pwm频道, 频率, 精度
ledcAttachPin(pwmA, 0);
ledcAttachPin(pwmB, 1);
ledcAttachPin(pwmC, 2);
Serial.println("完成PWM初始化设置");
//AS5600
S0_I2C.begin(19,18, 400000UL);
S0.Sensor_init(&S0_I2C); //初始化编码器0
Serial.println("编码器加载完毕");
//PID 加载
vel_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = voltage_power_supply/2};
//初始化电流传感器
CS_M0.init();
}
float _electricalAngle()
{
return _normalizeAngle((float)(DIR * PP) * S0.getMechanicalAngle()-zero_electric_angle);
}
void DFOC_alignSensor(int _PP,int _DIR)
{
PP=_PP;
DIR=_DIR;
setTorque(3, _3PI_2); //起劲
delay(1000);
S0.Sensor_update(); //更新角度，方便下面电角度读取
zero_electric_angle=_electricalAngle();
setTorque(0, _3PI_2); //松劲（解除校准）
Serial.print("0电角度：");
Serial.println(zero_electric_angle);
}
float DFOC_M0_Angle()
{
return DIR*S0.getAngle();
}
//无滤波
//float DFOC_M0_Velocity()
//{
// return DIR*S0.getVelocity();
//}
//有滤波
float DFOC_M0_Velocity()
{
//获取速度数据并滤波
float vel_M0_ori=S0.getVelocity();
float vel_M0_flit=M0_Vel_Flt(DIR*vel_M0_ori);
return vel_M0_flit; //考虑方向
}
//==============串口接收==============
float motor_target;
int commaPosition;
String serialReceiveUserCommand() {
// a string to hold incoming data
static String received_chars;
String command = "";
while (Serial.available()) {
// get the new byte:
char inChar = (char)Serial.read();
// add it to the string buffer:
received_chars += inChar;
// end of user input
if (inChar == '\n') {
// execute the user command
command = received_chars;
commaPosition = command.indexOf('\n');//检测字符串中的逗号
if(commaPosition != -1)//如果有逗号存在就向下执行
{
motor_target = command.substring(0,commaPosition).toDouble(); //电机角度
Serial.println(motor_target);
}
// reset the command buffer
received_chars = "";
}
}
return command;
}
float serial_motor_target()
{
return motor_target;
}
//================简易接口函数================
void DFOC_M0_set_Velocity_Angle(float Target)
{
setTorque(DFOC_M0_VEL_PID(DFOC_M0_ANGLE_PID((Target-DFOC_M0_Angle())*180/PI)),_electricalAngle()); //角度闭环
}
void DFOC_M0_setVelocity(float Target)
{
setTorque(DFOC_M0_VEL_PID((serial_motor_target()-DFOC_M0_Velocity())*180/PI),_electricalAngle()); //速度闭环
}
void DFOC_M0_set_Force_Angle(float Target) //力位
{
setTorque(DFOC_M0_ANGLE_PID((Target-DFOC_M0_Angle())*180/PI),_electricalAngle());
}
void DFOC_M0_setTorque(float Target)
{
setTorque(Target,_electricalAngle());
}
void runFOC()
{
//====传感器更新====
// S0.Sensor_update();
CS_M0.getPhaseCurrents();
//====传感器更新====
}
//=========================电流读取=========================
//通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
float cal_Iq_Id(float current_a,float current_b,float angle_el)
{
float I_alpha=current_a;
float I_beta = _1_SQRT3 * current_a + _2_SQRT3 * current_b;
float ct = cos(angle_el);
float st = sin(angle_el);
//float I_d = I_alpha * ct + I_beta * st;
float I_q = I_beta * ct - I_alpha * st;
return I_q;
}
float DFOC_M0_Current()
{
float I_q_M0_ori=cal_Iq_Id(CS_M0.current_a,CS_M0.current_b,_electricalAngle());
float I_q_M0_flit=M0_Curr_Flt(I_q_M0_ori);
return I_q_M0_flit;
}
dengFOC.h 源文件内容：
//灯哥开源，遵循GNU协议，转载请著名版权！
//GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。
//该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。
//仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源
//你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了
//函数声明
void setPwm(float Ua, float Ub, float Uc);
float setTorque(float Uq,float angle_el);
float _normalizeAngle(float angle);
void DFOC_Vbus(float power_supply);
void DFOC_alignSensor(int _PP,int _DIR);
float _electricalAngle();
float serial_motor_target();
String serialReceiveUserCommand();
//传感器读取
float DFOC_M0_Velocity();
float DFOC_M0_Angle();
//PID
void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp);
void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp);
float DFOC_M0_VEL_PID(float error);
float DFOC_M0_ANGLE_PID(float error);
void DFOC_M0_SET_CURRENT_PID(float P,float I,float D,float ramp);
//接口函数
void DFOC_M0_set_Velocity_Angle(float Target);
void DFOC_M0_setVelocity(float Target);
void DFOC_M0_set_Force_Angle(float Target);
void DFOC_M0_setTorque(float Target);
float DFOC_M0_Current();
float cal_Iq_Id(float current_a,float current_b,float angle_el);
void runFOC();
3.2 测试代码实现
代码37行： 设置供电电压
代码38行： 设置电极数和编码器的方向
代码57行： 获取电流参数
代码58行： 设置PID参数
代码59行：设置执行单元
源代码如下：
/* USER CODE BEGIN Header */
/**
******************************************************************************
* File Name : foc_function_test.c
* Description : FOC 闭环各种模式测试
******************************************************************************
* @attention
*
* COPYRIGHT: Copyright (c) 2025 tangmingfei2013@126.com
* CREATED BY: Alan.tang
* DATE: JAN 7th, 2025
* 参考代码： Deng FOC
*
******************************************************************************
*/
/* USER CODE END Header */
#include "DengFOC.h"
const int LED_PIN = 2;
int Sensor_DIR=-1; //传感器方向
int Motor_PP=7; //电机极对数
float Sensor_Vel;
/*
0: 电流模式
1: 速度模式
2: 位置模式
*/
#define SELECT_MODE 0
void setup()
{
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, LOW);
digitalWrite(LED_PIN, HIGH);
// put your setup code here, to run once:
Serial.begin(115200);
DFOC_Vbus(11.6); //设定驱动器供电电压
DFOC_alignSensor(Motor_PP,Sensor_DIR);
#if SELECT_MODE == 0
// 设置电流模式闭环PID
DFOC_M0_SET_CURRENT_PID(5,200,0,100000);
#elif SELECT_MODE == 1
// 设置速度闭环PID
DFOC_M0_SET_VEL_PID(0.01,0.00,0,0);
#elif SELECT_MODE == 2
//设置位置模式闭环PID
DFOC_M0_SET_VEL_PID(0.01,0.00,0,0);
DFOC_M0_SET_ANGLE_PID(0.5,0,0,0);
#endif
}
void postion_Mode_test( void )
{
//设置速度
DFOC_M0_set_Velocity_Angle(serial_motor_target());
}
void velocity_Mode_test( void )
{
//设置速度
DFOC_M0_setVelocity(serial_motor_target());
}
void current_Mode_test( void )
{
static int count=0;
// 电流模式
runFOC();
DFOC_M0_setTorque(serial_motor_target());
count++;
if(count>30)
{
count=0;
Serial.printf("%0.2f \n", DFOC_M0_Current());
}
}
void loop()
{
#if SELECT_MODE == 0
current_Mode_test();
#elif SELECT_MODE == 1
velocity_Mode_test();
#elif SELECT_MODE == 2
postion_Mode_test();
#endif
//接收串口
serialReceiveUserCommand();
}
4 测试
1） 设置电流值为0时，电流的输出波形
2）设置电流为2
3）设置电流值为-2
测试视频如下：
直流无刷电机控制（FOC）：电流模式测试视频