FOC 計(jì)算公式

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

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

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

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

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

　?、儆终{(diào)用smc.s 中的CalcEstI 計(jì)算估算電流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 計(jì)算實(shí)測(cè)電流與估算電流的偏差：

　　IalphaError = EstIalpha – Ialpha

　　Ibeta Error = EstIbeta – Ibeta

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

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

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

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

　　Zalpha 為正時(shí)Zalpha= Kslide 為負(fù)時(shí)Zalpha=－Kslide

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

　　Zbeta = （Kslide * IbetaError） / MaxSMCError

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

　　Z beta 為正時(shí)Z beta = Kslide 為負(fù)時(shí)Z beta=－Kslide

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

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

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

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

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

　　Ebeta = Ebeta + Kslf * （Zbeta – Ebeta）

　　兩級(jí)濾波，后級(jí)濾波系數(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 計(jì)算相角

　　Theta = atan2CORDIC（-EalphaFinal， EbetaFinal）

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

　?、拊趕mcpos.c 中的SMC_Position_Estimation函數(shù)中進(jìn)行速度預(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 （速度計(jì)算需要的PWM 定時(shí)次數(shù)） 時(shí)，速度預(yù)算值ω=Σθ

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

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

　　⑦調(diào)smc.s 中CalcOmegaFltred 對(duì)ω進(jìn)行數(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： 濾波器截止頻率（電機(jī)最低轉(zhuǎn)速*磁極對(duì)數(shù)/60）

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

　　Kslf = OMEGA0 * _PI / IRP_PERCALC;

　　KslfFinal = OMEGA0 * _PI / IRP_PERCALC

　　⑨在smcpos.c 中的SMC_Position_Estimation函數(shù)中進(jìn)行相位補(bǔ)償計(jì)算

　　ThetaOffset=OmegaFltred*（SLOPEFRACn+SLOPEINTn）+CONSTANTn

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

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

　　3. PMSM.C 中的ADC1Interrupt（）中調(diào)clrkepark.s 的ClarkePark 進(jìn)行座標(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（）進(jìn)行Id，Iq，Speed 的PI 迭代

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

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

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

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

　　強(qiáng)制角ParkParm.qAngle （輸出PWM 角）和滑模估算角Smc1.Theta 電機(jī)啟動(dòng)時(shí)只用強(qiáng)制角線(xiàn)性遞增（稱(chēng)斜坡法） Startup_Lock += 1 達(dá)到轉(zhuǎn)速后計(jì)算強(qiáng)制角和估算角偏差，對(duì)強(qiáng)制角進(jìn)行修正

　　計(jì)算保存Δθ：Theta_error = ParkParm.qAngle - smc1.Theta 修正量：對(duì)強(qiáng)制角修正：ParkParm.qAngle += Startup_Ramp

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

　　FOC（或矢量控制）算法的簡(jiǎn)要步驟

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

　　1.先測(cè)量三相定子電流ia和ib。根據(jù)ia+ib+ic=0，計(jì)算來(lái)自?xún)蓚€(gè)電流傳感器的電流。

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

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