186 lines
5.9 KiB
C
186 lines
5.9 KiB
C
#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îá/ìèí
|
|
#define IQ_170_RPM 2376772 //170îá/ìèí
|
|
#define IQ_5_RPM 69905 //5îá/ìèí
|
|
|
|
#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)) //Çàíîñèì òåêóùåå ñîñòîÿíèå â ðåãóë-òîð, ÷òî áû ïðè ñìåíå ðåæèìà íå áûëî ñêà÷êîâ
|
|
{ //
|
|
Fzad_rmp = Frot;
|
|
// prev_Fzad = Frot;
|
|
reset_F_pid(); //Áûëî íèæå, ìîæåò áûòü ÷òî-òî áûëî íå òàê
|
|
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;
|
|
}
|
|
|
|
//Ñèíõðîíèçàöèÿ ïàðíûõ Ï×
|
|
//Çàäàíèå ïî Iq ïåðåäà¸òñÿ ïî îïòè÷åñêîé øèíå
|
|
// 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) //Ðàìïà çàâèñèò îò ðåæèìà. Åñëè ïðîñòî ïîâûøåíèå, òî ìåäëåííî.
|
|
//{ //Åñëè ïîíèæåíèå èëè ðåâåðñ, òî áûñòðî.
|
|
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));
|
|
|
|
//Â êàêîé-òî ìîìåíò ïåðåñòàëà êîððåêòíî ðàáîòàòü ôóíêöèÿ zad_intensiv_q
|
|
// ïîýòîìó êîä èç íå¸ âñòàâèë ñþäà.
|
|
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
|
|
//Ðàñ÷¸ò d ñîñòàâë-þùåé òîêà
|
|
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;
|
|
|
|
}
|