目录
概述
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 phasegain_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 phasegain_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;// ampscurrent_b = (readADCVoltageInline(pinB) - offset_ib)*gain_b;// ampscurrent_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到1float 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初始化设置");//AS5600S0_I2C.begin(19,18, 400000UL);S0.Sensor_init(&S0_I2C); //初始化编码器0Serial.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 datastatic 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 inputif (inChar == '\n') {// execute the user commandcommand = 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 0void 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// 设置电流模式闭环PIDDFOC_M0_SET_CURRENT_PID(5,200,0,100000);#elif SELECT_MODE == 1// 设置速度闭环PIDDFOC_M0_SET_VEL_PID(0.01,0.00,0,0);#elif SELECT_MODE == 2//设置位置模式闭环PIDDFOC_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 == 0current_Mode_test();
#elif SELECT_MODE == 1velocity_Mode_test();
#elif SELECT_MODE == 2postion_Mode_test();
#endif//接收串口serialReceiveUserCommand();
}4 测试
1） 设置电流值为0时，电流的输出波形
2）设置电流为2
3）设置电流值为-2
测试视频如下：
直流无刷电机控制（FOC）：电流模式测试视频