144 lines
4.0 KiB
C
144 lines
4.0 KiB
C
#include "DSP281x_Device.h"
|
|
#include "IQmathLib.h"
|
|
|
|
#include "regul_turns.h"
|
|
|
|
#include <adc_tools.h>
|
|
#include <edrk_main.h>
|
|
#include <master_slave.h>
|
|
#include <params.h>
|
|
#include <params_motor.h>
|
|
#include <params_norma.h>
|
|
#include <params_pwm24.h>
|
|
#include "math.h"
|
|
#include "mathlib.h"
|
|
#include "pid_reg3.h"
|
|
#include "vector_control.h"
|
|
|
|
#pragma DATA_SECTION(turns,".fast_vars1");
|
|
TURNS turns = TURNS_DEFAULTS;
|
|
|
|
//IQ_OUT_NOM TODO:set Iq nominal
|
|
|
|
#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()
|
|
{
|
|
turns.pidFvect.Ref = 0;
|
|
turns.pidFvect.Kp = _IQ(5); // //_IQ(30);
|
|
turns.pidFvect.Ki = _IQ(0.005); //_IQ(0.008);//_IQ(0.002); //
|
|
turns.pidFvect.Kc = _IQ(0.5);
|
|
turns.pidFvect.OutMax = _IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP);
|
|
turns.pidFvect.OutMin = -_IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP);
|
|
|
|
turns.Fzad_rmp = 0;
|
|
turns.Fnominal = _IQ(FROT_MAX / NORMA_FROTOR);
|
|
turns.koef_fast =
|
|
_IQ(F_DEST / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM / 1.0);
|
|
turns.koef_slow =
|
|
_IQ(F_DEST / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM / 1.0);
|
|
turns.Iq_out_max = _IQ(MOTOR_CURRENT_MAX / NORMA_ACP);
|
|
turns.Id_out_max = 0;//_IQ(ID_OUT_NOM / NORMA_ACP);
|
|
turns.mzz_zad_int = 0;
|
|
}
|
|
|
|
void reset_F_pid()
|
|
{
|
|
turns.pidFvect.Up = 0;
|
|
turns.pidFvect.Up1 = 0;
|
|
turns.pidFvect.Ui = 0;
|
|
turns.pidFvect.Ud = 0;
|
|
turns.pidFvect.Out = 0;
|
|
}
|
|
|
|
//#pragma CODE_SECTION(vector_turns,".fast_run2");
|
|
void vector_turns(_iq Fzad, _iq Frot,
|
|
_iq Iq_measured, _iq Iq_limit, int n_alg,
|
|
unsigned int master, _iq *Iq_zad, int reset)
|
|
{
|
|
static int prev_n_alg = 0;
|
|
_iq koef_rmp; //, koef_spad;
|
|
_iq Iq_out_unsat, Iq_out_sat, Id_out_sat, Id_out_unsat;
|
|
_iq deltaVar;
|
|
|
|
// turns.mzz_zad_int = zad_intensiv_q(35480, 35480, turns.mzz_zad_int, Iq_limit);
|
|
turns.mzz_zad_int = Iq_limit;
|
|
|
|
turns.pidFvect.OutMax = turns.mzz_zad_int;
|
|
turns.pidFvect.OutMin = -turns.mzz_zad_int;
|
|
|
|
//Óáèðàåì ðåæèì òîðìîæåíèÿ
|
|
if (Fzad >= 0 && Frot > 0)
|
|
{
|
|
turns.pidFvect.OutMin = 0;
|
|
}
|
|
if (Fzad <= 0 && Frot < 0)
|
|
{
|
|
turns.pidFvect.OutMax = 0;
|
|
}
|
|
if (reset) { turns.Fzad_rmp = Frot;}
|
|
|
|
if ((n_alg < ALG_MODE_FOC_OBOROTS) || (!edrk.Go))
|
|
//Çàíîñèì òåêóùåå ñîñòîÿíèå â ðåãóë-òîð, ÷òî áû ïðè ñìåíå ðåæèìà íå áûëî ñêà÷êîâ
|
|
{ //
|
|
turns.Fzad_rmp = Frot;
|
|
// prev_Fzad = Frot;
|
|
reset_F_pid(); //Áûëî íèæå, ìîæåò áûòü ÷òî-òî áûëî íå òàê
|
|
turns.pidFvect.Ui = Iq_measured;
|
|
turns.pidFvect.Out = Iq_measured;
|
|
*Iq_zad = Iq_measured;
|
|
|
|
if (!edrk.Go)
|
|
{
|
|
*Iq_zad = 0;
|
|
}
|
|
|
|
return;
|
|
}
|
|
if (master == MODE_SLAVE) {
|
|
turns.Fzad_rmp = Frot;
|
|
turns.pidFvect.Ui = Iq_measured;
|
|
turns.pidFvect.Out = Iq_measured;
|
|
return;
|
|
}
|
|
//Â ðåæèìå ïîääåðæàíèÿ ìîùíîñòè çàäàþòñÿ ìàêñèìàëüíûå äîïóñòèìûå îáîðîòû
|
|
if (n_alg == ALG_MODE_FOC_POWER) {
|
|
Fzad = turns.Fnominal;
|
|
}
|
|
//Äëÿ ïåðåõîäà èõ ðåæèìà äîääåðæàíèÿ ìîùíîñè â ðåæèì ïîääåðæàíèÿ îáîðîòîâ
|
|
//ðàìïà ïîéä¸ò îò òåêóùåãî çíà÷åíèÿ îáîðîòîâ
|
|
if (n_alg == ALG_MODE_FOC_OBOROTS && prev_n_alg != ALG_MODE_FOC_OBOROTS) {
|
|
turns.Fzad_rmp = Frot;
|
|
}
|
|
|
|
if (_IQabs(turns.Fzad_rmp) <= _IQabs(Fzad)
|
|
&& (((Fzad >= 0) && (turns.Fzad_rmp >= 0))
|
|
|| ((Fzad < 0) && (turns.Fzad_rmp < 0))))
|
|
{
|
|
koef_rmp = turns.koef_slow;
|
|
}
|
|
else
|
|
{
|
|
koef_rmp = turns.koef_fast;
|
|
}
|
|
|
|
turns.Fzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, turns.Fzad_rmp, Fzad);
|
|
|
|
turns.pidFvect.Ref = turns.Fzad_rmp;
|
|
turns.pidFvect.Fdb = Frot;
|
|
|
|
turns.pidFvect.calc(&turns.pidFvect);
|
|
Iq_out_unsat = turns.pidFvect.Out;
|
|
|
|
Iq_out_sat = _IQsat(Iq_out_unsat, turns.mzz_zad_int, -turns.mzz_zad_int); //Test
|
|
*Iq_zad = Iq_out_sat;
|
|
|
|
prev_n_alg = n_alg;
|
|
}
|