/* * vector_control.c * * Created on: 16 нояб. 2020 г. * Author: star */ #include "IQmathLib.h" #include "vector_control.h" #include #include #include #include #include #include #include "math.h" #include "mathlib.h" #include "filter_v1.h" #include "abc_to_dq.h" #include "regul_power.h" #include "regul_turns.h" #include "teta_calc.h" //#define CALC_TWO_WINDINGS #define I_ZERO_LEVEL_IQ 27962 // 111848 ~ 20A //279620 ~ 50A //55924 ~ 10A #pragma DATA_SECTION(vect_control,".fast_vars"); VECTOR_CONTROL vect_control = VECTOR_CONTROL_DEFAULTS; void Idq_to_Udq_2_windings(_iq Id_zad, _iq Iq_zad, _iq Id_measured1, _iq Iq_measured1, _iq* Ud_zad1, _iq* Uq_zad1, _iq Id_measured2, _iq Iq_measured2, _iq* Ud_zad2, _iq* Uq_zad2, int reset); void analog_Udq_calc(_iq Ud1, _iq Uq1, _iq Ud2, _iq Uq2); void analog_dq_calc(_iq winding_displacement); _iq calcId(_iq Iq_limit, _iq Iq, int reset, int prepare_stop_PWM); inline void calcUdUqCompensation(_iq Frot); void initVectorControl() { vect_control.pidD1.Kp = _IQ(0.3);//_IQ(0.2); vect_control.pidD1.Ki = _IQ(0.01);//_IQ(0.9); vect_control.pidD1.OutMax = _IQ(0.9); vect_control.pidD1.OutMin = -_IQ(0.9); vect_control.pidQ1.Kp = _IQ(0.3); vect_control.pidQ1.Ki = _IQ(0.01); vect_control.pidQ1.OutMax = _IQ(0.9); vect_control.pidQ1.OutMin = -_IQ(0.9); vect_control.pidD2.Kp = _IQ(0.3); vect_control.pidD2.Ki = _IQ(0.3); vect_control.pidD2.OutMax = _IQ(0.9); vect_control.pidD2.OutMin = -_IQ(0.9); vect_control.pidQ2.Kp = _IQ(0.3); vect_control.pidQ2.Ki = _IQ(0.3); vect_control.pidQ2.OutMax = _IQ(0.9); vect_control.pidQ2.OutMin = -_IQ(0.9); // vect_control.iqId_nominal = _IQ(MOTOR_CURRENT_NOMINAL * sqrtf(1 - COS_FI * COS_FI) / NORMA_ACP); vect_control.iqId_nominal = _IQ(MOTOR_CURRENT_NOMINAL * 0.4 / NORMA_ACP); vect_control.iqId_min = _IQ(200 / NORMA_ACP); vect_control.iqId_start = _IQ(200.0 / NORMA_ACP); vect_control.koef_rmp_Id = (_iq)(vect_control.iqId_nominal / FREQ_PWM); vect_control.koef_filt_I = _IQ(0.5); vect_control.koef_Ud_comp = _IQ((L_SIGMA_S + L_M * L_SIGMA_R / (L_M + L_SIGMA_R)) * 2 * 3.14 * NORMA_FROTOR); //Lsigm_s + Lm*Lsigm_r / (Lm + Lsigm_r) vect_control.koef_Uq_comp = _IQ((L_M + L_SIGMA_S) * 2 * 3.14 * NORMA_FROTOR); //Lm + Lsigm_s // vect_control.koef_Ud_comp = _IQ(0.0002369 * 2 * 3.14 * NORMA_FROTOR); //Lsigm_s + Lm*Lsigm_r / (Lm + Lsigm_r) // vect_control.koef_Uq_comp = _IQ(0.0043567 * 2 * 3.14 * NORMA_FROTOR); //Lm + Lsigm_s vect_control.koef_zero_Uzad = _IQ(0.993); //_IQ(0.993); //_IQ(0.03); init_Pvect(); init_Fvect(); init_teta_calc_struct(); } void vectorControlConstId (_iq Pzad, _iq Fzad, int direction, _iq Frot, int n_alg, unsigned int master, _iq mzz_zad, _iq winding_displacement, _iq theta_from_master, _iq Iq_from_master, _iq P_from_slave, _iq *theta_to_slave, _iq *Iq_to_slave, _iq *P_to_master, int reset, int prepare_stop_PWM) { _iq Iq_zad = 0, Iq_zad_power = 0, Id_zad = 0; _iq P_measured = 0; static _iq Ud_zad1 = 0, Uq_zad1 = 0, Ud_zad2 = 0, Uq_zad2 = 0; // if(direction < 0) { Frot = -Frot; } if (reset) { Ud_zad1 = 0; Uq_zad1 = 0; Ud_zad2 = 0; Uq_zad2 = 0; } analog_dq_calc(winding_displacement); P_measured = vect_control.iqPvsi1 + vect_control.iqPvsi2; *P_to_master = P_measured; P_measured += P_from_slave; vector_power(Pzad, P_measured, n_alg, master, (vect_control.iqIq1 + vect_control.iqIq2), edrk.zadanie.iq_Izad_rmp, &Iq_zad_power, reset); vector_turns(Fzad, Frot, (vect_control.iqIq1 + vect_control.iqIq2), Iq_zad_power, n_alg, master, &Iq_zad, reset); Id_zad = calcId(edrk.zadanie.iq_Izad, Iq_zad, reset, prepare_stop_PWM); if (master == MODE_SLAVE) { vect_control.iqTheta = theta_from_master; *theta_to_slave = theta_from_master; Iq_zad = Iq_from_master; Iq_zad_power = Iq_from_master; } else { // calc_teta_Id(Frot, vect_control.iqId1, vect_control.iqIq1, &vect_control.iqTheta, theta_to_slave, // &vect_control.iqFsl, &vect_control.iqFstator, master, reset); calc_teta_Id(Frot, Id_zad, Iq_zad, &vect_control.iqTheta, theta_to_slave, &vect_control.iqFsl, &vect_control.iqFstator, master, reset); } calcUdUqCompensation(Frot); if (prepare_stop_PWM && Id_zad == 0) { vect_control.iqUdKm = _IQmpy(vect_control.iqUdKm, vect_control.koef_zero_Uzad); vect_control.iqUqKm = _IQmpy(vect_control.iqUqKm, vect_control.koef_zero_Uzad); } else { Idq_to_Udq_2_windings((Id_zad >> 1), (Iq_zad >> 1), vect_control.iqId1, vect_control.iqIq1, &Ud_zad1, &Uq_zad1, vect_control.iqId2, vect_control.iqIq2, &Ud_zad2, &Uq_zad2, reset); vect_control.iqUdKm = Ud_zad1 + vect_control.iqUdCompensation; vect_control.iqUqKm = Uq_zad1 + vect_control.iqUqCompensation; } vect_control.sqrtIdq1 = _IQsqrt(_IQmpy(vect_control.iqId1, vect_control.iqId1) + _IQmpy(vect_control.iqIq1, vect_control.iqIq1)); analog_Udq_calc(Ud_zad1, Uq_zad1, Ud_zad2, Uq_zad2); *Iq_to_slave = Iq_zad; vect_control.Iq_zad1 = Iq_zad; vect_control.Id_zad1 = Id_zad; } #pragma CODE_SECTION(analog_dq_calc,".fast_run"); void analog_dq_calc(_iq winding_displacement) { ABC_TO_DQ abc_dq_converter = ABC_TO_DQ_DEFAULTS; abc_dq_converter.Ia = analog.iqIu_1; abc_dq_converter.Ib = analog.iqIv_1; abc_dq_converter.Ic = analog.iqIw_1; abc_dq_converter.Tetta = vect_control.iqTheta + winding_displacement; abc_dq_converter.calc(&abc_dq_converter); vect_control.iqId1 = abc_dq_converter.Id; vect_control.iqIq1 = abc_dq_converter.Iq; vect_control.iqId1_filt = exp_regul_iq(vect_control.koef_filt_I, vect_control.iqId1_filt, vect_control.iqId1); vect_control.iqIq1_filt = exp_regul_iq(vect_control.koef_filt_I, vect_control.iqIq1_filt, vect_control.iqIq1); abc_dq_converter.Ia = analog.iqIu_2; abc_dq_converter.Ib = analog.iqIv_2; abc_dq_converter.Ic = analog.iqIw_2; abc_dq_converter.Tetta = vect_control.iqTheta + winding_displacement; abc_dq_converter.calc(&abc_dq_converter); vect_control.iqId2 = abc_dq_converter.Id; vect_control.iqIq2 = abc_dq_converter.Iq; vect_control.iqId2_filt = exp_regul_iq(vect_control.koef_filt_I, vect_control.iqId2_filt, vect_control.iqId2); vect_control.iqIq2_filt = exp_regul_iq(vect_control.koef_filt_I, vect_control.iqIq2_filt, vect_control.iqIq2); if (_IQabs(vect_control.iqId1) < I_ZERO_LEVEL_IQ) { vect_control.iqId1 = 0; } if (_IQabs(vect_control.iqIq1) < I_ZERO_LEVEL_IQ) { vect_control.iqIq1 = 0; } if (_IQabs(vect_control.iqId2) < I_ZERO_LEVEL_IQ) { vect_control.iqId2 = 0; } if (_IQabs(vect_control.iqIq2) < I_ZERO_LEVEL_IQ) { vect_control.iqIq2 = 0; } vect_control.iqPvsi1 = _IQmpy(_IQmpy(vect_control.iqIq1, _IQabs(vect_control.iqUq1)), 25165824L); vect_control.iqPvsi2 = _IQmpy(_IQmpy(vect_control.iqIq2, _IQabs(vect_control.iqUq2)), 25165824L); } #pragma CODE_SECTION(analog_dq_calc_external,".fast_run"); void analog_dq_calc_external(_iq winding_displacement, _iq theta) { ABC_TO_DQ abc_dq_converter = ABC_TO_DQ_DEFAULTS; abc_dq_converter.Ia = analog.iqIu_1; abc_dq_converter.Ib = analog.iqIv_1; abc_dq_converter.Ic = analog.iqIw_1; abc_dq_converter.Tetta = theta + winding_displacement; abc_dq_converter.calc(&abc_dq_converter); vect_control.iqId1 = abc_dq_converter.Id; vect_control.iqIq1 = abc_dq_converter.Iq; abc_dq_converter.Ia = analog.iqIu_2; abc_dq_converter.Ib = analog.iqIv_2; abc_dq_converter.Ic = analog.iqIw_2; abc_dq_converter.Tetta = theta + winding_displacement; abc_dq_converter.calc(&abc_dq_converter); vect_control.iqId2 = abc_dq_converter.Id; vect_control.iqIq2 = abc_dq_converter.Iq; if (_IQabs(vect_control.iqId1) < I_ZERO_LEVEL_IQ) { vect_control.iqId1 = 0; } if (_IQabs(vect_control.iqIq1) < I_ZERO_LEVEL_IQ) { vect_control.iqIq1 = 0; } if (_IQabs(vect_control.iqId2) < I_ZERO_LEVEL_IQ) { vect_control.iqId2 = 0; } if (_IQabs(vect_control.iqIq2) < I_ZERO_LEVEL_IQ) { vect_control.iqIq2 = 0; } vect_control.iqPvsi1 = _IQmpy(_IQmpy(vect_control.iqIq1, _IQabs(vect_control.iqUq1)), 25165824L); vect_control.iqPvsi2 = _IQmpy(_IQmpy(vect_control.iqIq2, _IQabs(vect_control.iqUq2)), 25165824L); } void Idq_to_Udq_2_windings(_iq Id_zad, _iq Iq_zad, _iq Id_measured1, _iq Iq_measured1, _iq* Ud_zad1, _iq* Uq_zad1, _iq Id_measured2, _iq Iq_measured2, _iq* Ud_zad2, _iq* Uq_zad2, int reset) { if (reset) { vect_control.pidD1.Ui = 0; vect_control.pidD1.Out = 0; vect_control.pidQ1.Ui = 0; vect_control.pidQ1.Out = 0; #ifdef CALC_TWO_WINDINGS vect_control.pidD2.Ui = 0; vect_control.pidD2.Out = 0; vect_control.pidQ2.Ui = 0; vect_control.pidQ2.Out = 0; #endif } vect_control.pidD1.Ref = Id_zad; vect_control.pidD1.Fdb = Id_measured1; vect_control.pidD1.calc(&vect_control.pidD1); *Ud_zad1 = vect_control.pidD1.Out; vect_control.pidQ1.Ref = Iq_zad; vect_control.pidQ1.Fdb = Iq_measured1; vect_control.pidQ1.calc(&vect_control.pidQ1); *Uq_zad1 = vect_control.pidQ1.Out; #ifdef CALC_TWO_WINDINGS vect_control.pidD2.Ref = Id_zad; vect_control.pidD2.Fdb = Id_measured2; vect_control.pidD2.calc(&vect_control.pidD2); *Ud_zad2 = vect_control.pidD2.Out; vect_control.pidQ2.Ref = Iq_zad; vect_control.pidQ2.Fdb = Iq_measured2; vect_control.pidQ2.calc(&vect_control.pidQ2); *Uq_zad2 = vect_control.pidQ2.Out; #else *Ud_zad2 = *Ud_zad1; *Uq_zad2 = *Ud_zad1; // *Uq_zad2 = *Uq_zad1; #endif } #pragma CODE_SECTION(analog_Udq_calc,".fast_run"); void analog_Udq_calc(_iq Ud1, _iq Uq1, _iq Ud2, _iq Uq2) { _iq Uzpt = filter.iqU_1_long + filter.iqU_2_long; vect_control.iqUd1 = _IQmpy(Ud1, _IQmpy(Uzpt, 8388608L)); // 8388608 = _IQ(0.5) vect_control.iqUq1 = _IQmpy(Uq1, _IQmpy(Uzpt, 8388608L)); vect_control.iqUd2 = _IQmpy(Ud2, _IQmpy(Uzpt, 8388608L)); vect_control.iqUq2 = _IQmpy(Uq2, _IQmpy(Uzpt, 8388608L)); } _iq calcId(_iq Iq_limit, _iq Iq, int reset, int prepare_stop_PWM) { static _iq Id_rmp = 0; _iq Id_zad = 0; if (reset) { Id_rmp = 0; } if (prepare_stop_PWM) { Id_zad = 0; } else if (Iq < vect_control.iqId_min) { Id_zad = vect_control.iqId_min; } else if (Iq > vect_control.iqId_nominal) { Id_zad = vect_control.iqId_nominal; } else { Id_zad = Iq; } // Id_zad = Iq_limit < vect_control.iqId_nominal ? Iq_limit : vect_control.iqId_nominal; Id_rmp = zad_intensiv_q(vect_control.koef_rmp_Id, vect_control.koef_rmp_Id << 1, Id_rmp, Id_zad); return Id_rmp; } void calcUdUqCompensation(_iq Frot) { _iq Uzpt = (filter.iqU_1_long + filter.iqU_2_long) >> 1; _iq UdVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Ud_comp), vect_control.iqIq1 + vect_control.iqIq2); _iq UqVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Uq_comp), vect_control.iqId1 + vect_control.iqId2); vect_control.iqUdCompensation = -_IQdiv(UdVolt, Uzpt); vect_control.iqUqCompensation = _IQdiv(UqVolt, Uzpt); }