FOC 計算公式

　　1. PMSM.C 中的ADC1Interrupt（）AD中斷處理函數(shù)中調(diào)MeasCurr.S 中MeasCompCurr：

　　讀入ACD1，ACD2（帶符號小數(shù)）

　　校正：ADC1*CarrADC1-offset=qIa ADC2*CarrADC2-offset=qIb

　　2. PMSM.C 中的ADC1Interrupt（）中調(diào)CalculateParkAngle（）對相角θ處理

　　先調(diào)用smcpos.c 滑模觀測器中SMC_Position_Estimation（），之中

　?、儆终{(diào)用smc.s 中的CalcEstI 計算估算電流I ：

　　EstIalpha=Gsmopos*（Valpha-Ealpha-Zalpha）+Fsmopos*EstIalpha

　　---- Isα（n+1）=G*（Vα（n）-Eα（n）-Zα（n））+F*αIs α（n）

　　EstI beta=Gsmopos*（V beta-E beta-Z beta）+Fsmopos*EstI beta

　　---- Isβ（n+1）=G*（Vβ（n）-Eβ（n）-Zβ（n））+F*Isβ（n）

　　②調(diào)smc.s 中CalcIError 計算實測電流與估算電流的偏差：

　　IalphaError = EstIalpha – Ialpha

　　Ibeta Error = EstIbeta – Ibeta

　　---- ΔI α= Is α- I α ΔI β= Is β- I β

　?、壅{(diào)smc.s 中CalcZalpha ，CalcZbeta 計算Z α， Zβ：

　　當(dāng)abs（IalphaError）《 MaxSMCError時 Zalpha = （Kslide * IalphaError） / MaxSMCError

　　當(dāng)abs（IalphaError）≥ MaxSMCError時

　　Zalpha 為正時Zalpha= Kslide 為負時Zalpha=－Kslide

　　當(dāng)abs（I betaError）《 MaxSMCError時

　　Zbeta = （Kslide * IbetaError） / MaxSMCError

　　當(dāng)abs（I beta Error）≥ MaxSMCError時

　　Z beta 為正時Z beta = Kslide 為負時Z beta=－Kslide

　　---- 當(dāng)|ΔI α|《Δmax 時：Z α= Ks*ΔI α/Δmax

　　當(dāng)|ΔI α|≥Δmax 時：｜Z α｜= Ks

　　當(dāng)|ΔI β|《Δmax 時：Z β= Ks*ΔI β/Δmax

　　當(dāng)|ΔI β|≥Δmax 時：｜Z β｜= Ks

　?、苷{(diào)smc.s 中CalcBEM 按低通濾波器公式計算反電動勢： Ealpha = Ealpha + Kslf * （Zalpha - Ealpha）

　　Ebeta = Ebeta + Kslf * （Zbeta – Ebeta）

　　兩級濾波，后級濾波系數(shù)為KslfFinal

　　---- Eα（n）= Eα（n-1）+Ksf*（Zα（n-1）- Eα（n-1））

　　E β（n）= Eβ（n-1）+Ksf* （Zβ（n-1）- Eβ（n-1））

　?、菡{(diào)atan2CORDIC.s 中反正切函數(shù)atan2CORDIC 計算相角

　　Theta = atan2CORDIC（-EalphaFinal， EbetaFinal）

　　---- θ=atan（-Eα/ Eβ）

　　⑥在smcpos.c 中的SMC_Position_Estimation函數(shù)中進行速度預(yù)算

　　PrevTheta = s-》Theta

　　AccumThetaCnt++

　　if （AccumThetaCnt == IRP_PERCALC）

　　Omega = AccumTheta

　　---- 求累加角Σθ+=θ（n）-θ（n-1）

　　更新θ（n-1）= θ（n） 累加次數(shù)m（θCnt）+=1

　　當(dāng)m = M （速度計算需要的PWM 定時次數(shù)） 時，速度預(yù)算值ω=Σθ

　　此前僅算出ω（Omega），未算 V（速度Speed） ，即未找到V=Kv*ω

　　Kv=60/T（測算速度時間）/磁極對數(shù)

　?、哒{(diào)smc.s 中CalcOmegaFltred 對ω進行數(shù)字濾波

　　OmegaFltred=OmegaFltred+FiltOmCoef*（Omega-OmegaFltred）

　　---- ωfl （n）= ωfl （n-1）+ Kωfl *［ω（n）- ωfl （n-1）］

　　Kωfl：濾波系數(shù)=2πFc/Fpwm Fpwm：脈寬調(diào)制頻率

　　Fc： 濾波器截止頻率（電機最低轉(zhuǎn)速*磁極對數(shù)/60）

　?、嘣趕mcpos.c 中的SMC_Position_Estimation函數(shù)中計算濾波器系數(shù) ？？

　　Kslf = OMEGA0 * _PI / IRP_PERCALC;

　　KslfFinal = OMEGA0 * _PI / IRP_PERCALC

　?、嵩趕mcpos.c 中的SMC_Position_Estimation函數(shù)中進行相位補償計算

　　ThetaOffset=OmegaFltred*（SLOPEFRACn+SLOPEINTn）+CONSTANTn

　　不同的速度有不同的補償，程序分幾個頻段， 式中3常數(shù)取不同值（n 不同）。

　　---- θΔ =ωfl *（Kscn+Kstn）+ Kcn

　　3. PMSM.C 中的ADC1Interrupt（）中調(diào)clrkepark.s 的ClarkePark 進行座標(biāo)變化

　　Ialpha = Ia

　　Ibeta = Ia*dOneBySq3 + 2*Ib*dOneBySq3;

　　Id = Ialpha*cos（Angle） + Ibeta*sin（Angle）

　　Iq = -Ialpha*sin（Angle） + Ibeta*cos（Angle）

　　─ ---- Iα= Ia Iβ=（Ia+2Ib）/√3

　　Id = Iα*Cosθ+Iβ*Sinθ Iq = -Iα*Sinθ+Iβ*Cosθ

　　4. PMSM.C 中的ADC1Interrupt（）中調(diào)DoControl（）進行Id，Iq，Speed 的PI 迭代

　　5. 調(diào)trig.s 中的SinCos 計算qSin θ q Cosθ值

　　6. 調(diào)InvPark.s 中的InvPark 計算 qVα qVβ值

　　7. 調(diào)CalcRef.s 中的CalcRefVec 由qV α qVβ計算Vr 1 Vr2 Vr3 值

　　8. 調(diào)SVGEN.s 中的CalcSVGen 由Vr 1 Vr2 Vr3計算3相PWM 占空比。

　　強制角ParkParm.qAngle （輸出PWM 角）和滑模估算角Smc1.Theta 電機啟動時只用強制角線性遞增（稱斜坡法） Startup_Lock += 1 達到轉(zhuǎn)速后計算強制角和估算角偏差，對強制角進行修正

　　計算保存Δθ：Theta_error = ParkParm.qAngle - smc1.Theta 修正量：對強制角修正：ParkParm.qAngle += Startup_Ramp

　　Δθ每次±0.05°直到≤0.05°：Theta_error += _0_05DEG

　　FOC（或矢量控制）算法的簡要步驟

　　以下總結(jié)了控制PMSM的FOC算法步驟。

　　1.先測量三相定子電流ia和ib。根據(jù)ia+ib+ic=0，計算來自兩個電流傳感器的電流。

　　2.將三相電流轉(zhuǎn)換到2軸坐標(biāo)系中。該轉(zhuǎn)換根據(jù)測量的ia、ib和ic值得到變量iα和iβ。從定子的角度來說，iα和iβ是時變正交電流值。該步驟稱為Park變換。

　　3.采用控制循環(huán)上一次迭代時計算的變換角旋轉(zhuǎn)2軸坐標(biāo)系，使之與轉(zhuǎn)子磁通對齊。該轉(zhuǎn)換根據(jù)iα和iβ得到變量id和iq?，F(xiàn)在，將正交電流id和iq變換到旋轉(zhuǎn)坐標(biāo)系中。在穩(wěn)態(tài)條件下，id和iq將保持恒定。該步驟稱為Clarke變換。