直流无刷电机控制（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();
}