Coal56AB/matlab_23550
4
0
Fork 0
Code Issues 4 Pull Requests Actions Packages Projects 1 Releases Wiki Activity
06c76feb3b
matlab_23550/Inu/Src2/VectorControl/regul_turns.c

186 lines
5.9 KiB
C
Raw Normal View History

init
2024-12-27 10:50:32 +03:00
#include "IQmathLib.h"
#include "math.h"
#include "params.h"
#include "adc_tools.h"
#include "regul_turns.h"
#include "pid_reg3.h"
#include "vector.h"
#include "teta_calc.h"
#include "pwm_vector_regul.h"
#include "my_filter.h"
// #pragma DATA_SECTION(pidFvect,".fast_vars1");
PIDREG3 pidFvect = PIDREG3_DEFAULTS;
#define MAX_PID_CURRENT IQ_OUT_NOM
#define IQ_165_RPM 2306867 //165<36><35>/<2F><><EFBFBD>
#define IQ_170_RPM 2376772 //170<37><30>/<2F><><EFBFBD>
#define IQ_5_RPM 69905 //5<><35>/<2F><><EFBFBD>
#define TIME_RMP_FAST 10.0 //sec
#define TIME_RMP_SLOW 30.0 //sec
#define F_DEST 3.0 //Hz
void init_Fvect_pid()
{
pidFvect.Ref = 0;
pidFvect.Kp = _IQ(10.0);//_IQ(30.0);// //
pidFvect.Ki = _IQ(0.03); //_IQ(0.015);
pidFvect.Kc = _IQ(0.5);
pidFvect.OutMax = _IQ(MAX_PID_CURRENT / NORMA_ACP);
pidFvect.OutMin = -_IQ(MAX_PID_CURRENT / NORMA_ACP);
}
void reset_F_pid()
{
pidFvect.Up = 0;
pidFvect.Up1 = 0;
pidFvect.Ui = 0;
pidFvect.Ud = 0;
pidFvect.Out = 0;
}
void vector_turns(_iq Fzad, _iq Frot, _iq Iq, int mode, _iq* Iq_zad, _iq* Id_zad)
{
static _iq Fzad_rmp = 0, koef_fast = _IQ(F_DEST / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM / 2.5);
static _iq koef_slow = _IQ(F_DEST / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM / 5.0);
// static _iq cos_fi_nom = _IQ(COS_FI), cos_fi_nom_squared = _IQ(COS_FI * COS_FI); //prev_Fzad = 0,
static _iq Iq_out_max = _IQ(IQ_OUT_NOM / NORMA_ACP);
static _iq Id_out_max = _IQ(500.0 / NORMA_ACP); //_IQ(ID_OUT_NOM / NORMA_ACP);
static _iq mzz_zad_int=0;
static int Iq_rmp_to_optica = 0;
// static int flag_Iq_synced_with_optica = 0;
// static _iq step_Iq_rmp = _IQ(20.0 / NORMA_ACP);
_iq koef_rmp; //, koef_spad;
_iq Iq_out_unsat, Iq_out_sat, Id_out_sat, Id_out_unsat;
_iq deltaVar;
// if (f.DownTemperature) {
// // mzz_zad_int = zad_intensiv_q(35480, 35480, mzz_zad_int, _IQmpy(Iq_out_max, temperature_limit_koeff));
// mzz_zad_int = zad_intensiv_q(25480, 25480, mzz_zad_int, _IQmpy(f.iq_mzz_zad, temperature_limit_koeff));
// } else
{
mzz_zad_int = zad_intensiv_q(35480, 35480, mzz_zad_int, f.iq_mzz_zad);
// mzz_zad_int = zad_intensiv_q(25480, 25480, mzz_zad_int, pid_Out_max);
}
pidFvect.OutMax = Iq_out_max;
pidFvect.OutMin = -Iq_out_max;
if (Fzad >= 0 && Frot > 0) {
pidFvect.OutMin = 0;
}
if (Fzad <= 0 && Frot < 0) {
pidFvect.OutMax = 0;
}
if((mode != 1) || (!f.Go)) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD>-<2D><><EFBFBD>, <20><><EFBFBD> <20><> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{ //
Fzad_rmp = Frot;
// prev_Fzad = Frot;
reset_F_pid(); //<2F><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD>-<2D><> <20><><EFBFBD><EFBFBD> <20><> <20><><EFBFBD>
pidFvect.Ui = Iq;
pidFvect.Out = Iq;
*Iq_zad = Iq;
Iq_rmp_to_optica = Iq;
// *Id_zad = _IQdiv(_IQmpy(Iq, _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom);
if(!f.Go)
{
*Iq_zad = *Id_zad = 0;
}
// flag_Iq_synced_with_optica = 0;
return;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> Iq <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>
// if ((f.flag_leading == 0) && (f.read_task_from_optical_bus == 1) && (f.sync_Iq_from_optical_bus == 1)) {
// // if (flag_Iq_synced_with_optica == 0) {
// // if (_IQabs(analog.iqIq_zad_from_optica - Iq_rmp_to_optica) > 1118481) { //200A
// // Iq_rmp_to_optica = zad_intensiv_q(step_Iq_rmp, step_Iq_rmp, Iq_rmp_to_optica, analog.iqIq_zad_from_optica);
// // } else {
// // Iq_rmp_to_optica = analog.iqIq_zad_from_optica;
// // flag_Iq_synced_with_optica = 1;
// // }
// // } else {
// // Iq_rmp_to_optica = analog.iqIq_zad_from_optica;
// // }
// Iq_rmp_to_optica = analog.iqIq_zad_from_optica;
// Iq_out_unsat = Iq_rmp_to_optica;
// Iq_out_sat = _IQsat(Iq_out_unsat, mzz_zad_int, -mzz_zad_int);
// Id_out_unsat = _IQdiv(_IQmpy(_IQabs(Iq_out_sat), _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom);
// Id_out_sat = _IQsat(Id_out_unsat, Id_out_max, 0);
// *Iq_zad = Iq_out_sat;
// *Id_zad = Id_out_sat;
// reset_F_pid();
// pidFvect.Ui = Iq_out_sat;
// pidFvect.Out = Iq_out_sat;
// Fzad_rmp = Frot;
// return;
// }
//if(Fzad != prev_Fzad) //<2F><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
//{ //<2F><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
if(_IQabs(Fzad_rmp) <= _IQabs(Fzad) &&
(((Fzad >= 0) && (Fzad_rmp >= 0)) || ((Fzad < 0 ) && (Fzad_rmp < 0))))
{
koef_rmp = koef_slow;
}
else
{
koef_rmp = koef_fast;
}
// prev_Fzad = Fzad;
//}
// Fzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, Fzad_rmp, Fzad);
// logpar.log2 = (int16)(_IQtoIQ15(Fzad));
// logpar.log19 = (int16)(_IQtoIQ15(Fzad_rmp));
//<2F> <20><><EFBFBD><EFBFBD><EFBFBD>-<2D><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> zad_intensiv_q
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>.
deltaVar = Fzad-Fzad_rmp;
if ((deltaVar>koef_rmp) || (deltaVar<-koef_rmp))
{
if (deltaVar>0) Fzad_rmp += koef_rmp;
else Fzad_rmp -= koef_rmp;
}
else
Fzad_rmp = Fzad;
pidFvect.Ref = Fzad_rmp;
pidFvect.Fdb = Frot;
pidFvect.calc(&pidFvect);
Iq_out_unsat = pidFvect.Out;
//TODO: ATTENTION!!! worked better on stend
if (_IQabs(Iq_out_unsat) < 27962) //5A
{
pidTetta.Ui = 0;
pidTetta.SatErr = 0;
}
Iq_out_sat = _IQsat(Iq_out_unsat, Iq_out_max, -Iq_out_max);
// if(f.DownToNominal) //In this mode max I equial Inom
// {
// Iq_out_sat = _IQsat(Iq_out_unsat, I_OUT_NOMINAL_IQ, -I_OUT_NOMINAL_IQ);
// }
// Iq_out_sat = _IQsat(Iq_out_unsat, mzz_zad_int, -mzz_zad_int); //Test
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> d <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-<2D><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>
Iq_out_unsat = _IQabs(Iq_out_unsat);
Id_out_unsat = _IQdiv(_IQmpy(Iq_out_unsat, _IQsqrt(ONE_IQ24 - cos_fi.cos_fi_nom_squared)), cos_fi.cos_fi_nom);
Id_out_sat = _IQsat(Id_out_unsat, Id_out_max, 0);
*Iq_zad = Iq_out_sat;
*Id_zad = Id_out_sat;
}
Reference in New Issue Copy Permalink