89 lines
2.5 KiB
C
89 lines
2.5 KiB
C
|
/*
|
|||
|
|
* regul_power.c
|
|||
|
|
*
|
|||
|
|
* Created on: 16 <EFBFBD><EFBFBD><EFBFBD><EFBFBD>. 2020 <EFBFBD>.
|
|||
|
|
* Author: star
|
|||
|
|
*/
|
|||
|
|
#include "IQmathLib.h"
|
|||
|
|
#include "regul_power.h"
|
|||
|
|
|
|||
|
|
#include <edrk_main.h>
|
|||
|
|
#include <master_slave.h>
|
|||
|
|
#include <params_motor.h>
|
|||
|
|
#include <params_norma.h>
|
|||
|
|
#include <params_pwm24.h>
|
|||
|
|
#include "math.h"
|
|||
|
|
|
|||
|
|
#include "mathlib.h"
|
|||
|
|
|
|||
|
|
#define TIME_RMP_SLOW 20.0 //sec
|
|||
|
|
#define TIME_RMP_FAST 20.0 //sec
|
|||
|
|
|
|||
|
|
POWER power = POWER_DEFAULTS;
|
|||
|
|
|
|||
|
|
void init_Pvect(void) {
|
|||
|
|
power.pidP.Ref = 0;
|
|||
|
|
power.pidP.Kp = _IQ(1);
|
|||
|
|
power.pidP.Ki = _IQ(0.1);
|
|||
|
|
power.pidP.Kc = _IQ(0.5);
|
|||
|
|
power.pidP.OutMax = _IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP);
|
|||
|
|
power.pidP.OutMin = -_IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP);
|
|||
|
|
|
|||
|
|
power.Pzad_rmp = 0;
|
|||
|
|
power.koef_fast = _IQ(FROT_NOMINAL / (float)NORMA_FROTOR / TIME_RMP_FAST / (float)FREQ_PWM);
|
|||
|
|
power.koef_slow = _IQ(FROT_NOMINAL / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM);
|
|||
|
|
power.Iq_out_max = _IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP);
|
|||
|
|
power.Pnominal = _IQ(P_NOMINAL * 1000.0 / NORMA_ACP / NORMA_ACP);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#pragma CODE_SECTION(vector_power,".fast_run");
|
|||
|
|
_iq vector_power(_iq Pzad, _iq P_measured, int n_alg, unsigned int master,
|
|||
|
|
_iq Iq_measured, _iq Iq_limit, _iq *Iq_zad, int reset)
|
|||
|
|
{
|
|||
|
|
static int prev_n_alg = 0;
|
|||
|
|
_iq Iq_out = 0;
|
|||
|
|
_iq koef_rmp = 0;
|
|||
|
|
if (n_alg != ALG_MODE_FOC_POWER || !edrk.Go || master != MODE_MASTER || reset) {
|
|||
|
|
power.pidP.Ui = Iq_measured;
|
|||
|
|
power.pidP.Out = Iq_measured;
|
|||
|
|
if (reset) { power.Pzad_rmp = 0; }
|
|||
|
|
}
|
|||
|
|
if (n_alg == ALG_MODE_FOC_OBOROTS) {
|
|||
|
|
Pzad = power.Pnominal;
|
|||
|
|
}
|
|||
|
|
if (master == MODE_SLAVE) {
|
|||
|
|
power.Pzad_rmp = P_measured;
|
|||
|
|
return *Iq_zad;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,
|
|||
|
|
//<2F>.<2E>. <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <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>
|
|||
|
|
if (n_alg == ALG_MODE_FOC_POWER && prev_n_alg != ALG_MODE_FOC_POWER) {
|
|||
|
|
power.Pzad_rmp = P_measured;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if((_IQabs(power.Pzad_rmp) <= _IQabs(Pzad)) &&
|
|||
|
|
(((Pzad >= 0) && (power.Pzad_rmp >= 0)) || ((Pzad < 0) && (power.Pzad_rmp < 0))))
|
|||
|
|
{
|
|||
|
|
koef_rmp = power.koef_slow;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
koef_rmp = power.koef_fast;
|
|||
|
|
}
|
|||
|
|
power.Pzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, power.Pzad_rmp, Pzad);
|
|||
|
|
|
|||
|
|
power.pidP.OutMax = Iq_limit;
|
|||
|
|
power.pidP.OutMin = -Iq_limit;
|
|||
|
|
power.pidP.Ref = power.Pzad_rmp;
|
|||
|
|
power.pidP.Fdb = P_measured;
|
|||
|
|
power.pidP.calc(&power.pidP);
|
|||
|
|
Iq_out = power.pidP.Out;
|
|||
|
|
Iq_out = _IQsat(Iq_out, Iq_limit, -Iq_limit);
|
|||
|
|
*Iq_zad = Iq_out;
|
|||
|
|
|
|||
|
|
prev_n_alg = n_alg;
|
|||
|
|
|
|||
|
|
return Iq_out;
|
|||
|
|
}
|