298 lines
11 KiB
C
298 lines
11 KiB
C
/*
|
|
* vector_control.c
|
|
*
|
|
* Created on: 16 íî˙á. 2020 ă.
|
|
* Author: star
|
|
*/
|
|
#include "IQmathLib.h"
|
|
|
|
#include "vector_control.h"
|
|
|
|
#include <adc_tools.h>
|
|
#include <edrk_main.h>
|
|
#include <master_slave.h>
|
|
#include <params_motor.h>
|
|
#include <params_norma.h>
|
|
#include <params_pwm24.h>
|
|
#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);
|
|
}
|
|
|