SimpleFOC v2.0.1库部分解读

最新推荐文章于 2024-08-04 22:21:31 发布

Ron Liang

最新推荐文章于 2024-08-04 22:21:31 发布

阅读量5.8k

点赞数 17

文章标签：算法嵌入式

本文链接：https://blog.csdn.net/weixin_43819326/article/details/116141900

版权

本文详细解读了SimpleFOC v2.0.1库的common基础文件，包括defaults.h中的默认参数设置，foc_utils中的角度计算和低通滤波函数，以及pid和time_utils的相关实现。同时，介绍了drivers模块中的硬件特定配置和BLDC电机驱动，如BLDCDriver3PWM和6PWM的初始化与占空比设置，为理解FOC控制提供了深入的洞察。

摘要由CSDN通过智能技术生成

一、SimpleFOC v2.0.1 库

1.common 基础文件

文件里是基本的foc底层算法，pid，低通滤波，三角函数等

1）defaults.h

配置默认的参数值，可通过程序初始化设定进行改动

//电源电压
DEF_POWER_SUPPLY 12.0 
    
//速度环PID控制器参数（速度环通常只用到PI）
DEF_PID_VEL_P 0.5 
DEF_PID_VEL_I 10.0 
DEF_PID_VEL_D 0.0 
DEF_PID_VEL_U_RAMP 1000.0 //默认PID控制器电压斜坡值
    
//位置环P控制器参数（位置环这里只用到P）
DEF_P_ANGLE_P 20.0 
DEF_VEL_LIM 20.0 //默认角速度限制
    
//索引搜索目标速度 
DEF_INDEX_SEARCH_TARGET_VELOCITY 1.0 //默认索引搜索速度

//校准电压
DEF_VOLTAGE_SENSOR_ALIGN 6.0 //传感器和电机校准调零的默认电压 

//低通滤波速度
DEF_VEL_FILTER_Tf 0.005 //默认低通滤波速度时间常数

2）foc_utils

头文件定义：

// 函数符号
_sign(a) ( ( (a) < 0 )  ?  -1   : ( (a) > 0 ) )
_round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
_constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))//约束

// 基本参数定义
#define _2_SQRT3 1.15470053838
#define _SQRT3 1.73205080757
#define _1_SQRT3 0.57735026919
#define _SQRT3_2 0.86602540378
#define _SQRT2 1.41421356237
#define _120_D2R 2.09439510239
#define _PI 3.14159265359
#define _PI_2 1.57079632679
#define _PI_3 1.0471975512
#define _2PI 6.28318530718
#define _3PI_2 4.71238898038

#define NOT_SET -12345.0

角度相关计算函数：

使用int数组代替float数组进行运算，2x存储保存（int 2Byte float 4Byte）

sin*10000

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};

正弦函数（用定长数组逼近正弦函数），输入0-2π角度

float _sin(float a){
   
  if(a < _PI_2){
   
    return 0.0001*sine_array[_round(126.6873* a)];       //整数数组优化
  }else if(a < _PI){
   
    return 0.0001*sine_array[398 - _round(126.6873*a)];  
  }else if(a < _3PI_2){
   
    return -0.0001*sine_array[-398 + _round(126.6873*a)]; 
  } else {
   
    return -0.0001*sine_array[796 - _round(126.6873*a)];  
  }
}

余弦函数（用定长数组逼近正弦函数），输入0-2π角度

float _cos(float a){
   
  float a_sin = a + _PI_2;
  a_sin = a_sin > _2PI ? a_sin - _2PI : a_sin;
  return _sin(a_sin);
}

弧度规格化成0-2π

float _normalizeAngle(float angle){
   
  float a = fmod(angle, _2PI);
  return a >= 0 ? a : (a + _2PI);
}

电角度计算（shaft_angle 轴角， pole_pairs 极对数）

float _electricalAngle(float shaft_angle, int pole_pairs) {
   
  return (shaft_angle * pole_pairs);
}

3）lowpass_filter

低通滤波

头文件类定义：

class LowPassFilter
{
   
public:
    /**
     * @param Tf - Low pass filter time constant
     */
    LowPassFilter(float Tf);
    ~LowPassFilter() = default;

    float operator() (float x);
    float Tf; //!< Low pass filter time constant

protected:
    unsigned long timestamp_prev;  //!< Last execution timestamp
    float y_prev; //!< filtered value in previous execution step 
};

函数：

LowPassFilter::LowPassFilter(float time_constant)
    : Tf(time_constant)
    , y_prev(0.0f)
{
   
    timestamp_prev = _micros();
}


float LowPassFilter::operator() (float x)
{
   
    unsigned long timestamp = _micros();
    float dt = (timestamp - timestamp_prev)*1e-6f;

    if (dt < 0.0f || dt > 0.5f)
        dt = 1e-3f;

    float alpha = Tf/(Tf + dt);
    float y = alpha*y_prev + (1.0f - alpha)*x;

    y_prev = y;
    timestamp_prev = timestamp;
    return y;
}

4）pid

头文件类定义

/**
 *  PID controller class
 */
class PIDController
{
   
public:
    /**
     *  
     * @param P - Proportional gain 
     * @param I - Integral gain
     * @param D - Derivative gain 
     * @param ramp - Maximum speed of change of the output value
     * @param limit - Maximum output value
     */
    PIDController(float P, float I, float D, float ramp, float limit);
    ~PIDController() = default;

    float operator() (float error);

    float P; //!< Proportional gain 
    float I; //!< Integral gain 
    float D; //!< Derivative gain 
    float output_ramp; //!< Maximum speed of change of the output value
    float limit; //!< Maximum output value

protected:
    float integral_prev; //!< last integral component value
    float error_prev; //!< last tracking error value
    float timestamp_prev; //!< Last execution timestamp
    float output_prev;  //!< last pid output value
};

具体函数

PIDController::PIDController(float P, float I, float D, float ramp, float limit)
    : P(P)
    , I(I)
    , D(D)
    , output_ramp(ramp)    // 输出导数限制（斜坡） [volts/second]
    , limit(limit)         // 输出数值（电压）限制 [volts]
    , integral_prev(0.0)
    , error_prev(0.0)
    , output_prev(0.0)
{
   
    timestamp_prev = _micros()*1e-6;
}

// PID控制器功能
float PIDController::operator() (float error){
   
    // 计算上一次时间
    unsigned long timestamp_now = _micros();
    float Ts = (timestamp_now - timestamp_prev) * 1e-6;
    // 异常情况修复（时间溢出）
    if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; 

    // u(s) = (P + I/s + Ds)e(s)
    // 实现离散
    // 比例部分 
    // u_p  = P *e(k)
    float proportional = P * error;
    // 积分部分双线性变换
    /