以下是一个基于C的FOC电机驱动程序，采用滑膜观测器：
```c
#include <stdio.h>
#include <math.h>
#define PI 3.14159265358979323846
// 定义电机相关参数
#define R 1.5 // 电机电阻
#define L 0.001 // 电机电感
#define J 0.01 // 电机转动惯量
#define B 0.1 // 电机阻尼系数
#define Ke 0.01 // 电动势常数
#define Kt 0.01 // 转矩常数
// 定义控制参数
#define Ts 0.001 // 采样时间
#define Kp 2 // 比例系数
#define Ki 1 // 积分系数
// 定义变量
double Ia, Ib, Id, Iq, Vd, Vq, theta_e, omega_e, theta_m, omega_m, Te, Ua, Ub, Uc;
double Id_ref, Iq_ref, theta_e_ref, omega_e_ref, omega_m_ref, Te_ref;
double Id_err, Iq_err, theta_e_err, omega_e_err, omega_m_err, Te_err;
double Id_sum, Iq_sum, theta_e_sum, omega_e_sum, omega_m_sum, Te_sum;
double I_alpha, I_beta, theta_e_hat, omega_e_hat, theta_m_hat, omega_m_hat;
double Ls, Lr, Lm, Rs, Rr, Vdc, Vs_alpha, Vs_beta;
// 定义函数
void ClarkeParkTransform(double Ia, double Ib, double theta_e, double *I_alpha, double *I_beta)
{
double sin_theta_e = sin(theta_e);
double cos_theta_e = cos(theta_e);
*I_alpha = Ia * cos_theta_e + Ib * sin_theta_e;
*I_beta = -Ia * sin_theta_e + Ib * cos_theta_e;
}
void InverseClarkeParkTransform(double I_alpha, double I_beta, double theta_e, double *Ia, double *Ib)
{
double sin_theta_e = sin(theta_e);
double cos_theta_e = cos(theta_e);
*Ia = I_alpha * cos_theta_e - I_beta * sin_theta_e;
*Ib = I_alpha * sin_theta_e + I_beta * cos_theta_e;
}
void SMOObserver(double Ls, double Lr, double Rs, double Rr, double Lm, double Ke, double Kt, double B,
double U_alpha, double U_beta, double I_alpha, double I_beta, double omega_e_hat, double theta_e_hat,
double *Iq_hat, double *Id_hat, double *theta_e_hat_dot, double *omega_e_hat_dot)
{
double Ls_plus_Lm = Ls + Lm;
double Lr_plus_Lm = Lr + Lm;
double sin_theta_e_hat = sin(theta_e_hat);
double cos_theta_e_hat = cos(theta_e_hat);
double Lm_div_Lr_plus_Lm = Lm / Lr_plus_Lm;
double Lm_div_Ls_plus_Lm = Lm / Ls_plus_Lm;
double Rr_div_Lr_plus_Lm = Rr / Lr_plus_Lm;
double Lm_mul_Ke_div_Lr_plus_Lm = Lm * Ke / Lr_plus_Lm;
double Kt_div_Ls_plus_Lm = Kt / Ls_plus_Lm;
double Kt_div_Lr_plus_Lm = Kt / Lr_plus_Lm;
double B_div_Lr_plus_Lm = B / Lr_plus_Lm;
// 计算电磁转矩
double Iq = *Iq_hat - Lm_div_Lr_plus_Lm * omega_e_hat * I_alpha;
double Id = *Id_hat + Lm_div_Ls_plus_Lm * omega_e_hat * I_beta;
double Te = Kt_div_Ls_plus_Lm * (Lm_div_Lr_plus_Lm * omega_e_hat * Iq - (Rs + Rr_div_Lr_plus_Lm) * Id);
// 更新滑膜观测器状态
*Iq_hat += Ts * (Lm_mul_Ke_div_Lr_plus_Lm * I_alpha - Kt_div_Lr_plus_Lm * omega_e_hat * Iq + Te * Lm_div_Lr_plus_Lm);
*Id_hat += Ts * (-Lm_mul_Ke_div_Ls_plus_Lm * I_beta - Kt_div_Ls_plus_Lm * omega_e_hat * Id + Te * Lm_div_Ls_plus_Lm);
*theta_e_hat += Ts * omega_e_hat;
*omega_e_hat += Ts * (Te - B_div_Lr_plus_Lm * omega_e_hat - (Rs + Rr_div_Lr_plus_Lm) * Iq) / J;
*theta_e_hat_dot = omega_e_hat;
*omega_e_hat_dot = (Te - B_div_Lr_plus_Lm * omega_e_hat - (Rs + Rr_div_Lr_plus_Lm) * Iq) / J;
}
int main()
{
// 初始化变量
Ia = 0;
Ib = 0;
Id = 0;
Iq = 0;
Vd = 0;
Vq = 0;
theta_e = 0;
omega_e = 0;
theta_m = 0;
omega_m = 0;
Te = 0;
Ua = 0;
Ub = 0;
Uc = 0;
Id_ref = 0;
Iq_ref = 0;
theta_e_ref = 0;
omega_e_ref = 0;
omega_m_ref = 0;
Te_ref = 0;
Id_err = 0;
Iq_err = 0;
theta_e_err = 0;
omega_e_err = 0;
omega_m_err = 0;
Te_err = 0;
Id_sum = 0;
Iq_sum = 0;
theta_e_sum = 0;
omega_e_sum = 0;
omega_m_sum = 0;
Te_sum = 0;
I_alpha = 0;
I_beta = 0;
theta_e_hat = 0;
omega_e_hat = 0;
theta_m_hat = 0;
omega_m_hat = 0;
Ls = 0.001;
Lr = 0.001;
Lm = 0.002;
Rs = 1;
Rr = 1;
Vdc = 24;
Vs_alpha = 0;
Vs_beta = 0;
// 循环
while (1) {
// 读取电机电流和位置
scanf("%lf %lf %lf", &Ia, &Ib, &theta_m);
// 计算 alpha-beta 坐标下电流
ClarkeParkTransform(Ia, Ib, theta_e, &I_alpha, &I_beta);
// 使用滑膜观测器计算电流的 d-q 坐标下值
SMOObserver(Ls, Lr, Rs, Rr, Lm, Ke, Kt, B, Vs_alpha, Vs_beta, I_alpha, I_beta, omega_e_hat, theta_e_hat, &Iq, &Id, &theta_e_hat_dot, &omega_e_hat_dot);
// 计算电机转速
omega_m_hat = omega_e_hat + omega_m_ref;
// 计算电机电压
Vd = Id_ref * R + L * (Id_err / Ts) + omega_e_hat * L * Iq;
Vq = Iq_ref * R + L * (Iq_err / Ts) - omega_e_hat * L * Id;
// 使用逆变器计算输出电压
Ua = 2 / 3 * (Vdc * cos(theta_e) - Vd);
Ub = 2 / 3 * (Vdc * cos(theta_e - 2 * PI / 3) - Vd);
Uc = 2 / 3 * (Vdc * cos(theta_e + 2 * PI / 3) - Vd);
// 输出控制信号
printf("%lf %lf %lf\n", Ua, Ub, Uc);
// 更新控制参数
Id_err = Id_ref - Id;
Iq_err = Iq_ref - Iq;
Id_sum += Id_err * Ts;
Iq_sum += Iq_err * Ts;
theta_e_err = theta_e_ref - theta_e_hat;
theta_e_sum += theta_e_err * Ts;
omega_e_err = omega_e_ref - omega_e_hat;
omega_e_sum += omega_e_err * Ts;
omega_m_err = omega_m_ref - omega_m_hat;
omega_m_sum += omega_m_err * Ts;
Te_err = Te_ref - Te;
Te_sum += Te_err * Ts;
Id_ref += Kp * Id_err + Ki * Id_sum;
Iq_ref += Kp * Iq_err + Ki * Iq_sum;
theta_e_ref += Kp * theta_e_err + Ki * theta_e_sum;
omega_e_ref += Kp * omega_e_err + Ki * omega_e_sum;
omega_m_ref += Kp * omega_m_err + Ki * omega_m_sum;
Te_ref += Kp * Te_err + Ki * Te_sum;
}
return 0;
}
```
122
非常没帮助
没帮助
一般
有帮助
非常有帮助
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
