#include "teta_calc.h"
#include "params.h"
#include "pid_reg3.h"
#include "IQmathLib.h"
#include "vector.h"
#include "my_filter.h"
#define CONST_IQ_2PI 105414357
#define PI 3.1415926535897932384626433832795
PIDREG3 pidTetta = PIDREG3_DEFAULTS;
#define MAX_Ud_Pid_Out (167772 * 100) //419430 ~ 0.5 //251658 ~ 0.3 //209715 ~ 0.25 //167772 ~ 0.2Hz //100663 //0.12Hz
// 184549 ~ 2.2
#define MAX_Ud_Pid_Out_Id 176160 //0.2 ~ 167772 //0.21 ~ 176160
//0.15 ~ 125829
#define BPSI_START 0.17 //0.15
void init_tetta_pid()
{
pidTetta.Ref = 0;
pidTetta.Fdb = 0;
pidTetta.Kp = _IQ(0.1); //_IQ(0.15);
pidTetta.Ki = _IQ(0.0003); //_IQ(0.00003)
pidTetta.Kc = _IQ(0.5);
pidTetta.OutMax = MAX_Ud_Pid_Out;
pidTetta.OutMin = -MAX_Ud_Pid_Out;
pidTetta.Up = 0;
pidTetta.Ui = 0;
}
void reset_tetta_pid()
{
pidTetta.Up = 0;
pidTetta.Up1 = 0;
pidTetta.Ui = 0;
pidTetta.Out = 0;
}
//#pragma CODE_SECTION(calc_tetta,".fast_run");
void calc_tetta(_iq Frot, int direction, _iq Ud, int direct_zadan, _iq *tetta_out, _iq *Fsl_out, _iq *Fstator_out, int mode, int reset) //
{
static _iq tetta = 0, Fsl_start = 0, bpsi_start = _IQ(BPSI_START / NORMA_FROTOR);// , bpsi_start_on_go = _IQ(0.1 / NORMA_FROTOR);
static _iq hz_to_angle = _IQ(2.0 * PI * NORMA_FROTOR / FREQ_PWM / 2.0);
static int flag_start = 0;
static int koeff_zad_int = 30; //1000;
static long stop_frot = 27962; //2 rpm // 10000;
// static _iq prev_Fsl = 0;
static int flag_reverse = 0;
// static int flag_out_of_reverse = 0;
static int prev_direction = 0;
static _iq koeffKpRmp = _IQ(0.0003750);
static _iq koeffKiRmp = _IQ(0.00005);
static _iq lowSpeedKp = _IQ(0.1);
static _iq lowSpeedKi = _IQ(0.0003);
static _iq highSpeedKp = _IQ(0.01);
static _iq highSpeedKi = _IQ(0.011); //_IQ(0.02);
_iq Fstat, Fsl;
int reverse_Ud = 1;
if(reset == 1)
{
flag_start = 1; //Won`t work, if machine will stop without changing mode
Fsl_start = 0;
flag_reverse = 0;
// flag_out_of_reverse = 0;
}
if (_IQabs(Frot) < 768955) { //55 rpm
pidTetta.Kp = zad_intensiv_q(koeffKpRmp, koeffKpRmp, pidTetta.Kp, lowSpeedKp);
pidTetta.Ki = zad_intensiv_q(koeffKiRmp, koeffKiRmp, pidTetta.Ki, lowSpeedKi);
}
else if ((_IQabs(Frot) < 1118481)) { //80 rpm
pidTetta.Kp = zad_intensiv_q(koeffKpRmp, koeffKpRmp, pidTetta.Kp, highSpeedKp);
pidTetta.Ki = zad_intensiv_q(koeffKiRmp, koeffKiRmp, pidTetta.Ki, highSpeedKi);
}
//������ ����������� direction == 1
//direction == 0 - ��������� �����
// if(((Frot >= 0) && (direct_zadan == 1) && (direction >= 0)) ||
// ((direct_zadan == -1) && (direction >= 0) && (Frot >= stop_frot)))
//27962 ~ 2 rpm
// if(((direct_zadan == 1) && (direction == 0)) || (direction > 0))
// {
// Ud = -Ud;
// }
if(Frot >= 27962) {
reverse_Ud = -1;
flag_reverse = (Frot >= 27962 && direct_zadan == -1) ? 1 : 0;
prev_direction = 1;
}
else if (Frot <= -27962) {
reverse_Ud = 1;
flag_reverse = (Frot <= -27962 && direct_zadan == 1) ? 1 : 0;
prev_direction = -1;
}
if(_IQabs(Frot) < 27962) {
if (flag_reverse == 1) {
if (prev_direction == 1) {
reverse_Ud = -1;
} else {
reverse_Ud = 1;
}
} else
// if (flag_start)
{
if (direct_zadan == 1) {
reverse_Ud = -1;
} else {
reverse_Ud = 1;
}
}
}
Ud = Ud * reverse_Ud;
pidTetta.Ref = Ud * 100; //Ref -�������
pidTetta.calc(&pidTetta);
Fsl = pidTetta.Out / 100;
if(flag_start) //
{
if(_IQabs(Frot) < stop_frot)
{
Fsl_start = bpsi_start * direct_zadan; //���� ��������� �����, �������-�� ����������.
}
else
{
if(Fsl_start == 0)
{
Fsl_start = bpsi_start * direct_zadan; //direction;
}
flag_start = 0;
}
}
else
{
//����� ��������� ����� ����, ����� �������� ����������, �.�. ���������� ���� �� ������������ ���������� ���������.
if(_IQabs(Frot) < (stop_frot))
{
// if((direct_zadan == 1) && (Fsl + Fsl_start < bpsi_start))
// {
// Fsl_start += (koeff_zad_int << 2);
//// Fsl_start += koeff_zad_int;
// }
// if((direct_zadan == -1) && (Fsl + Fsl_start > -bpsi_start))
// {
// Fsl_start -= (koeff_zad_int << 2);
//// Fsl_start -= koeff_zad_int;
// }
}
else
{
if(_IQabs(Fsl_start) > koeff_zad_int)
{
if(Fsl_start > 0)
{
Fsl_start -= koeff_zad_int;
}
if(Fsl_start < 0)
{
Fsl_start += koeff_zad_int;
}
}
else
{
Fsl_start = 0;
}
}
}
Fsl += Fsl_start;
Fstat = Frot * POLUS + Fsl;
tetta += _IQmpy(Fstat, hz_to_angle);
if (tetta > CONST_IQ_2PI)
{
tetta -= CONST_IQ_2PI;
}
if (tetta < 0)
{
tetta += CONST_IQ_2PI;
}
*tetta_out = tetta;
*Fsl_out = Fsl;
// prev_Fsl = Fsl;
*Fstator_out = Fstat;
}
TETTA_CALC tetta_calc = TETTA_CALC_DEF;
void init_tetta_calc_struct()
{
tetta_calc.Imds = 0;
tetta_calc.tetta = 0;
tetta_calc.k_r = _IQ(0.015);
tetta_calc.k_t = _IQ(0.0045); //_IQ(0.0045);
tetta_calc.hz_to_angle = _IQ(2.0 * PI * NORMA_FROTOR / FREQ_PWM / 2.);
}
void calc_tetta_Id(_iq Frot, _iq Id, _iq Iq, _iq *tetta_out, _iq *Fsl_out, _iq *Fstator_out, int reset) {
_iq Fsl, Fst;
if (reset) {
tetta_calc.Imds = 0;
}
tetta_calc.Imds = tetta_calc.Imds + _IQmpy(tetta_calc.k_r, (Id - tetta_calc.Imds));
Fsl = _IQmpy(tetta_calc.k_t, Iq);
if (tetta_calc.Imds != 0) {
Fsl = _IQdiv(Fsl, tetta_calc.Imds);
} else {
Fsl = 0;
}
// Fsl = _IQ(0.2 / NORMA_FROTOR);
if (Fsl > MAX_Ud_Pid_Out_Id) { Fsl = MAX_Ud_Pid_Out_Id;}
if (Fsl < -MAX_Ud_Pid_Out_Id) { Fsl = -MAX_Ud_Pid_Out_Id;}
Fst = Frot * POLUS + Fsl;
tetta_calc.tetta += _IQmpy(Fst, tetta_calc.hz_to_angle);
if (tetta_calc.tetta > CONST_IQ_2PI)
{
tetta_calc.tetta -= CONST_IQ_2PI;
}
if (tetta_calc.tetta < 0)
{
tetta_calc.tetta += CONST_IQ_2PI;
}
*Fsl_out = Fsl;
*tetta_out = tetta_calc.tetta;
*Fstator_out = Fst;
}
#define LEVEL_REDUCE_UD 838860 //150V
void correct_tetta_calc_Kt(void) {
static _iq coeff_add_Kt = _IQ(0.00005);
static _iq max_Kt = _IQ(0.0069);
static _iq min_Kt = _IQ(0.0029);
if (tetta_calc.Imds > LEVEL_REDUCE_UD) {
tetta_calc.k_t -= coeff_add_Kt;
}
if (tetta_calc.Imds < -LEVEL_REDUCE_UD) {
tetta_calc.k_t += coeff_add_Kt;
}
if (tetta_calc.k_t > max_Kt) {tetta_calc.k_t = max_Kt; }
if (tetta_calc.k_t < min_Kt) {tetta_calc.k_t = min_Kt; }
}