735 lines
18 KiB
C
735 lines
18 KiB
C
/**************************************************************************
|
|
Description: Ôóíêöèÿ ðåàëèçóåò àëãîðèòì óïðàâëåíèÿ INU
|
|
(îòðàáîòêà N è P).
|
|
|
|
Àâòîð: Óëèòîâñêèé Ä.È.
|
|
Äàòà ïîñëåäíåãî îáíîâëåíèÿ: 2021.11.08
|
|
**************************************************************************/
|
|
|
|
|
|
#include "def.h"
|
|
#include "upr.h"
|
|
#include "estimate.h"
|
|
|
|
|
|
#pragma CODE_SECTION(control_current, "ramfuncs");
|
|
#pragma CODE_SECTION(control_flux, "ramfuncs");
|
|
#pragma CODE_SECTION(control_speed_power, "ramfuncs");
|
|
#pragma CODE_SECTION(indirect_vector_control, "ramfuncs");
|
|
#pragma CODE_SECTION(ipark, "ramfuncs");
|
|
#pragma CODE_SECTION(limit_current, "ramfuncs2");
|
|
#pragma CODE_SECTION(pwm, "ramfuncs2");
|
|
#pragma CODE_SECTION(reference_flux, "ramfuncs");
|
|
#pragma CODE_SECTION(reference_power, "ramfuncs");
|
|
#pragma CODE_SECTION(reference_speed, "ramfuncs");
|
|
#pragma CODE_SECTION(select_feedback, "ramfuncs");
|
|
#pragma CODE_SECTION(upr, "ramfuncs");
|
|
|
|
|
|
void control_current(void);
|
|
void control_flux(void);
|
|
void control_speed_power(void);
|
|
void indirect_vector_control(void);
|
|
void ipark(void);
|
|
void limit_current(void);
|
|
void pwm(float ya, float yb, float yc);
|
|
void reference_flux(void);
|
|
void reference_power(void);
|
|
void reference_speed(void);
|
|
void select_feedback(void);
|
|
|
|
|
|
void upr(void) {
|
|
static float yAux1;
|
|
static float yAux2;
|
|
static float ya;
|
|
static float yb;
|
|
static float yc;
|
|
static short decim_psi_wm_pm;
|
|
static Estimate_Test_t step;
|
|
|
|
if ( onceUpr == 0 ) {
|
|
estimate_init();
|
|
onceUpr = 1;
|
|
decim_psi_wm_pm = (short)DECIM_PSI_WM_PM;
|
|
psi = 0;
|
|
rf.once = 0;
|
|
rs.once = 0;
|
|
rp.once = 0;
|
|
cf.once = 0;
|
|
csp.once = 0;
|
|
ivc.once = 0;
|
|
cc.once = 0;
|
|
estimate_start(ESTIMATE_TEST_RR_L);
|
|
}
|
|
|
|
// âíåøíèå êîíòóðû ðåãóëèðîâàíèÿ (ïî ïîòîêó, îáîðîòàì è ìîùíîñòè)
|
|
// íå äîëæíû áûòü òàêèìè æå áûñòðûìè êàê âíóòðåííèå (ïî òîêó) (?)
|
|
if ( decim_psi_wm_pm < (short)DECIM_PSI_WM_PM ) {
|
|
decim_psi_wm_pm++;
|
|
}
|
|
else {
|
|
decim_psi_wm_pm = 1;
|
|
// Èçìåðåíèå Rr è Lk: ñèíóñ ïî Iq
|
|
|
|
estimate_process(cc.yd1, cc.yq1, id1, iq1, TY * DECIM_PSI_WM_PM,
|
|
&idZ, &iqZ, NULL);
|
|
|
|
//// çàäàííîå ïîòîêîñöåïëåíèå
|
|
//// (rf.PsiZ -> rf.psiZ, rf.pPsiZ)
|
|
//reference_flux();
|
|
|
|
//// çàäàííàÿ ñêîðîñòü
|
|
//// (mst.wmZz, mst.wmLim -> rs.wmZ, rs.pWmZ)
|
|
//reference_speed();
|
|
|
|
//// çàäàííàÿ ìîùíîñòü
|
|
//// (mst.pmZz, mst.pmLim -> rp.pmZ)
|
|
//reference_power();
|
|
|
|
//// çàäàííûé òîê (idZ, iqZ)
|
|
//// ... ðåãóëÿòîð ïîòîêîñöåïëåíèÿ
|
|
//// (rf.psiZ, psi, rf.pPsiZ, cf.IdLim, cf.IdLimNeg -> idZ)
|
|
//control_flux();
|
|
//// ... ðåãóëÿòîðû ñêîðîñòè è ìîùíîñòè
|
|
//// (rs.wmZ, wm, rs.pWmZ, rp.pmZ, mst.wmLim, mst.pmLim, csp.IqLim,
|
|
//// csp.IqLimNeg -> iqZ, inuWork)
|
|
//control_speed_power();
|
|
//// ... îãðàíè÷åíèå ïîëíîãî òîêà
|
|
//// (idZ, iqZ, IzLim -> idZ, iqZ, csp.iqLimFlag)
|
|
//limit_current();
|
|
} //decim_psi_wm_pm
|
|
|
|
// indirect vector control
|
|
// (wmNf, wm, ix1, iy1, ix2, iy2 -> ivc.ws, ivc.sinTheta, ivc.cosTheta,
|
|
// ivc.id1, ivc.iq1, ivc.id2, ivc.iq2, ivc.psi)
|
|
indirect_vector_control();
|
|
|
|
// âûáîð ñèãíàëîâ î.ñ.
|
|
// (... -> ws, sinTheta, cosTheta, id1, iq1, id2, iq2, psi)
|
|
select_feedback();
|
|
|
|
// ðåãóëÿòîðû òîêà
|
|
// (idZ, iqZ, id1, iq1, id2, iq2, ws, wm, psi ->
|
|
// -> cc.yd1, cc.yq1, cc.yd2, cc.yq2)
|
|
control_current();
|
|
|
|
// ïåðåâîä ñèãíàëîâ óïðàâëåíèÿ èç ñ.ê. d-q â ñ.ê. x-y
|
|
// (cc.yd1, cc.yq1, cc.yd2, cc.yq2, sinTheta, cosTheta, ws ->
|
|
// -> ip.yx1, ip.yy1, ip.yx2, ip.yy2)
|
|
ipark();
|
|
|
|
// ØÈÌ
|
|
// (ip.yx1, ip.yy1, ip.yx2, ip.yy2 ->
|
|
// -> EPwm1Regs.CMPA.half.CMPA, EPwm2Regs.CMPA.half.CMPA,
|
|
// EPwm3Regs.CMPA.half.CMPA, EPwm4Regs.CMPA.half.CMPA,
|
|
// EPwm5Regs.CMPA.half.CMPA, EPwm6Regs.CMPA.half.CMPA,
|
|
// EPwm7Regs.CMPA.half.CMPA, EPwm8Regs.CMPA.half.CMPA,
|
|
// EPwm9Regs.CMPA.half.CMPA, EPwm10Regs.CMPA.half.CMPA,
|
|
// EPwm11Regs.CMPA.half.CMPA, EPwm12Regs.CMPA.half.CMPA)
|
|
|
|
|
|
|
|
// ïåðåâîäèì èç ñ.ê. x-y â ñ.ê. a-b-c
|
|
yAux1 = ip.yx1 * (-0.5 * ISQRT3);
|
|
yAux2 = ip.yy1 * 0.5;
|
|
ya = ip.yx1 * ISQRT3;
|
|
yb = yAux1 + yAux2;
|
|
yc = yAux1 - yAux2;
|
|
pwm(ya, yb, yc);
|
|
} //void upr(void)
|
|
|
|
|
|
|
|
// Ðåãóëèðóåò òîê
|
|
// (idZ, iqZ, id1, iq1, id2, iq2, ws, wm, psi ->
|
|
// -> cc.yd1, cc.yq1, cc.yd2, cc.yq2)
|
|
void control_current(void) {
|
|
if ( cc.once == 0 ) {
|
|
cc.once = 1;
|
|
cc.y1LimFlag = 0;
|
|
cc.yd1I = 0;
|
|
cc.yq1I = 0;
|
|
cc.yd1 = 0;
|
|
cc.yq1 = 0;
|
|
// äëÿ ñîêðàùåíèÿ âû÷èñëåíèé
|
|
cc.K1 = sgmPar.SigmaLs*I_BAZ*WE_BAZ*U_2_Y;
|
|
cc.K2 = sgmPar.Rr*sgmPar.Lm/(sgmPar.Lr*sgmPar.Lr)*PSI_BAZ*U_2_Y;
|
|
cc.K3 = sgmPar.Kl*PSI_BAZ*WE_BAZ*U_2_Y;
|
|
}
|
|
|
|
// äëÿ ñîêðàùåíèÿ âû÷èñëåíèé
|
|
cc.Xyff = ws*cc.K1;
|
|
cc.yffAux2 = psi*cc.K2;
|
|
cc.yffAux3 = psi*wm*cc.K3;
|
|
|
|
// ðåãóëÿòîð Id1
|
|
cc.del = idZ - id1;
|
|
cc.yd1P = cc.del*cc.Kp;
|
|
if ( (cc.y1LimFlag == 0) || (cc.yd1*cc.del < 0) )
|
|
cc.yd1I += cc.del*cc.Ki;
|
|
cc.yd1FF = -iq1*cc.Xyff - cc.yffAux2;
|
|
cc.yd1 = cc.yd1P + cc.yd1I + cc.yd1FF;
|
|
// ðåãóëÿòîð Iq1
|
|
cc.del = iqZ - iq1;
|
|
cc.yq1P = cc.del*cc.Kp;
|
|
if ( (cc.y1LimFlag == 0) || (cc.yq1*cc.del < 0) )
|
|
cc.yq1I += cc.del*cc.Ki;
|
|
cc.yq1FF = id1*cc.Xyff + cc.yffAux3;
|
|
if ( estimate_get_step() == ESTIMATE_TEST_LM ) {
|
|
cc.yd1FF = 0;
|
|
cc.yq1FF = 0;
|
|
}
|
|
cc.yq1 = cc.yq1P + cc.yq1I + cc.yq1FF;
|
|
// îãðàíè÷åíèå
|
|
cc.y1 = sqrt(cc.yd1*cc.yd1 + cc.yq1*cc.yq1);
|
|
if ( cc.y1 > Y_LIM ) {
|
|
cc.kYlim = Y_LIM/cc.y1;
|
|
cc.yd1 *= cc.kYlim;
|
|
cc.yq1 *= cc.kYlim;
|
|
cc.y1LimFlag = 1;
|
|
}
|
|
else {
|
|
cc.y1LimFlag = 0;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
// Ðåãóëèðóåò ïîòîêîñöåïëåíèå
|
|
// (rf.psiZ, psi, rf.pPsiZ, cf.IdLim, cf.IdLimNeg -> idZ)
|
|
void control_flux(void) {
|
|
if ( cf.once == 0 ) {
|
|
cf.once = 1;
|
|
cf.idLimFlag = 0;
|
|
cf.idI = 0;
|
|
idZ = 0;
|
|
}
|
|
|
|
// ðåãóëÿòîð Psi
|
|
cf.del = rf.psiZ - psi;
|
|
cf.idP = cf.del*cf.Kp;
|
|
if ( (cf.idLimFlag == 0) || (idZ*cf.del < 0) )
|
|
cf.idI += cf.del*cf.Ki;
|
|
cf.idFF = (rf.psiZ + rf.pPsiZ*sgmPar.Tr)*sgmPar.LmInv*PSI_BAZ/I_BAZ;
|
|
idZ = cf.idP + cf.idI + cf.idFF;
|
|
// îãðàíè÷åíèå ïîòîêîîáðàçóþùåãî òîêà
|
|
if ( idZ > cf.IdLim ) {
|
|
idZ = cf.IdLim;
|
|
cf.idLimFlag = 1;
|
|
}
|
|
else if ( idZ < cf.IdLimNeg ) {
|
|
idZ = cf.IdLimNeg;
|
|
cf.idLimFlag = 1;
|
|
}
|
|
else {
|
|
cf.idLimFlag = 0;
|
|
}
|
|
} //void control_flux(void)
|
|
|
|
|
|
|
|
// Ðåãóëèðóåò ñêîðîñòü èëè ìîùíîñòü
|
|
// (rs.wmZ, wm, rs.pWmZ, rp.pmZ, mst.wmLim, mst.pmLim, csp.IqLim,
|
|
// csp.IqLimNeg -> iqZ, inuWork)
|
|
void control_speed_power(void) {
|
|
if ( csp.once == 0 ) {
|
|
csp.once = 1;
|
|
csp.wmLimZi = mst.wmLim;
|
|
csp.pmLimZi = mst.pmLim;
|
|
csp.iqLimFlag = 0;
|
|
csp.iqI = 0;
|
|
iqZ = 0;
|
|
csp.iqLimZi = csp.IqLim;
|
|
csp.iqLim = csp.IqLim;
|
|
csp.pmZiRampDown = 0;
|
|
}
|
|
|
|
// äëÿ îãðàíè÷åíèÿ ñêîðîñòè
|
|
if ( mst.wmLim - csp.wmLimZi > rs.WlimIncr ) {
|
|
csp.wmLimZi += rs.WlimIncr;
|
|
}
|
|
else if ( csp.wmLimZi - mst.wmLim > rs.WlimIncr ) {
|
|
csp.wmLimZi -= rs.WlimIncr;
|
|
}
|
|
else {
|
|
csp.wmLimZi = mst.wmLim;
|
|
}
|
|
// äëÿ îãðàíè÷åíèÿ ìîùíîñòè
|
|
if ( mst.pmLim - csp.pmLimZi > rp.PlimIncr ) {
|
|
csp.pmLimZi += rp.PlimIncr;
|
|
}
|
|
else if ( csp.pmLimZi - mst.pmLim > rp.PlimIncr ) {
|
|
csp.pmLimZi -= rp.PlimIncr;
|
|
}
|
|
else {
|
|
csp.pmLimZi = mst.pmLim;
|
|
}
|
|
|
|
if ( inuWork == 0 ) {
|
|
if ( mst.start == 1 ) {
|
|
// ÃÝÄ íàìàãíè÷åí, ìîæíî ïåðåõîäèòü ê îòðàáîòêå N èëè P
|
|
if ( (rf.psiZ > rf.PsiZ*0.97) && (psi > rf.psiZ*0.97) )
|
|
inuWork = 1;
|
|
}
|
|
else {
|
|
// âñ¸ âûêëþ÷àåì
|
|
inuWork = 2;
|
|
}
|
|
// ÷òîáû ñòàðòàíóòü áåç áðîñêîâ òîêà
|
|
rs.wmZi = rs.wmZ = wm;
|
|
rp.pmZi = rp.pmZ = 0;
|
|
iqZ = 0;
|
|
}
|
|
else if ( inuWork == 1 ) {
|
|
if ( mst.start == 1 ) {
|
|
// ðåãóëÿòîð N --------------
|
|
if ( mst.pzMode == 0 ) {
|
|
csp.del = rs.wmZ - wm;
|
|
csp.iqP = csp.del*csp.Kp;
|
|
if ( (csp.iqLimFlag == 0) || (iqZ*csp.del < 0) )
|
|
csp.iqI += csp.del*csp.Ki;
|
|
csp.iqFF = rs.pWmZ/kMe*(WM_BAZ*J/M_BAZ);
|
|
iqZ = csp.iqP + csp.iqI + csp.iqFF;
|
|
// îãðàíè÷åíèå òîêà äëÿ îãðàíè÷åíèÿ ìîùíîñòè
|
|
if ( wmAbs > WM_MIN ) {
|
|
csp.iqLimAux = csp.pmLimZi/(wmAbs*kMe);
|
|
}
|
|
else {
|
|
csp.iqLimAux = csp.pmLimZi/(WM_MIN*kMe);
|
|
}
|
|
if ( csp.iqLimAux < csp.IqLim ) {
|
|
csp.iqLim = csp.iqLimAux;
|
|
}
|
|
else {
|
|
csp.iqLim = csp.IqLim;
|
|
}
|
|
}
|
|
// ðåãóëÿòîð P --------------
|
|
else { //if ( mst.pzMode == 1 )
|
|
if ( wmAbs <= WM_MIN ) {
|
|
iqZ = rp.pmZ/(WM_MIN*kMe);
|
|
}
|
|
else if ( wmAbs <= rf.WmNomPsi ) {
|
|
iqZ = rp.pmZ/(wmAbs*kMe);
|
|
csp.kMeNom = kMe;
|
|
}
|
|
else {
|
|
iqZ = rp.pmZ/(wmAbs*csp.kMeNom);
|
|
}
|
|
// îãðàíè÷åíèå òîêà äëÿ îãðàíè÷åíèÿ îáîðîòîâ
|
|
if ( wmAbs < csp.wmLimZi*0.98 ) {
|
|
csp.iqLimAux = fabs(iqZ);
|
|
}
|
|
else if ( wmAbs > csp.wmLimZi*1.02 ) {
|
|
csp.iqLimAux = 0;
|
|
}
|
|
else {
|
|
csp.iqLimAux = csp.iqLimZi;
|
|
}
|
|
// ... ìåíÿåì ñêîðîñòü èçìåíåíèÿ îãðàíè÷åíèÿ òîêà (?)
|
|
csp.delWmAbs = fabs(wmAbs - csp.wmLimZi);
|
|
if ( csp.delWmAbs > 0.12 )
|
|
csp.KizIncr = 10.0;
|
|
else if ( csp.delWmAbs < 0.02 )
|
|
csp.KizIncr = 0.1;
|
|
else
|
|
csp.KizIncr = 0.1 + (csp.delWmAbs - 0.02)*(10.0 - 0.1)/(0.12 - 0.02);
|
|
// ... ÇÈ
|
|
if ( csp.iqLimAux - csp.iqLimZi > csp.IlimIncr*csp.KizIncr )
|
|
csp.iqLimZi += csp.IlimIncr*csp.KizIncr;
|
|
else if ( csp.iqLimZi - csp.iqLimAux > csp.IlimIncr*csp.KizIncr )
|
|
csp.iqLimZi -= csp.IlimIncr*csp.KizIncr;
|
|
else
|
|
csp.iqLimZi = csp.iqLimAux;
|
|
if ( csp.iqLimZi < csp.IqLim ) {
|
|
csp.iqLim = csp.iqLimZi;
|
|
}
|
|
else {
|
|
csp.iqLim = csp.IqLim;
|
|
}
|
|
} //mst.pzMode
|
|
// äëÿ ïëàâíîé îñòàíîâêè
|
|
csp.pmZiRampDown = rp.pmEqv;
|
|
}
|
|
else { //if ( mst.start == 0 )
|
|
// ñíèæàåì çàäàííóþ ìîùíîñòü
|
|
if ( 0 - csp.pmZiRampDown > mst.pDecrMaxTy ) {
|
|
csp.pmZiRampDown += mst.pDecrMaxTy;
|
|
}
|
|
else if ( csp.pmZiRampDown - 0 > mst.pDecrMaxTy ) {
|
|
csp.pmZiRampDown -= mst.pDecrMaxTy;
|
|
}
|
|
else {
|
|
csp.pmZiRampDown = 0;
|
|
// òîê ñíèæåí - çàâåðøàåì ðàáîòó
|
|
inuWork = 2;
|
|
}
|
|
// ôîðìèðóåì çàäàííûé òîê
|
|
if ( wmAbs > WM_MIN ) {
|
|
iqZ = csp.pmZiRampDown/(wmAbs*kMe);
|
|
}
|
|
else {
|
|
iqZ = csp.pmZiRampDown/(WM_MIN*kMe);
|
|
}
|
|
// íà ñëó÷àé, åñëè mst.start âîññòàíîâèòñÿ ðàíüøå
|
|
// çàâåðøåíèÿ ðàáîòû (inuWork = 2)
|
|
rs.wmZi = rs.wmZ = wm;
|
|
csp.iqI = iqZ;
|
|
rp.pmZi = rp.pmZ = rp.pmEqv;
|
|
} //mst.start
|
|
|
|
// óñòàâêà îãðàíè÷åíèÿ ñíèçó
|
|
if ( -csp.iqLim > csp.IqLimNeg )
|
|
csp.iqLimNeg = -csp.iqLim;
|
|
else
|
|
csp.iqLimNeg = csp.IqLimNeg;
|
|
// îãðàíè÷åíèå ìîìåíòîîáðàçóþùåãî òîêà
|
|
if ( iqZ > csp.iqLim ) {
|
|
iqZ = csp.iqLim;
|
|
csp.iqLimFlag = 1;
|
|
}
|
|
else if ( iqZ < csp.iqLimNeg ) {
|
|
iqZ = csp.iqLimNeg;
|
|
csp.iqLimFlag = 1;
|
|
}
|
|
else {
|
|
csp.iqLimFlag = 0;
|
|
}
|
|
// äëÿ ïëàâíîãî ïåðåõîäà
|
|
if ( mst.pzMode == 0 ) {
|
|
// ... â ðåæèì ðåãóëèðîâàíèÿ P
|
|
rp.pmZ = iqZ*kMe*wmAbs;
|
|
rp.pmZi = rp.pmZ;
|
|
csp.iqLimZi = fabs(iqZ);
|
|
}
|
|
else {
|
|
// ... â ðåæèì ðåãóëèðîâàíèÿ N
|
|
csp.iqI = iqZ;
|
|
csp.iqFF = 0;
|
|
rs.wmZ = wm;
|
|
rs.wmZi = rs.wmZ + csp.iqFF*0.05;
|
|
}
|
|
} //inuWork
|
|
} //void control_speed_power(void)
|
|
|
|
|
|
|
|
// Ðàñ÷¸òû äëÿ indirect vector control
|
|
// (wmNf, wm, ix1, iy1, ix2, iy2 -> ivc.ws, ivc.sinTheta, ivc.cosTheta,
|
|
// ivc.id1, ivc.iq1, ivc.id2, ivc.iq2, ivc.psi)
|
|
void indirect_vector_control(void) {
|
|
static float theta;
|
|
|
|
if ( ivc.once == 0 ) {
|
|
ivc.once = 1;
|
|
ivc.im = 0;
|
|
ivc.iq1 = 0;
|
|
ivc.wr = 0;
|
|
theta = 0;
|
|
}
|
|
|
|
// ÷àñòîòà ðîòîðíîé ÝÄÑ, o.e.
|
|
if ( ivc.im > 4e-3 ) {
|
|
ivc.wr = (ivc.iq1)/ivc.im*sgmPar.TrInv*(1.0/WE_BAZ);
|
|
}
|
|
if ( estimate_get_step() == ESTIMATE_TEST_LM ) {
|
|
ivc.wr = 0;
|
|
ivc.wsNf = 0;
|
|
ivc.ws = 0;
|
|
}
|
|
else {
|
|
ivc.wsNf = wmNf + ivc.wr;
|
|
ivc.ws = wm + ivc.wr;
|
|
theta += ivc.wsNf*WE_BAZ*TY;
|
|
if ( theta > PI2 )
|
|
theta -= PI2;
|
|
else if ( theta < 0 )
|
|
theta += PI2;
|
|
}
|
|
|
|
theta_out = theta;//Âûâîä óãëà òåòòà â Simulink
|
|
|
|
// äëÿ êîîðäèíàòíûõ ïðåîáðàçîâàíèé
|
|
sincos(theta, &ivc.sinTheta, &ivc.cosTheta);
|
|
// park transformations, î.å.
|
|
ivc.id1 = ix1*ivc.cosTheta + iy1*ivc.sinTheta;
|
|
ivc.iq1 = -ix1*ivc.sinTheta + iy1*ivc.cosTheta;
|
|
// òîê íàìàãíè÷èâàíèÿ, o.e.
|
|
ivc.im += (ivc.id1 - ivc.im)*sgmPar.TrInv*TY;
|
|
// àìïëèòóäà ïîòîêà, o.e.
|
|
ivc.psi = ivc.im*sgmPar.Lm*(1.0/L_BAZ);
|
|
} //void indirect_vector_control(void)
|
|
|
|
|
|
|
|
// Ïåðåâîäèò ñèãíàëû óïðàâëåíèÿ èç ñ.ê. d-q â ñ.ê. x-y
|
|
// (cc.yd1, cc.yq1, cc.yd2, cc.yq2, sinTheta, cosTheta, ws ->
|
|
// -> ip.yx1, ip.yy1, ip.yx2, ip.yy2)
|
|
void ipark(void) {
|
|
ip.yx1Aux = cc.yd1*cosTheta - cc.yq1*sinTheta;
|
|
ip.yy1Aux = cc.yd1*sinTheta + cc.yq1*cosTheta;
|
|
// êîððåêöèÿ, ñâÿçàííàÿ ñ äèñêðåòíîñòüþ ÑÓ
|
|
ip.theta = ws*WE_BAZ*TY*1.5;//ðàä.
|
|
sincos(ip.theta, &ip.sinTheta, &ip.cosTheta);
|
|
ip.yx1 = ip.yx1Aux*ip.cosTheta - ip.yy1Aux*ip.sinTheta;
|
|
ip.yy1 = ip.yx1Aux*ip.sinTheta + ip.yy1Aux*ip.cosTheta;
|
|
} //void ipark(void)
|
|
|
|
|
|
|
|
// Îãðàíè÷èâàåò ïîëíûé òîê
|
|
// (idZ, iqZ, IzLim -> idZ, iqZ, csp.iqLimFlag)
|
|
void limit_current(void) {
|
|
iZ = sqrt(idZ*idZ + iqZ*iqZ);
|
|
if ( iZ > IzLim ) {
|
|
if ( iqZ >= 0 ) {
|
|
iqZ = sqrt(IzLim*IzLim - idZ*idZ);
|
|
}
|
|
else {
|
|
iqZ = -sqrt(IzLim*IzLim - idZ*idZ);
|
|
}
|
|
csp.iqLimFlag = 1;
|
|
}
|
|
} //void limit_current(void)
|
|
|
|
|
|
|
|
// ØÈÌ
|
|
// (ip.yx1, ip.yy1, ip.yx2, ip.yy2 ->
|
|
// -> EPwm1Regs.CMPA.half.CMPA, EPwm2Regs.CMPA.half.CMPA,
|
|
// EPwm3Regs.CMPA.half.CMPA, EPwm4Regs.CMPA.half.CMPA,
|
|
// EPwm5Regs.CMPA.half.CMPA, EPwm6Regs.CMPA.half.CMPA,
|
|
// EPwm7Regs.CMPA.half.CMPA, EPwm8Regs.CMPA.half.CMPA,
|
|
// EPwm9Regs.CMPA.half.CMPA, EPwm10Regs.CMPA.half.CMPA,
|
|
// EPwm11Regs.CMPA.half.CMPA, EPwm12Regs.CMPA.half.CMPA)
|
|
void pwm(float ya, float yb, float yc) {
|
|
static float yPredm = 0;
|
|
static float yaPredm;
|
|
static float ybPredm;
|
|
static float ycPredm;
|
|
|
|
// ïðåäìîäóëèðóþùåå âîçäåéñòâèå
|
|
if ((ya >= yb) && (ya <= yc)) {
|
|
yPredm = ya*0.5;
|
|
}
|
|
else if ((yc >= yb) && (yc <= ya)) {
|
|
yPredm = yc*0.5;
|
|
}
|
|
else if ((yb >= yc) && (yb <= ya)) {
|
|
yPredm = yb*0.5;
|
|
}
|
|
else if ((ya >= yc) && (ya <= yb)) {
|
|
yPredm = ya*0.5;
|
|
}
|
|
else if ((yc >= ya) && (yc <= yb)) {
|
|
yPredm = yc*0.5;
|
|
}
|
|
else if ((yb >= ya) && (yb <= yc)) {
|
|
yPredm = yb*0.5;
|
|
}
|
|
yaPredm = (ya + yPredm)*2.;
|
|
ybPredm = (yb + yPredm)*2.;
|
|
ycPredm = (yc + yPredm)*2.;
|
|
// full compare unit compare registers
|
|
if (yaPredm >= 0) {
|
|
EPwm1Regs.CMPA.half.CMPA = (unsigned short)yaPredm;
|
|
EPwm2Regs.CMPA.half.CMPA = 0;
|
|
}
|
|
else {
|
|
EPwm1Regs.CMPA.half.CMPA = 0;
|
|
EPwm2Regs.CMPA.half.CMPA = (unsigned short)(-yaPredm);
|
|
}
|
|
if (ybPredm >= 0) {
|
|
EPwm3Regs.CMPA.half.CMPA = (unsigned short)ybPredm;
|
|
EPwm4Regs.CMPA.half.CMPA = 0;
|
|
}
|
|
else {
|
|
EPwm3Regs.CMPA.half.CMPA = 0;
|
|
EPwm4Regs.CMPA.half.CMPA = (unsigned short)(-ybPredm);
|
|
}
|
|
if (ycPredm >= 0) {
|
|
EPwm5Regs.CMPA.half.CMPA = (unsigned short)ycPredm;
|
|
EPwm6Regs.CMPA.half.CMPA = 0;
|
|
}
|
|
else {
|
|
EPwm5Regs.CMPA.half.CMPA = 0;
|
|
EPwm6Regs.CMPA.half.CMPA = (unsigned short)(-ycPredm);
|
|
}
|
|
|
|
|
|
|
|
// ðàçðåøàåì èìïóëüñû
|
|
EALLOW;
|
|
EPwm1Regs.TZCLR.all = 0x0004;
|
|
EPwm2Regs.TZCLR.all = 0x0004;
|
|
EPwm3Regs.TZCLR.all = 0x0004;
|
|
EPwm4Regs.TZCLR.all = 0x0004;
|
|
EPwm5Regs.TZCLR.all = 0x0004;
|
|
EPwm6Regs.TZCLR.all = 0x0004;
|
|
} //void pwm(void)
|
|
|
|
|
|
|
|
// Ôîðìèðóåò çàäàííûé ïîòîê
|
|
// (rf.PsiZ -> rf.psiZ, rf.pPsiZ)
|
|
void reference_flux(void) {
|
|
if ( rf.once == 0 ) {
|
|
rf.once = 1;
|
|
rf.KpsiSub = TY*DECIM_PSI_WM_PM/6.0;
|
|
rf.psiZi = 0;
|
|
cc.y1 = 0;
|
|
rf.psiSub = 0;
|
|
rf.psiZ = 0;
|
|
rf.psiZPrev1 = 0;
|
|
rf.psiZPrev2 = 0;
|
|
rf.psiZPrev3 = 0;
|
|
}
|
|
|
|
// ÇÈ
|
|
if ( rf.PsiZ - rf.psiZi > rf.PsizIncr ) {
|
|
rf.psiZi += rf.PsizIncr;
|
|
}
|
|
else if ( rf.psiZi - rf.PsiZ > rf.PsizIncr ) {
|
|
rf.psiZi -= rf.PsizIncr;
|
|
}
|
|
else {
|
|
rf.psiZi = rf.PsiZ;
|
|
}
|
|
// êîððåêöèÿ â ñîîòâåòñòâèè ñî ñêîðîñòüþ
|
|
if ( wmAbs <= rf.WmNomPsi )
|
|
rf.psiZCorr = rf.psiZi;
|
|
else
|
|
rf.psiZCorr = rf.psiZi*rf.WmNomPsi/wmAbs;
|
|
// êîððåêöèÿ â ñîîòâåòñòâèè ñ ïðîòèâîÝÄÑ
|
|
if (cc.y1 > rf.YlimPsi) {
|
|
rf.psiSub += (rf.psiZCorr - rf.psiSub)*rf.KpsiSub;
|
|
}
|
|
else {
|
|
rf.psiSub += (0 - rf.psiSub)*rf.KpsiSub;
|
|
}
|
|
rf.psiZCorr2 = rf.psiZCorr - rf.psiSub;
|
|
// ÷òîáû çàäàíèå ìåíÿëîñü ÷óòü ïëàâíåå
|
|
rf.psiZ += (rf.psiZCorr2 - rf.psiZ)*rf.Kpsiz;
|
|
|
|
// ïðîèçâîäíàÿ çàäàííîãî ïîòîêîñöåïëåíèÿ
|
|
rf.pPsiZ = (rf.psiZ - rf.psiZPrev3)/(TY*DECIM_PSI_WM_PM*3.);
|
|
rf.psiZPrev3 = rf.psiZPrev2;
|
|
rf.psiZPrev2 = rf.psiZPrev1;
|
|
rf.psiZPrev1 = rf.psiZ;
|
|
} //void reference_flux(void)
|
|
|
|
|
|
|
|
// Ôîðìèðóåò çàäàííóþ ìîùíîñòü
|
|
// (mst.pmZz, mst.pmLim -> rp.pmZ)
|
|
void reference_power(void) {
|
|
if ( rp.once == 0 ) {
|
|
rp.once = 1;
|
|
rp.pmZi = 0;
|
|
rp.pmZ = 0;
|
|
}
|
|
|
|
// îãðàíè÷åíèå
|
|
if ( fabs(mst.pmZz) > mst.pmLim ) {
|
|
if ( mst.pmZz >= 0 )
|
|
rp.pmZz = mst.pmLim;
|
|
else
|
|
rp.pmZz = -mst.pmLim;
|
|
}
|
|
else {
|
|
rp.pmZz = mst.pmZz;
|
|
}
|
|
// äëÿ îãðàíè÷åíèÿ ïðèðàùåíèÿ ìîùíîñòè (?)
|
|
if ( fabs(rp.pmZi - rp.pmEqv) > 0.02 )
|
|
rp.KpIncrDecr = 0.10;
|
|
else
|
|
rp.KpIncrDecr = 1.00;
|
|
// ÇÈ
|
|
if ( rp.pmZz - rp.pmZi > mst.pIncrMaxTy*rp.KpIncrDecr ) {
|
|
rp.pmZi += mst.pIncrMaxTy*rp.KpIncrDecr;
|
|
}
|
|
else if ( rp.pmZi - rp.pmZz > mst.pDecrMaxTy*rp.KpIncrDecr ) {
|
|
rp.pmZi -= mst.pDecrMaxTy*rp.KpIncrDecr;
|
|
}
|
|
else {
|
|
rp.pmZi = rp.pmZz;
|
|
}
|
|
// ÷òîáû çàäàíèå ìåíÿëîñü ÷óòü ïëàâíåå
|
|
rp.pmZ += (rp.pmZi - rp.pmZ)*rp.Kpmz;
|
|
} //void reference_power(void)
|
|
|
|
|
|
|
|
// Ôîðìèðóåò çàäàííóþ ñêîðîñòü
|
|
// (mst.wmZz, mst.wmLim -> rs.wmZ, rs.pWmZ)
|
|
void reference_speed(void) {
|
|
if ( rs.once == 0 ) {
|
|
rs.once = 1;
|
|
rs.wmZi = rs.wmZ = wm;
|
|
rs.wzIncr = rs.WlimIncr;
|
|
rs.wmZPrev1 = rs.wmZ;
|
|
rs.wmZPrev2 = rs.wmZ;
|
|
rs.wmZPrev3 = rs.wmZ;
|
|
rs.tPwmZ = 0;
|
|
}
|
|
|
|
// îãðàíè÷åíèå
|
|
if ( fabs(mst.wmZz) > mst.wmLim ) {
|
|
if ( mst.wmZz >= 0 )
|
|
rs.wmZz = mst.wmLim;
|
|
else
|
|
rs.wmZz = -mst.wmLim;
|
|
}
|
|
else {
|
|
rs.wmZz = mst.wmZz;
|
|
}
|
|
// äëÿ îãðàíè÷åíèÿ ïðèðàùåíèÿ ìîùíîñòè (?)
|
|
if ( fabs(rs.wmZi) < 0.5 )
|
|
rs.wzIncrNf = rs.WlimIncr*3.5;
|
|
else if ( fabs(rs.wmZi) < 0.8 )
|
|
rs.wzIncrNf = rs.WlimIncr*2.0;
|
|
else
|
|
rs.wzIncrNf = rs.WlimIncr;
|
|
rs.wzIncr += (rs.wzIncrNf - rs.wzIncr)*(TY*DECIM_PSI_WM_PM)/0.25;
|
|
// ÇÈ
|
|
if ( rs.wmZz - rs.wmZi > rs.wzIncr ) {
|
|
rs.wmZi += rs.wzIncr;
|
|
}
|
|
else if ( rs.wmZi - rs.wmZz > rs.wzIncr ) {
|
|
rs.wmZi -= rs.wzIncr;
|
|
}
|
|
else {
|
|
rs.wmZi = rs.wmZz;
|
|
}
|
|
// ÷òîáû çàäàíèå ìåíÿëîñü ÷óòü ïëàâíåå
|
|
rs.wmZ += (rs.wmZi - rs.wmZ)*rs.Kwmz;
|
|
|
|
// ïðîèçâîäíàÿ çàäàííîé ñêîðîñòè
|
|
rs.pWmZ = (rs.wmZ - rs.wmZPrev3)/(TY*DECIM_PSI_WM_PM*3.);
|
|
rs.wmZPrev3 = rs.wmZPrev2;
|
|
rs.wmZPrev2 = rs.wmZPrev1;
|
|
rs.wmZPrev1 = rs.wmZ;
|
|
// ... ÷òîáû èçáåæàòü áðîñêîâ ïðè âõîäå â ðàáî÷èé ðåæèì
|
|
if ( (inuWork == 0) || (mst.start == 0) || (mst.pzMode == 1) )
|
|
rs.tPwmZ = 0;
|
|
if ( rs.tPwmZ <= 3 ) {
|
|
rs.tPwmZ++;
|
|
rs.pWmZ = 0;
|
|
}
|
|
} //void reference_speed(void)
|
|
|
|
|
|
|
|
// Âûáèðàåò î.ñ.
|
|
// (... -> ws, sinTheta, cosTheta, id1, iq1, id2, iq2, psi)
|
|
void select_feedback(void) {
|
|
ws = ivc.ws;
|
|
sinTheta = ivc.sinTheta;
|
|
cosTheta = ivc.cosTheta;
|
|
id1 = ivc.id1;
|
|
iq1 = ivc.iq1;
|
|
psi = ivc.psi;
|
|
} //void select_feedback(void)
|