学习丨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_PID和DF0C_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