92 lines
2.4 KiB
C
92 lines
2.4 KiB
C
#include "IQmathLib.h"
|
|
|
|
#include "teta_calc.h"
|
|
|
|
#include <master_slave.h>
|
|
#include <params_alg.h>
|
|
#include <params_motor.h>
|
|
#include <params_norma.h>
|
|
#include <params_pwm24.h>
|
|
|
|
#include "mathlib.h"
|
|
#include "pid_reg3.h"
|
|
|
|
|
|
#define CONST_IQ_2PI 105414357
|
|
#define PI 3.1415926535897932384626433832795
|
|
|
|
#define MAX_Ud_Pid_Out_Id 176160 //0.2 ~ 167772 //0.21 ~ 176160
|
|
#define BPSI_START 0.17 //0.15
|
|
|
|
TETTA_CALC tetta_calc = TETTA_CALC_DEF;
|
|
|
|
void init_teta_calc_struct()
|
|
{
|
|
float Tr_cm = (L_M + L_SIGMA_R) / (R_ROTOR_SHTRIH / SLIP_NOM);
|
|
// tetta_calc.k_r = _IQ(1 / FREQ_PWM / Tr_cm);
|
|
// tetta_calc.k_t = _IQ(R_ROTOR / (L_M + L_SIGMA_R) / 2.0 / 3.14 / 50 / NORMA_FROTOR);
|
|
tetta_calc.k_r = _IQ(0.005168); //_IQ(0.015);
|
|
tetta_calc.k_t = _IQ(0.0074); //_IQ(0.0045);
|
|
tetta_calc.Imds = 0;
|
|
tetta_calc.theta = 0;
|
|
tetta_calc.hz_to_angle = _IQ(2.0 * PI * NORMA_FROTOR / FREQ_PWM);
|
|
}
|
|
|
|
//#pragma CODE_SECTION(calc_teta_Id,".fast_run");
|
|
void calc_teta_Id(_iq Frot, _iq Id, _iq Iq, _iq *theta_out, _iq *theta_to_slave, _iq *Fsl_out, _iq *Fstator_out,
|
|
unsigned int master, int reset) {
|
|
|
|
_iq Fsl, Fst;
|
|
_iq add_to_tic = 0;
|
|
_iq to_slave = 0;
|
|
|
|
if (reset) {
|
|
tetta_calc.Imds = 0;
|
|
}
|
|
|
|
tetta_calc.Imds = tetta_calc.Imds + _IQmpy(tetta_calc.k_r, (Id - tetta_calc.Imds));
|
|
|
|
if (master == MODE_SLAVE){
|
|
return;
|
|
}
|
|
|
|
Fsl = _IQmpy(tetta_calc.k_t, Iq);
|
|
if (tetta_calc.Imds != 0) {
|
|
Fsl = _IQdiv(Fsl, tetta_calc.Imds);
|
|
} else {
|
|
Fsl = 0;
|
|
}
|
|
|
|
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;}
|
|
// if (Fsl < 0) { Fsl = 0;}
|
|
|
|
Fst = Frot * POLUS + Fsl;
|
|
add_to_tic = _IQmpy(Fst, tetta_calc.hz_to_angle);
|
|
tetta_calc.theta += add_to_tic;
|
|
to_slave = tetta_calc.theta + add_to_tic;
|
|
|
|
if (tetta_calc.theta > CONST_IQ_2PI) {
|
|
tetta_calc.theta -= CONST_IQ_2PI;
|
|
} else if (tetta_calc.theta < 0) {
|
|
tetta_calc.theta += CONST_IQ_2PI;
|
|
}
|
|
|
|
if (to_slave > CONST_IQ_2PI) {
|
|
to_slave -= CONST_IQ_2PI;
|
|
} else if (to_slave < 0) {
|
|
to_slave += CONST_IQ_2PI;
|
|
}
|
|
|
|
*Fsl_out = Fsl;
|
|
*theta_out = tetta_calc.theta;
|
|
*theta_to_slave = to_slave;
|
|
*Fstator_out = Fst;
|
|
|
|
// logpar.log26 = (int16)(_IQtoIQ15(Fsl));
|
|
// logpar.log27 = (int16)(_IQtoIQ15(tetta_calc.Imds));
|
|
// logpar.log28 = (int16)(_IQtoIQ15(Iq));
|
|
// logpar.log3 = (int16)(_IQtoIQ15(Id));
|
|
}
|
|
|