263 lines
6.3 KiB
C
263 lines
6.3 KiB
C
#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);
|
||
}
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> direction == 1
|
||
//direction == 0 - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
|
||
// 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 -<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
pidTetta.calc(&pidTetta);
|
||
Fsl = pidTetta.Out / 100;
|
||
|
||
if(flag_start) //
|
||
{
|
||
if(_IQabs(Frot) < stop_frot)
|
||
{
|
||
Fsl_start = bpsi_start * direct_zadan; //<2F><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-<2D><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
||
}
|
||
else
|
||
{
|
||
if(Fsl_start == 0)
|
||
{
|
||
Fsl_start = bpsi_start * direct_zadan; //direction;
|
||
}
|
||
flag_start = 0;
|
||
}
|
||
|
||
}
|
||
else
|
||
{
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20>.<2E>. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
||
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; }
|
||
}
|
||
|
||
|