当前位置:   article > 正文

学习丨DengFOC库

dengfoc

学习 | DengFOC库(SimpleFOC同原理)

引言:

DengFOC库可以控制市面上大多的无刷电机,且布置在一个成本较低的Dengfoc驱动板上,非常方便开发者使用FOC来控制无刷电机。灯哥教程也很耐心,这是看完做的一些总结和个人的一些理解。(代码大部分源于DengFOC:https://space.bilibili.com/493192058/channel/collectiondetail?sid=1104775)

setTorque:

实际上是在对Uq进行计算,通过帕克逆变换和卡拉克变换得到Ua Ub Uc。

我们知道无刷电机FOC控制其实是需要三相电的,而且是相差120°的。Ua Ub Uc 的值也是需要随着时间,力矩来进行改变的。因此我们需要根据Uq来推出Ua Ub Uc

void setTorque(float Uq,float angle_el) {
  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);
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14

setPWM:

此函数可看出,先是调用_constrain()函数限制Ua b c 让其归一化在0-1之间,然后ledcWrite写入dc_a*255 使其范围从0-1放大到0-255,这样方便8位PWM输出输入。

void setPwm(float Ua, float Ub, float Uc) {

  // 计算占空比
  // 限制占空比从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);
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13

ledc:

ledc是ESP32里的一个库,库函数在下面列出。LEDC:LED CONTROL 顾名思义用于控制LED的,该库简化了PWM控制LED的代码,可以通过函数来改变PWM的频率从而改变输出电压,实现对LED的精准控制。在DengFOC库中用来定义PWM信号的频率和精度。

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初始化设置");
  BeginSensor();
 }
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15

LEDC的函数列出如下

ledcSetup(channel, freq, resolution)

Configures an LEDC channel.
channel: The channel number (0-15).
freq: Frequency in Hz.
resolution: Bit resolution (1-16), determines the PWM signal's duty cycle granularity.
ledcWriteTone(channel, frequency)

Generates a tone signal on a specific channel.
channel: The LEDC channel number.
frequency: The frequency of the tone in Hz.
ledcWriteNote(channel, note, octave)

Plays a musical note on a specific channel.
channel: The LEDC channel number.
note: Musical note to play (e.g., NOTE_A).
octave: Octave number.
ledcRead(channel)

Reads the duty cycle of a given channel.
channel: The LEDC channel number.
ledcReadFreq(channel)

Reads the frequency of a given channel.
channel: The LEDC channel number.
ledcAttachPin(pin, channel)

Attaches a pin to a specific channel.
pin: The GPIO pin number.
channel: The LEDC channel number.
ledcDetachPin(pin)

Detaches the LEDC channel from the specified pin.
pin: The GPIO pin number.
ledcFaderFuncInstall(intr_alloc_flags)

Installs the LEDC fade function.
intr_alloc_flags: Allocation flags for interrupt.
ledcFadeFuncUninstall()

Uninstalls the LEDC fade function.
ledcSetFadeWithTime(mode, channel, target_duty, max_fade_time_ms)

Fades a channel to a target duty over a specified time.
mode: LEDC high or low-speed mode.
channel: The LEDC channel number.
target_duty: Target duty cycle.
max_fade_time_ms: Time in milliseconds for the fade.
ledcSetFadeWithStep(mode, channel, target_duty, scale, cycle_num)

Fades a channel with a specified number of steps.
mode, channel, target_duty as above.
scale: Step scale.
cycle_num: Number of cycles for the fade.
ledcFadeStart(channel, fade_mode)

Starts fading on a channel.
channel: The LEDC channel number.
fade_mode: Fade mode (LEDC_FADE_NO_WAIT or LEDC_FADE_WAIT_DONE).
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59

DFOC_alignSensor:

此函数读取电角度并且储存,电角度由编码器AS5600 getRawAngle 和 getAngle_Without_track俩个函数得出。


void DFOC_alignSensor(int _PP,int _DIR)
{ 
  PP=_PP;
  DIR=_DIR;
  setTorque(3, _3PI_2);
  delay(3000);
  zero_electric_angle=_electricalAngle();
  setTorque(0, _3PI_2);
  Serial.print("0电角度:");Serial.println(zero_electric_angle);
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11

serialReceiveUserCommand:

不是特别理解,应该是检测输入的串口信息是否有逗号,如果有逗号就继续识别,例如给三个数据20,30,40这样?

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;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31

速度环设置

在DengFOC第七课,讲解了速度环的编写过程。
其中包含了,滤波,PID速度环的设置,传感器数据读取等。

setTorque:

可以看出要比L6的多了一行,用以更新从传感器获取的数值

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);
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15

我们可以在AS5600的库文件中找到这个Sensor_update的函数,他是通过最新一次的机械角度减去前一次的机械角度的位置得到d_angle,同时判断一下转动的圈数,full_rotations加一减一取决于d_angle>0?,且一定是大于0.8圈的才会计数。

void Sensor_AS5600::Sensor_update() {
    float val = getSensorAngle();
    angle_prev_ts = micros();
    float d_angle = val - angle_prev;
    // 圈数检测
    if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 
    angle_prev = val;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

getAngle:

获取角度,通过圈数*2pi+之前的角度值。

float Sensor_AS5600::getAngle(){
    return (float)full_rotations * _2PI + angle_prev;
}
  • 1
  • 2
  • 3

getVelocity:

获取速度,通过角度计算出角速度,公式为角度/转动角度所需时间Ts得到
Ts可能会因为Arduino运行时置零导致出现-的TS,所以用一句判断将Ts置成0.001s.
full_rotations-vel_full_rotations得到此次转动的圈数,*2pi得到角度,+angle_pre-vel_angle_prev得到实际的转动角度/Ts得到角速度。

float Sensor_AS5600::getVelocity() {
    // 计算采样时间
    float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6;
    // 快速修复奇怪的情况(微溢出)
    if(Ts <= 0) Ts = 1e-3f;
    // 速度计算
    float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;    
    // 保存变量以待将来使用
    vel_angle_prev = angle_prev;
    vel_full_rotations = full_rotations;
    vel_angle_prev_ts = angle_prev_ts;
    return vel;
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12

L7主循环

void loop() 
{
  //设置速度环PID
   DFOC_M0_SET_VEL_PID(0.005,0.00,0,0);
  //设置速度
   DFOC_M0_setVelocity(serial_motor_target());
  //接收串口
  serialReceiveUserCommand();

}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10

循环执行DFOC_MO_SET_VEL_PIDDF0C_M0_setVelocity这两个函数。在DengFOC库文件找到该函数

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;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
void DFOC_M0_setVelocity(float Target)
{
  setTorque(DFOC_M0_VEL_PID((serial_motor_target()-DFOC_M0_Velocity())*180/PI),_electricalAngle());   //速度闭环
}
  • 1
  • 2
  • 3
  • 4

Lowpass Filter

Tf是1/fs(截止频率的倒数)
y = alpha*y_prev + (1.0f - alpha)*x 这一段是低通滤波器的主要算法,alpha是比例因子,可以叫做平滑因子。
对于alpha的描述可以看:alpha = Tf/(Tf+dt),其实这是类似于一个时间的占比来决定他的权重。
dt<0.0s就让dt=0.001s
dt>0.3s就无视,因为间隔时间过远会影响滤波效果。

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

    if (dt < 0.0f ) dt = 1e-3f;
    else if(dt > 0.3f) {
        y_prev = x;
        timestamp_prev = timestamp;
        return x;
    }

    float alpha = Tf/(Tf + dt);
    float y = alpha*y_prev + (1.0f - alpha)*x;
    y_prev = y;
    timestamp_prev = timestamp;
    return y;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18

PID控制器

PID三个环节,P比例,I积分,D微分
在速度闭环中,在实际情况中,电机负载不同,提速也会不同。这就意味着要达到预设的V需要更长时间。如果只有P环,Error=Ref-Detection,PID控制器只有P环参与调节会不断接近,但无法到达预设值Ref;因此加入I环积分环,Integral error,不断叠加误差,就会让到达Ref的时间缩短并且更接近Ref。
在这里插入图片描述

这里有个图示,是ITs0.5*(error+error_prev)的物理意义。

// PID 控制器函数
float PIDController::operator() (float error){
    // 计算两次循环中间的间隔时间
    unsigned long timestamp_now = micros();
    float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
    if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
    
    // P环
    float proportional = P * error;
    // Tustin 散点积分(I环)
    float integral = integral_prev + I*Ts*0.5f*(error + error_prev);
    integral = _constrain(integral, -limit, limit);
    // D环(微分环节)
    float derivative = D*(error - error_prev)/Ts;

    // 将P,I,D三环的计算值加起来
    float output = proportional + integral + derivative;
    output = _constrain(output, -limit, limit);

    if(output_ramp > 0){
        // 对PID的变化速率进行限制
        float output_rate = (output - output_prev)/Ts;
        if (output_rate > output_ramp)
            output = output_prev + output_ramp*Ts;
        else if (output_rate < -output_ramp)
            output = output_prev - output_ramp*Ts;
    }
    // 保存值(为了下一次循环)
    integral_prev = integral;
    output_prev = output;
    error_prev = error;
    timestamp_prev = timestamp_now;
    return output;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/小小林熬夜学编程/article/detail/103150
推荐阅读
相关标签
  

闽ICP备14008679号