学习 | 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);
}
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);
}
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();
}
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).
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);
}
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;
}
速度环设置
在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);
}
我们可以在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;
}
getAngle:
获取角度,通过圈数*2pi+之前的角度值。
float Sensor_AS5600::getAngle(){
return (float)full_rotations * _2PI + angle_prev;
}
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;
L7主循环
void loop()
{
//设置速度环PID
DFOC_M0_SET_VEL_PID(0.005,0.00,0,0);
//设置速度
DFOC_M0_setVelocity(serial_motor_target());
//接收串口
serialReceiveUserCommand();
}
循环执行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;
}
void DFOC_M0_setVelocity(float Target)
{
setTorque(DFOC_M0_VEL_PID((serial_motor_target()-DFOC_M0_Velocity())*180/PI),_electricalAngle()); //速度闭环
}
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;
}
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;
}