电机库整成的foc无感代码讲解

// Clarke变换：三相静止→两相静止（α-β）
void ClarkeTransform(float Ia, float Ib, float Ic, float *Ialpha, float *Ibeta) {
    *Ialpha = Ia;
    *Ibeta = (Ia + 2*Ib) / sqrt(3);  // 假设三相平衡（Ic = -Ia - Ib）
}

// Park变换：两相静止→两相旋转（d-q）
void ParkTransform(float Ialpha, float Ibeta, float theta, float *Id, float *Iq) {
    *Id = Ialpha * cos(theta) + Ibeta * sin(theta);
    *Iq = -Ialpha * sin(theta) + Ibeta * cos(theta);
}

// 逆Park变换：d-q→α-β
void InvParkTransform(float Vd, float Vq, float theta, float *Valpha, float *Vbeta) {
    *Valpha = Vd * cos(theta) - Vq * sin(theta);
    *Vbeta = Vd * sin(theta) + Vq * cos(theta);
}

// 滑模观测器估算反电动势
void SMOPLL(float Ialpha, float Ibeta, float Valpha, float Vbeta, float *Ealpha, float *Ebeta) {
    // 1. 电机模型误差计算
    static float Ialpha_hat, Ibeta_hat;
    float alpha = R * Ialpha + L * (Ialpha - Ialpha_hat_prev)/Ts;
    float beta = R * Ibeta + L * (Ibeta - Ibeta_hat_prev)/Ts;

    // 2. 滑模控制项（sign函数或饱和函数）
    float K_slide = 10.0; // 滑模增益
    float e_alpha = Ialpha_hat - Ialpha;
    float e_beta = Ibeta_hat - Ibeta;
    *Ealpha = K_slide * sign(e_alpha);
    *Ebeta = K_slide * sign(e_beta);

    // 3. 更新观测器状态
    Ialpha_hat_prev = Ialpha_hat;
    Ibeta_hat_prev = Ibeta_hat;
}

void PLL_Update(float Ealpha, float Ebeta, float *theta, float *speed) {
    // 1. 计算反电动势角度
    float theta_est = atan2(-Ebeta, Ealpha); // 注意符号可能需调整

    // 2. PLL闭环修正角度误差
    static float integral_error = 0;
    float Kp = 100.0, Ki = 1000.0; // PLL参数

    float error = sin(theta_est - *theta); // 小角度近似线性化
    integral_error += error * Ts;

    *speed = Kp * error + Ki * integral_error; // 速度估算
    *theta += *speed * Ts;                     // 角度积分更新
}

void FOC_Loop() {
    // 1. 电流采样（ADC获取Ia, Ib）
    float Ia = Read_CurrentA();
    float Ib = Read_CurrentB();
    float Ic = -Ia - Ib; // 假设三相平衡

    // 2. Clarke/Park变换
    float Ialpha, Ibeta;
    ClarkeTransform(Ia, Ib, Ic, &Ialpha, &Ibeta);
    ParkTransform(Ialpha, Ibeta, theta, &Id, &Iq);

    // 3. 速度环PI控制（外环）
    static float speed_ref = 100.0; // 目标转速（RPM）
    Iq_ref = PI_Speed_Regulator(speed_ref - speed_est);

    // 4. 电流环PI控制（内环）
    Vd = PI_CurrentD_Regulator(0 - Id); // 通常Id=0（最大扭矩控制）
    Vq = PI_CurrentQ_Regulator(Iq_ref - Iq);

    // 5. 逆Park变换 + SVPWM
    float Valpha, Vbeta;
    InvParkTransform(Vd, Vq, theta, &Valpha, &Vbeta);
    SvpwmGenerate(Valpha, Vbeta); // 生成PWM占空比

    // 6. 无感估算
    float Ealpha, Ebeta;
    SMOPLL(Ialpha, Ibeta, Valpha, Vbeta, &Ealpha, &Ebeta);
    PLL_Update(Ealpha, Ebeta, &theta, &speed_est);
}

加载更多