/* * vector.c * * Created on: 20 ����. 2023 �. * Author: seklyuts * * Это проект epwm_test_motor_cpu2 * дополнение к EFC_Application (на CPU2) */ #include "f28x_project.h" #include "pwm_interrupts.h" #include "pwm_init.h" #include "pi.h" #include "vector.h" #include "iqmath.h" #include "sdfm.h" #include "driverlib.h" #include "device.h" //#include "board.h" #define IMAX_A 25.0 //A #define IMAX (IMAX_A*BIT_MAX/FACTOR_CURRENT_MOTOR_A) typedef struct { int16_t Value16in; // int16_t Value16filtered; // int32_t Value32; // int16_t ShiftFilter; // Filters } Filtered; Filtered UdcFilter = {0,0,0,10}; int16_t Filter(int16_t inValue, Filtered *Struct); const int16_t mcgenSineTable256[257] = \ { 0,-201,-402,-603,-804,-1005,-1206,-1407,-1607,-1808, -2009,-2210,-2410,-2611,-2811,-3011,-3211,-3411,-3611,-3811, -4011,-4210,-4409,-4609,-4808,-5006,-5205,-5403,-5602,-5800, -5997,-6195,-6392,-6589,-6786,-6983,-7179,-7375,-7571,-7766, -7961,-8156,-8351,-8545,-8739,-8933,-9126,-9319,-9512,-9704, -9896,-10087,-10278,-10469,-10659,-10849,-11039,-11228,-11416,-11605, -11793,-11980,-12167,-12353,-12539,-12725,-12910,-13094,-13278,-13462, -13645,-13828,-14010,-14191,-14372,-14552,-14732,-14912,-15090,-15269, -15446,-15623,-15800,-15976,-16151,-16325,-16499,-16673,-16846,-17018, -17189,-17360,-17530,-17700,-17869,-18037,-18204,-18371,-18537,-18703, -18868,-19032,-19195,-19358,-19519,-19681,-19841,-20001,-20159,-20318, -20475,-20631,-20787,-20942,-21097,-21250,-21403,-21555,-21706,-21856, -22005,-22154,-22301,-22448,-22594,-22740,-22884,-23027,-23170,-23312, -23453,-23593,-23732,-23870,-24007,-24144,-24279,-24414,-24547,-24680, -24812,-24943,-25073,-25201,-25330,-25457,-25583,-25708,-25832,-25955, -26077,-26199,-26319,-26438,-26557,-26674,-26790,-26905,-27020,-27133, -27245,-27356,-27466,-27576,-27684,-27791,-27897,-28002,-28106,-28208, -28310,-28411,-28511,-28609,-28707,-28803,-28898,-28993,-29086,-29178, -29269,-29359,-29447,-29535,-29621,-29707,-29791,-29874,-29956,-30037, -30117,-30196,-30273,-30350,-30425,-30499,-30572,-30644,-30714,-30784, -30852,-30919,-30985,-31050,-31114,-31176,-31237,-31298,-31357,-31414, -31471,-31526,-31581,-31634,-31685,-31736,-31785,-31834,-31881,-31927, -31971,-32015,-32057,-32098,-32138,-32176,-32214,-32250,-32285,-32319, -32351,-32383,-32413,-32442,-32469,-32496,-32521,-32545,-32568,-32589, -32610,-32629,-32647,-32663,-32679,-32693,-32706,-32718,-32728,-32737, -32745,-32752,-32758,-32762,-32765,-32767,-32767 }; int16_t Ia,Ib,Ic; Tvector2dq vectorUdq, vectorIdq; Tvector2ph vectorU2ph, vectorI2ph; Tvector3abc vectorUabc, vectorIabc; TvectorSinCos vectorSinCos; uint16_t VecSector, OverCur[3] = {0,0,0}, ErrCur=0, CurFault = 0; float FactorCurrent = FACTOR_CURRENT_MOTOR; TvectorCurrentLoop CurrLoop; TMode Mode = OffMode; ///< да хер вам! uint16_t Step = 0, StepS, NoLoop = 0, StepShift = 6; ///< начальные значения int16_t Nstep = 0; float CurrentRegTuneAmpl = 0, FRM_cur = 0; int16_t Angle_test=0,sin_int,cos_int, AngleErr; float Udc; uint16_t SectorCheckOn=1; volatile int16_t UdcFiltered = 0; float UdcVolt = 0; volatile float VdcFactor = FACTOR_VDC; volatile uint16_t VoltProcImit = 0; float Test1, Test2; float FRM_Ud, FRM_Uq; void vectorFault(void) { Mode = OffMode; } /* * Расчет шагов в автономном тесте в локали * globals: Step */ void testStep() { // Автономная работа, прямо здесь. :( // Разгоняемся, едем, тормозим, ждём, реверсим, едем, тормозим, ждем.... enum Pstate {Rzg, Ttim, Torm, Pauss, Pauss2, Revers}; static uint16_t razg = 0; // для контроля разгона/торможения int16_t razgD = 88; // шаг разгона int16_t NstepMax = 180; // считаем это Макс скоростью в тесте int16_t NstepMax2 = -180; // считаем это Макс скоростью в тесте static uint32_t RolTime = 1; uint32_t RolTimeMax = 40000; // время вращения uint32_t RolTimeMax2 = 10000; // CurrLoop.piId.Ref = 2.55; // старое = 0; static enum Pstate cur_state = Rzg; static int rever = 0; // индикатор реверса // Работа: // Rzg - Ttim - Torm - Pauss - Revers - // // Turn on LED // GPIO_writePin(32U, 1); switch(cur_state){ case Rzg: ///< разгон // Gpio_rainbow(2); GPIO_writePin(34U, 0); if(Nstep < NstepMax){ if(razg <= razgD) razg++; else{ razg = 0; Nstep++; } } else cur_state = Ttim; break; case Ttim: ///< время работы/пауза RolTime++; if(RolTime > RolTimeMax){ if(rever) {rever = 0; cur_state = Rzg;} else cur_state = Torm; RolTime = 0;} // DEVICE_DELAY_US(5000000); // мкс 5сек break; case Torm: ///< торможение if(Nstep > 0) { if(razg <= razgD) razg++; else{ razg = 0; Nstep--; } } else{ cur_state = Pauss; Nstep = 0; } break; case Pauss: ///< пауза после торможения if(RolTime++ > RolTimeMax2){ cur_state = Revers; razg = 0; } break; case Pauss2: ///< пауза после реверса if(RolTime++ > RolTimeMax){ cur_state = Rzg; razg = 0; } break; case Revers: ///< разгоняемся назад if(Nstep > NstepMax2){ if(razg <= razgD) razg++; else{ razg = 0; Nstep--; } } else {cur_state = Ttim; RolTime = 0; rever = 1;} break; default: break; } } /* * Управление вектором * AMG: добавил тетстовые штуки для автономной работы * целевые параметры: * CurrLoop.piId.Ref = 2.55; * Nstep = 100; * */ void vectorControl(int16_t CurrentA, int16_t CurrentB, int16_t CurrentC, int16_t sdfmUdc) { pwm_clr_PwmFlagStartADC(); Ia = -CurrentA;//sdfm_get(6); Ib = -CurrentB;//sdfm_get(3); Ic = -CurrentC;//sdfm_get(4); if((Ia > IMAX)||(Ib>IMAX)||(Ic>IMAX)||(Ia<-IMAX)||(Ib<-IMAX)||(Ic<-IMAX) ) {OverCur[0] = Ia; OverCur[1] = Ib; OverCur[2] = Ic; ErrCur++; if(ErrCur >= 3) {Mode = OffMode; CurFault++;}} else ErrCur = 0; UdcFiltered = Filter(sdfmUdc, &UdcFilter); if(UdcFiltered < 0) UdcFiltered = 0; if(VoltProcImit) UdcVolt = VoltProcImit; else UdcVolt = (float)UdcFiltered*VdcFactor/32768; testStep(); Mode = StepMode; Step = Step + Nstep; CurrLoop.piId.Ref = 2.55; /** поехали!! */ switch(Mode) { case OffMode: PWM_ABC_StopAllClose(); vectorResCurrLoop(); break; case StepMode: PWM_ABC_StartOut(); vectorSinCos.Angle = Step; CurrLoop.piIq.Ref = CurrentRegTuneAmpl; break; case StayMode: PWM_ABC_StartOut(); break; case CurrentRegTune: vectorSinCos.Angle = 0; PWM_ABC_StartOut(); StepS = Step&((1< (1<<(StepShift-1))) CurrLoop.piId.Ref = 0; else CurrLoop.piId.Ref = CurrentRegTuneAmpl; break; default: Mode = OffMode; PWM_ABC_StopAllClose(); } Udc = sdfmUdc * FACTOR_VDC; vector_klark_park(SectorCheckOn,Ia,Ib,Ic); vector_inversion(); // BissStartSet(); // FMSTREnableSet(); // AdcStartSet(); } void vectorInitCurrLoop(void) { CurrLoop.CurrentLimit = CURRENT_MAX; CurrLoop.piId.Ref = 0; // Input: reference set-point CurrLoop.piId.Fbk = 0; // Input: feedback CurrLoop.piId.uCorr = 0; // Input: ��������� ������, ��� ���������� ����������� ������ CurrLoop.piId.Out = 0; // Output: controller output CurrLoop.piId.Kp = PI_REG_I_PROPOR; // Parameter: proportional loop gain CurrLoop.piId.Ki = PI_REG_I_INTEGR; // Parameter: integral gain CurrLoop.piId.Umax = VOLT_MAX_PROC; // Parameter: upper saturation limit CurrLoop.piId.Umin = -VOLT_MAX_PROC; // Parameter: lower saturation limit CurrLoop.piId.up = 0; // Data: proportional term CurrLoop.piId.ui = 0; // Data: integral term CurrLoop.piId.v1 = 0; // Data: pre-saturated controller output CurrLoop.piId.i1 = 0; // Data: integrator storage: ui(k-1) CurrLoop.piIq.Ref = 0; // Input: reference set-point CurrLoop.piIq.Fbk = 0; // Input: feedback CurrLoop.piIq.uCorr = 0; // Input: ��������� ������, ��� ���������� ����������� ������ CurrLoop.piIq.Out = 0; // Output: controller output CurrLoop.piIq.Kp = PI_REG_I_PROPOR; // Parameter: proportional loop gain CurrLoop.piIq.Ki = PI_REG_I_INTEGR; // Parameter: integral gain CurrLoop.piIq.Umax = VOLT_MAX_PROC; // Parameter: upper saturation limit CurrLoop.piIq.Umin = -VOLT_MAX_PROC; // Parameter: lower saturation limit CurrLoop.piIq.up = 0; // Data: proportional term CurrLoop.piIq.ui = 0; // Data: integral term CurrLoop.piIq.v1 = 0; // Data: pre-saturated controller output CurrLoop.piIq.i1 = 0; // Data: integrator storage: ui(k-1) } void vectorResCurrLoop(void) { CurrLoop.piId.Ref = 0; // Input: reference set-point CurrLoop.piId.uCorr = 0; // Input: ��������� ������, ��� ���������� ����������� ������ CurrLoop.piId.Out = 0; // Output: controller output CurrLoop.piId.v1 = 0; // Data: pre-saturated controller output CurrLoop.piId.i1 = 0; // Data: integrator storage: ui(k-1) CurrLoop.piIq.Ref = 0; // Input: reference set-point CurrLoop.piIq.uCorr = 0; // Input: ��������� ������, ��� ���������� ����������� ������ CurrLoop.piIq.Out = 0; // Output: controller output CurrLoop.piIq.v1 = 0; // Data: pre-saturated controller output CurrLoop.piIq.i1 = 0; // Data: integrator storage: ui(k-1) } int16_t vector_mcsinPIxLUT(int16_t Value, int16_t *psinTable) { // set saturation on if (Value>=0) { return ( Value > ANGLE_PI_DIVIDE_2_F16 ? -psinTable[(2*SIN_LENGTH_TABLE- (Value>> SIN_INDEX_SHIFT))] : -psinTable[(Value >> SIN_INDEX_SHIFT)] ); } else { return ( Value < -ANGLE_PI_DIVIDE_2_F16 ? psinTable[(2*SIN_LENGTH_TABLE- -(Value >> SIN_INDEX_SHIFT))] : psinTable[(-(Value >> SIN_INDEX_SHIFT))] ); } } void vector_inversion(void) { float temp1, temp2, temp3; vectorU2ph.Alfa = _IQmpy(vectorUdq.d,vectorSinCos.cos) - _IQmpy(vectorUdq.q,vectorSinCos.sin); vectorU2ph.Beta = _IQmpy(vectorUdq.q,vectorSinCos.cos) + _IQmpy(vectorUdq.d,vectorSinCos.sin); temp1= vectorU2ph.Beta; temp2= _IQdiv2(vectorU2ph.Beta) + (_IQmpy(_IQ(0.866),vectorU2ph.Alfa)); temp3= temp2 - temp1; VecSector=3; VecSector=(temp2> 0)?( VecSector-1):VecSector; VecSector=(temp3> 0)?( VecSector-1):VecSector; VecSector=(temp1< 0)?(7-VecSector) :VecSector; if (VecSector==1 || VecSector==4) { vectorUabc.a= temp2; vectorUabc.b= temp1-temp3; vectorUabc.c=-temp2; } else if(VecSector==2 || VecSector==5) { vectorUabc.a= temp3+temp2; vectorUabc.b= temp1; vectorUabc.c=-temp1; } else { vectorUabc.a= temp3; vectorUabc.b=-temp3; vectorUabc.c=-(temp1+temp2); } pwm_set_volt_3F(vectorUabc.a,vectorUabc.b,vectorUabc.c, UdcVolt); } float Test1, Test2; float FRM_Ud, FRM_Uq; void vector_klark_park(uint16_t SectorOn, int16_t CurrentA, int16_t CurrentB, int16_t CurrentC) { /* if (Inputs->UpdateUdc) { //���������� Udc ��� ������� ����(����������� Umax � Umin � ��-����������) Outputs->Udc = PmsmVect.UdcFilter.Output; //������ �� ������� �� 0 � ������������� ����� if (Outputs->Udc <= 0.0f) Outputs->Udc = VOLTAGE_UDC; //������ ������������ ��� ������� ���������� ���(���� ��� �����, ����� ������ ���� ��� ��������, ��� �����������) PmsmVect.UdcPwmFactor = 1.0f/Outputs->Udc; //����� ����� Inputs->UpdateUdc = false; }*/ if (SectorOn ) { switch (VecSector) { case 2: case 3: vectorIabc.a = (float)CurrentA * FactorCurrent; vectorIabc.c = (float)CurrentC * FactorCurrent; vectorIabc.b = - vectorIabc.a - vectorIabc.c; break; case 4: case 5: vectorIabc.a = (float)CurrentA * FactorCurrent; vectorIabc.b = (float)CurrentB * FactorCurrent; vectorIabc.c = - vectorIabc.a - vectorIabc.b; break; case 1: case 6: default: vectorIabc.b = (float)CurrentB * FactorCurrent; vectorIabc.c = (float)CurrentC * FactorCurrent; vectorIabc.a = - vectorIabc.b - vectorIabc.c; break; } } else { vectorIabc.a = (float)CurrentA * FactorCurrent; vectorIabc.b = (float)CurrentB * FactorCurrent; vectorIabc.c = (float)CurrentC * FactorCurrent; } // �������������� ����� vectorI2ph.Alfa = vectorIabc.a; vectorI2ph.Beta = _IQmpy((vectorIabc.a +_IQmpy2(vectorIabc.b)),_IQ(ONEbySQRT3)); //vectorI2ph.Beta = _IQmpy((vectorIabc.b - vectorIabc.c),_IQ(ONEbySQRT3)); sin_int = vector_mcsinPIxLUT(vectorSinCos.Angle, (int16_t *)&mcgenSineTable256); cos_int = vector_mcsinPIxLUT((vectorSinCos.Angle + ANGLE_PI_DIVIDE_2_F16),(int16_t *)&mcgenSineTable256); vectorSinCos.sin = ((float)sin_int)/32768.0; vectorSinCos.cos = ((float)cos_int)/32768.0; // �������������� ����� vectorIdq.d = _IQmpy(vectorI2ph.Alfa,vectorSinCos.cos) + _IQmpy(vectorI2ph.Beta,vectorSinCos.sin); vectorIdq.q = _IQmpy(vectorI2ph.Beta,vectorSinCos.cos) - _IQmpy(vectorI2ph.Alfa,vectorSinCos.sin); // Angle_test = Atan(sin_int, cos_int); // AngleErr = Angle_test - vectorSinCos.Angle; // CurrLoop.piIq.Ref = Inputs->IqRef;//�������� if (FABS(CurrLoop.piIq.Ref) > CurrLoop.CurrentLimit) { (CurrLoop.piIq.Ref >= 0.0f) ? ( CurrLoop.piIq.Ref = CurrLoop.CurrentLimit ) : ( CurrLoop.piIq.Ref = -CurrLoop.CurrentLimit); } CurrLoop.piIq.Fbk = vectorIdq.q; // CurrLoop.piId.Ref = 0; CurrLoop.piId.Fbk = vectorIdq.d; // CurrLoop.piId.Umax = CurrLoop.piIq.Umax = Inputs->Udc; // CurrLoop.piId.Umin = CurrLoop.piIq.Umin = -CurrLoop.piIq.Umax; #ifdef UCORR_ENABLE //������������ ����� float Velectr = VFbk * CurrLoop.VelElectrFactor; CurrLoop.piId.uCorr = Velectr * CurrLoop.PhaseInduct * CurrLoop.piIq.Fbk; CurrLoop.piIq.uCorr = -Velectr * (CurrLoop.FluxLinkage + CurrLoop.PhaseInduct * CurrLoop.piId.Fbk); #else CurrLoop.piId.uCorr = CurrLoop.piIq.uCorr = 0.0f; #endif float UmaxVolt = VOLT_MAX_FACTOR * UdcVolt; CurrLoop.piIq.Umax = UmaxVolt; // Parameter: upper saturation limit CurrLoop.piIq.Umin = -UmaxVolt; CurrLoop.piId.Umax = UmaxVolt; // Parameter: upper saturation limit CurrLoop.piId.Umin = -UmaxVolt; PI_MACRO(CurrLoop.piIq); PI_MACRO(CurrLoop.piId); /*���������������� ����������� ���������� Uq � Ud*/ // Test1 = my_sqrtf(Test2); float Ulim = my_sqrtf(CurrLoop.piId.Out * CurrLoop.piId.Out + CurrLoop.piIq.Out * CurrLoop.piIq.Out); if (Ulim > UmaxVolt)// ������������ ���������� � ������� { if(Ulim > ZERO_LVL) { float K = UmaxVolt / Ulim; FRM_Ud = CurrLoop.piId.Out *= K; FRM_Uq = CurrLoop.piIq.Out *= K; } } if(!NoLoop) { vectorUdq.q = CurrLoop.piIq.Out; vectorUdq.d = CurrLoop.piId.Out; } } float my_sqrtf(float x) { if (x == 0) return x; /* * We'll compute 1/sqrt(x) using the Newton-Raphson method from an * initial approximation using EISQRTF32; this instruction gives * 1/sqrtf(x) "accurate to approximately 8 bits" (SPRUEO2A). The * number of iterations used here may not be optimal for this * instruction. * * n.b. __eisqrtf32 cannot generate -0, NaN, or denormal values * * n.b. this loop is divergent for very small x */ float x0 = __eisqrtf32(x); int i; for (i = 0; i < 3; i++) x0 *= (1.5f - x * 0.5f * x0 * x0); /* now sqrt(x) = x * 1/sqrt(x) */ return x * x0; } // ������ ������� int16_t Filter(int16_t inValue, Filtered *Struct) { (*Struct).Value16in = inValue; (*Struct).Value32+=(int32_t)((*Struct).Value16in)-(int32_t)((*Struct).Value16filtered); (*Struct).Value16filtered =(int16_t)(((*Struct).Value32)>>((*Struct).ShiftFilter)); return ((*Struct).Value16filtered); }