/*
* alg_simple_scalar.c
*
* Created on: 26 èşí. 2020 ã.
* Author: Yura
*/
#include <alg_simple_scalar.h>
#include <edrk_main.h>
//#include <log_to_mem.h>
#include <master_slave.h>
#include <math.h>
#include <params_alg.h>
#include <params_norma.h>
#include <project.h>
//#include "log_to_mem.h"
#include "IQmathLib.h"
#include "math_pi.h"
#include "mathlib.h"
#include "params_pwm24.h"
#include "filter_v1.h"
#include "log_to_memory.h"
#pragma DATA_SECTION(simple_scalar1,".slow_vars");
ALG_SIMPLE_SCALAR simple_scalar1 = ALG_SIMPLE_SCALAR_DEFAULT;
_iq koefBpsi = _IQ(0.05); //0.05
void init_simple_scalar(void)
{
simple_scalar1.mzz_add_1 = _IQ(MZZ_ADD_1/NORMA_MZZ);
simple_scalar1.mzz_add_2 = _IQ(MZZ_ADD_2/NORMA_MZZ);
simple_scalar1.mzz_add_3 = _IQ(MZZ_ADD_3/NORMA_MZZ);
simple_scalar1.poluses = _IQ(POLUS);
simple_scalar1.iq_mzz_max_for_fzad = _IQ(1000.0/NORMA_MZZ);
simple_scalar1.powerzad_add = _IQ(POWERZAD_ADD_MAX);
simple_scalar1.powerzad_dec = _IQ(POWERZAD_DEC);
// simple_scalar1.k_freq_for_pid = _IQ(1.0);
simple_scalar1.k_freq_for_pid = _IQ(450.0/FREQ_PWM);
simple_scalar1.iq_add_kp_df = _IQ(ADD_KP_DF);
simple_scalar1.iq_add_ki_df = _IQ(ADD_KI_DF);
simple_scalar1.min_mzz_for_df = _IQ(MIN_MZZ_FOR_DF/NORMA_MZZ);
simple_scalar1.pidF_Kp = _IQ(PID_KP_F);
simple_scalar1.pidF_Ki = _IQ(PID_KI_F);
simple_scalar1.pidIm1.Kp=_IQ(PID_KP_IM);
simple_scalar1.pidIm1.Ki=_IQ(PID_KI_IM);
simple_scalar1.pidIm_Ki = simple_scalar1.pidIm1.Ki;
simple_scalar1.pidIm1.Kc=_IQ(PID_KC_IM);
simple_scalar1.pidIm1.Kd=_IQ(PID_KD_IM);
simple_scalar1.pidIm1.OutMax=_IQ(K_STATOR_MAX);
simple_scalar1.pidIm1.OutMin=_IQ(K_STATOR_MIN);
//////////////
simple_scalar1.pidF.Kp=_IQ(PID_KP_F);
simple_scalar1.pidF.Ki=_IQ(PID_KI_F);
simple_scalar1.pidF.Kc=_IQ(PID_KC_F);
simple_scalar1.pidF.Kd=_IQ(PID_KD_F);
simple_scalar1.pidF.OutMax=_IQ(500/NORMA_MZZ);
simple_scalar1.pidF.OutMin=_IQ(0);
// iq_MAX_DELTA_pidF = _IQ(MAX_DELTA_pidF/NORMA_WROTOR);
/////////////////////////
// simple_scalar1.pidPower_Kp = _IQ(PID_KP_POWER);
// simple_scalar1.pidPower_Ki = _IQ(PID_KI_POWER);
// iq_add_kp_dpower = _IQ(ADD_KP_DPOWER);
// iq_add_ki_dpower = _IQ(ADD_KI_DPOWER);
simple_scalar1.pidPower.Kp=_IQ(PID_KP_POWER);
simple_scalar1.pidPower.Ki=_IQ(PID_KI_POWER);
simple_scalar1.pidPower.Ki = _IQmpy(simple_scalar1.pidPower.Ki, simple_scalar1.k_freq_for_pid); // ââîäèì êîğğåêöèş Ki ïî ÷àñòîòå ØÈÌà
simple_scalar1.pidPower.Kc=_IQ(PID_KC_POWER);
simple_scalar1.pidPower.Kd=_IQ(PID_KD_POWER);
simple_scalar1.pidPower.OutMax=_IQ(500/NORMA_MZZ);
simple_scalar1.pidPower.OutMin=_IQ(0);
simple_scalar1.iq_spad_k = _IQ(0.993); //0.993 ~ 0.4 sek äî 5%
// ìèí. ñêîëüæåíèå
simple_scalar1.min_bpsi = _IQ(BPSI_MINIMAL/NORMA_FROTOR);
simple_scalar1.max_bpsi = _IQ(BPSI_MAXIMAL/NORMA_FROTOR);
}
/***************************************************************/
/* Ï/ï óïğàâëåíèy ïñåâäî âåêòîğ 3 -
vector_moment(real Frot, - îáîğîòû ğîòîğà
real fzad, - çàäàííûå îáîğîòû ğîòîğà
real mzz_zad, - ìàêñèìàëüíûé âîçìîæíûé ìîìåíò-îãğàíè÷åíèå òîêà
real *Fz, - ğåçóëüòàò ğàñ÷åòà - ÷àñòîòà íàïğyæåíèy â ñòàòîğå
real *Uz1, - ğåçóëüòàò ğàñ÷åòà - êîıô. ìîäóëyöèè â 1 îáìîòêå ñòàòîğà
real *Uz2) - ğåçóëüòàò ğàñ÷åòà - êîıô. ìîäóëyöèè â 2 îáìîòêå ñòàòîğà
Èñïîëüçóåòñy ğåãóëyòîğ ñêîğîñòè, êîòîğûé âûïîëíyåò çàäàíèå fzad ïî ñêîğîñòè
Çàâîäèòñy ïîñòîyííîå ñêîëüæåíèå = 0.5
Èäåò ğàñ÷åò íàïğyæåíèy ÷åğåç ìîäóëü òîêà ïî îäíîé èç 3-õ ôàçíîé ñòîåê.
Çàìûêàåòñy îáğàòíày ñâyçü ïî îáîğîòàì */
/****************************************************************/
//#pragma CODE_SECTION(simple_scalar,".fast_run");
void simple_scalar(int n_alg,
int n_wind_pump,
int direction,
_iq Frot_pid,
_iq Frot,
_iq fzad,
_iq iqKoefOgran,
_iq mzz_zad,
_iq bpsi_const,
_iq iqIm,
_iq iqUin,
_iq Iin,
_iq powerzad,
_iq power_pid,
_iq power_limit,
int mode_oborots_power,
_iq Izad_from_master,
int master,
int count_bs_work,
_iq *Fz,
_iq *Uz1,
_iq *Uz2,
_iq *Izad_out)
{
_iq mzz, dF, dI1, Izad, Uz_t1, Kpred_Ip, pred_Ip;//, znak_moment;
_iq dI2, Uz_t2;
_iq pkk=0,ikk=0;
_iq Im_regul=0;
static _iq bpsi=0;
// static _iq IQ_POLUS=0;
static _iq mzz_zad_int=0;
static _iq mzzi=0;
static _iq I1_i=0;
static _iq I2_i=0;
static _iq Im1=0;
static _iq Im2=0;
static _iq Uze_t1=0;
static _iq Uze_t2=0;
// static _iq fzad_ogr=0;
// static _iq koef_Uz_t_filter=0;
static _iq dI1_prev=0;
static _iq Uz_t1_prev=0;
static _iq dF_prev = 0;
static _iq mzz_prev = 0;
// static _iq mzz_add_1, mzz_add_2;
static _iq fzad_int=0;//, fzad_add_max;//,iq_mzz_max_for_fzad ;
static _iq fzad_add=0; //fzad_dec
_iq halfIm1, halfIm2;
static _iq powerzad_int=0, powerzad_add_max=0, pidFOutMax = 0, pidFOutMin = 0 ;
//powerzad_dec powerzad_add
// static _iq koef_bpsi=0;
// static _iq min_bpsi=0;
static int flag_uz_t1=0;
// static _iq correct_err=0;
// static _iq iq_dF_min1=0;
// static _iq iq_dF_min2=0;
_iq pred_dF,Kpred_dF;
static _iq dF_PREDEL_LEVEL2 = 0,dF_PREDEL_LEVEL1=0;
_iq Uze_ogr=0;
// static _iq iq_spad_k=1;
static _iq myq_temp=0;
static _iq bpsi_filter=0;
static _iq _iq_koef_im_on_tormog = _IQ(KOEF_IM_ON_TORMOG);
static _iq _iq_koef_im_on_tormog_max_temper_break = _IQ(KOEF_IM_ON_TORMOG_WITH_MAX_TEMPER_BREAK);
static _iq n_iq_koef_im_on_tormog = CONST_IQ_1, t_iq_koef_im_on_tormog = CONST_IQ_1;
static _iq _iq_koef_im_on_tormog_add =_IQ(0.0005), _iq_koef_im_on_tormog_dec = _IQ(0.01);
// static _iq F_revers_level00= _IQ(70.0/60.0/NORMA_FROTOR);
static _iq F_revers_level0 = _IQ(90.0/60.0/NORMA_FROTOR);
static _iq F_revers_level1 = _IQ(100.0/60.0/NORMA_FROTOR);
static _iq F_revers_level2 = _IQ(110.0/60.0/NORMA_FROTOR);
static _iq F_revers_level3 = _IQ(120.0/60.0/NORMA_FROTOR);
static _iq F_revers_level4 = _IQ(130.0/60.0/NORMA_FROTOR);
static _iq F_revers_level5 = _IQ(140.0/60.0/NORMA_FROTOR);
static _iq F_revers_level6 = _IQ(150.0/60.0/NORMA_FROTOR);
static _iq F_revers_level7 = _IQ(160.0/60.0/NORMA_FROTOR);
static _iq F_revers_level8 = _IQ(170.0/60.0/NORMA_FROTOR);
static _iq F_revers_level9 = _IQ(180.0/60.0/NORMA_FROTOR);
static _iq F_revers_level10 = _IQ(190.0/60.0/NORMA_FROTOR);
static _iq F_revers_level11 = _IQ(200.0/60.0/NORMA_FROTOR);
static _iq F_revers_level12 = _IQ(210.0/60.0/NORMA_FROTOR);
static _iq F_revers_level13 = _IQ(220.0/60.0/NORMA_FROTOR);
static _iq kF_revers_level00 = _IQ(0.65);
static _iq kF_revers_level0 = _IQ(0.70);
static _iq kF_revers_level1 = _IQ(0.75);
static _iq kF_revers_level2 = _IQ(0.78);
static _iq kF_revers_level3 = _IQ(0.80);
static _iq kF_revers_level4 = _IQ(0.82);
static _iq kF_revers_level5 = _IQ(0.84);
static _iq kF_revers_level6 = _IQ(0.86);
static _iq kF_revers_level7 = _IQ(0.88);
static _iq kF_revers_level8 = _IQ(0.90);
static _iq kF_revers_level9 = _IQ(0.91);
static _iq kF_revers_level10 = _IQ(0.92);
static _iq kF_revers_level11 = _IQ(0.93);
static _iq kF_revers_level12 = _IQ(0.94);
static _iq kF_revers_level13 = _IQ(0.96);
static _iq P_level0 = _IQ(70.0/60.0/NORMA_FROTOR);
static _iq P_level1 = _IQ(150.0/60.0/NORMA_FROTOR);
static _iq P_level2 = _IQ(160.0/60.0/NORMA_FROTOR);
static _iq P_level3 = _IQ(170.0/60.0/NORMA_FROTOR);
static _iq P_level4 = _IQ(180.0/60.0/NORMA_FROTOR);
static _iq P_level5 = _IQ(190.0/60.0/NORMA_FROTOR);
static _iq P_level6 = _IQ(200.0/60.0/NORMA_FROTOR);
static _iq P_level7 = _IQ(210.0/60.0/NORMA_FROTOR);
static _iq P_level8 = _IQ(220.0/60.0/NORMA_FROTOR);
static _iq P_level9 = _IQ(230.0/60.0/NORMA_FROTOR);
// static _iq P_level9 = _IQ(300.0/60.0/NORMA_FROTOR);
static _iq kP_level0 = _IQ(0.9);
static _iq kP_level1 = _IQ(0.9);
static _iq kP_level2 = _IQ(0.9);
static _iq kP_level3 = _IQ(0.85);
static _iq kP_level4 = _IQ(0.8);
static _iq kP_level5 = _IQ(0.75);
static _iq kP_level6 = _IQ(0.7);
static _iq kP_level7 = _IQ(0.65);
static _iq kP_level8 = _IQ(0.6);
static _iq kP_level9 = _IQ(0.55);
static _iq pid_kp_power = _IQ(PID_KP_POWER);
static _iq add_mzz_outmax_pidp = _IQ(100.0/NORMA_MZZ);
_iq new_pidP_OutMax = 0;
_iq k_ogr_p_koef_1 = 0;
_iq k_ogr_p_koef_2 = 0;
_iq k_ogr_n = 0;
_iq Frot_pid_abs;
_iq d_m=0;
_iq iq_decr_mzz_power;
_iq level1_power_ain_decr_mzz, level2_power_ain_decr_mzz;
_iq new_power_limit = 0;
static _iq koef_Power_filter2 = _IQ(1.0/(FREQ_PWM*EXP_FILTER_KOEF_OGRAN_POWER_LIMIT));//2.2 ñåê //30000;// 0,0012//16777;//0,001//13981;
Frot_pid_abs = _IQabs(Frot_pid);
// ïîïûòêà ïåğåñ÷èòàòü pidPower.Kp íà ëåòó
//
// if (Frot_pid_abs>=P_level0)
// {
// k_ogr_p_koef_1 = CONST_IQ_1 - _IQdiv( (Frot_pid_abs-P_level0) , (P_level9-P_level0) );
// if (k_ogr_p_koef_1<0) k_ogr_p_koef_1 = 0;
// }
// else
// k_ogr_p_koef_1 = CONST_IQ_1;
//
// //k_ogr_p_koef_1 ìåíÿåòñÿ îò 1 äî 0 ñ
//
// k_ogr_p_koef_2 = CONST_IQ_01 + _IQmpy(CONST_IQ_09, k_ogr_p_koef_1);
// simple_scalar1.pidPower.Kp = _IQmpy (pid_kp_power, k_ogr_p_koef_2);// _IQ(PID_KP_POWER)
simple_scalar1.pidPower.Kp = pid_kp_power;//_IQmpy (pid_kp_power, k_ogr_p_koef_2);// _IQ(PID_KP_POWER)
if (mode_oborots_power == ALG_MODE_SCALAR_OBOROTS)
{
if (simple_scalar1.cmd_new_calc_p_limit)
{
simple_scalar1.flag_decr_mzz_power = 0;
simple_scalar1.iq_decr_mzz_power_filter = CONST_IQ_1;
simple_scalar1.iq_decr_mzz_power = CONST_IQ_1;
new_power_limit = power_limit;
}
else
{
// ğàñ÷åò êîıô îãğàíè÷åíèÿ ìîùíîñòè ïî ïğåâûøåíèş çàïàñà, âûøå ëèìèòà
// åñëè òåê ìîùíîñòü ïğèáëèæàåòñÿ ê ëèìèòó òî íà÷èíàåì ïğîïîğóöèîíàëüíî óìåíüøàòü òîê ÷åğåç êîıô.
// simple_scalar1.iq_decr_mzz_power_filter êîòîğûé èäåò îò 1.0 - íåò îãğàíè÷åíèÿ,
// äî 1-MAX_KOEF_OGRAN_POWER_LIMIT - ïîëíîå îãğàíè÷åíèå
new_power_limit = power_limit - simple_scalar1.sdvig_power_limit;
if (new_power_limit<MIN_DELTA_LEVEL_POWER_AIN_DECR_MZZ_DEF)
new_power_limit = MIN_DELTA_LEVEL_POWER_AIN_DECR_MZZ_DEF;
// ïåğâûé óğîâåíü
level1_power_ain_decr_mzz = new_power_limit - DELTA_LEVEL_POWER_AIN_DECR_MZZ_DEF - simple_scalar1.add_power_limit;
if (level1_power_ain_decr_mzz<0)
level1_power_ain_decr_mzz = 0;
// âòîğîé óğîâåíü
level2_power_ain_decr_mzz = level1_power_ain_decr_mzz + DELTA_LEVEL_POWER_AIN_DECR_MZZ_DEF + simple_scalar1.add_power_limit;
// ñìåñòèëè óğîâíè íà SMEWENIE_LEVEL_POWER_AIN_DECR_MZZ_DEF ââåğõ
level1_power_ain_decr_mzz = level1_power_ain_decr_mzz + SMEWENIE_LEVEL_POWER_AIN_DECR_MZZ_DEF;
level2_power_ain_decr_mzz = level2_power_ain_decr_mzz + SMEWENIE_LEVEL_POWER_AIN_DECR_MZZ_DEF;
d_m = power_pid - level1_power_ain_decr_mzz;
if (d_m<0)
d_m=0; // âñå â íîğìå
else
{
// ïîğà îãğàíè÷èâàòü ìîìåíò
if (d_m>=(level2_power_ain_decr_mzz-level1_power_ain_decr_mzz))
d_m = CONST_IQ_1;
else
d_m = _IQdiv(d_m,(level2_power_ain_decr_mzz - level1_power_ain_decr_mzz));
}
if (d_m<0)
d_m=0; // âñå â íîğìå
if (d_m>CONST_IQ_1)
d_m=CONST_IQ_1; // ïîëíîå îãğàíè÷åíèå
// ïåğåâåëè óğîâåíü îò 1.0 äî 0.0 â óğîâåíü îò MAX_KOEF_OGRAN_POWER_LIMIT äî 0.0
d_m = _IQmpy(d_m, MAX_KOEF_OGRAN_POWER_LIMIT); //
simple_scalar1.iq_decr_mzz_power = CONST_IQ_1 - d_m;// òåïåğü êîıô ìåíÿåòñÿ îò 1.0 - íåò îãğàí. äî MAX_KOEF_OGRAN_POWER_LIMIT ìàêñ. îãğàíè÷.
if (simple_scalar1.iq_decr_mzz_power<0)
simple_scalar1.iq_decr_mzz_power=0;
simple_scalar1.iq_decr_mzz_power_filter = exp_regul_iq(koef_Power_filter2,
simple_scalar1.iq_decr_mzz_power_filter,
simple_scalar1.iq_decr_mzz_power);
if (simple_scalar1.iq_decr_mzz_power_filter<0)
simple_scalar1.iq_decr_mzz_power_filter = 0;
if (d_m>0)
simple_scalar1.flag_decr_mzz_power = 1;
else
simple_scalar1.flag_decr_mzz_power=0;
}
}
else
{
simple_scalar1.flag_decr_mzz_power = 0;
simple_scalar1.iq_decr_mzz_power_filter = CONST_IQ_1;
simple_scalar1.iq_decr_mzz_power = CONST_IQ_1;
new_power_limit = power_limit;
}
#if (ENABLE_DECR_MZZ_POWER_IZAD)
if (simple_scalar1.disable_KoefOgranIzad==0)
simple_scalar1.iqKoefOgranIzad = _IQmpy(iqKoefOgran,simple_scalar1.iq_decr_mzz_power_filter);
else
simple_scalar1.iqKoefOgranIzad = iqKoefOgran;
#else
simple_scalar1.iqKoefOgranIzad = iqKoefOgran;
#endif
//static _iq _iq_1 = _IQ(1.0);
// static _iq mzz_int_level1_on_F=0;
// mzz = _IQsat(mzz,mzz_zad_int,0);
simple_scalar1.mzz_zad_in1 = mzz_zad;
simple_scalar1.Izad_from_master = Izad_from_master;
iqKoefOgran = _IQsat(iqKoefOgran,CONST_IQ_1,0);
/* óñòàíàâëèâàåì íà÷àëüíûå óñëîâèy âñåõ ğåãóëyòîğîâ */
if ( (Frot==0) && (fzad==0) )
{
mzzi = 0;
fzad_int = 0;
powerzad_int = 0;
bpsi_filter = 0;
pidFOutMax = pidFOutMin = 0;
n_iq_koef_im_on_tormog = CONST_IQ_1;//_IQ(1.0);
simple_scalar1.iq_decr_mzz_power_filter = CONST_IQ_1;
}
if (mzz_zad==0)
{
bpsi_filter = 0;
mzz=0;
I1_i=0;
mzzi=0;
mzz_zad_int = 0;
fzad_int = 0;
powerzad_int = 0;
simple_scalar1.pidIm1.Up1 = 0;
simple_scalar1.pidIm1.Ui = 0;
simple_scalar1.pidF.Up1 = 0;
simple_scalar1.pidF.Ui = 0;
simple_scalar1.pidPower.Up1 = 0;
simple_scalar1.pidPower.Ui = 0;
Uze_t1=0;
Uze_t2=0;
dI1_prev = 0;
Uz_t1_prev = 0;
dF_prev = 0;
mzz_prev = 0;
// çàäàò÷èê èíòåíñèâíîñòè fzad
fzad_add = _IQ(FZAD_ADD_MAX/NORMA_FROTOR);
// çàäàò÷èê èíòåíñèâíîñòè fzad ïî ìèíóñó
// fzad_dec = _IQ(FZAD_DEC/NORMA_FROTOR);
//
//
// çàäàò÷èê èíòåíñèâíîñòè mzz_max
// iq_mzz_max_for_fzad = _IQ(1000.0/NORMA_MZZ);
// êîıô. ôèëüòğà Uz_t_filter
// koef_Uz_t_filter = _IQ(0.001/0.5); //0.0333
// êîıô. ğàñ÷åòà ñêîëüæåíèå îò mzz
// koef_bpsi = _IQ((0.6/NORMA_WROTOR)/(200.0/NORMA_MZZ));
flag_uz_t1=0;
// êîıô. óñèëåíèß äëß èíòåãğàëüíîãî àäàïòèâíîãî êîıô. â ğåãóëßòîğå ñêîğîñòè
// correct_err = _IQ(2.5/NORMA_WROTOR);
// ìèí. óğîâåíü äëß ğàáîòû êîıô. óñèëåíèß äëß èíòåãğàëüíîãî àäàïòèâíîãî êîıô. â ğåãóëßòîğå ñêîğîñòè
// iq_dF_min1 = _IQ(1.0/NORMA_WROTOR);
// iq_dF_min2 = _IQ(1.5/NORMA_WROTOR);
// êîıô. çàäàíèß ñêîğîñòè ñïàäà Km
// iq_spad_k = _IQ(0.993); //0.993 ~ 0.4 sek äî 5%
// iq_spad_k = _IQ(0.9965); //0.993 ~ 0.4 sek äî 5%
// dF_PREDEL_LEVEL1 = _IQ(0.5/NORMA_WROTOR);
// dF_PREDEL_LEVEL2 = _IQ(1.5/NORMA_WROTOR);
// mzz_int_level1_on_F = _IQ(1.0/NORMA_WROTOR);
// mzz_int_level2_on_F = _IQ(1.5/NORMA_WROTOR);
}
// îãğàíè÷åíèå ïğè ğåêóïåğàöèè
if (direction==0)
{
// ñòîèì
n_iq_koef_im_on_tormog = CONST_IQ_1;//_IQ(1.0);
}
else
if (direction==-1 && fzad <= 0)
{
// åäåì íàçàä, çàäàíèå ñîâïàäàåò ñ íàïğàâëåíèåì âğàùåíèÿ
if (Frot_pid<-F_revers_level13)
n_iq_koef_im_on_tormog = kF_revers_level13;
else
if (Frot_pid<-F_revers_level12)
n_iq_koef_im_on_tormog = kF_revers_level12;
else
if (Frot_pid<-F_revers_level11)
n_iq_koef_im_on_tormog = kF_revers_level11;
else
if (Frot_pid<-F_revers_level10)
n_iq_koef_im_on_tormog = kF_revers_level10;
else
if (Frot_pid<-F_revers_level9)
n_iq_koef_im_on_tormog = kF_revers_level9;
else
if (Frot_pid<-F_revers_level8)
n_iq_koef_im_on_tormog = kF_revers_level8;
else
if (Frot_pid<-F_revers_level7)
n_iq_koef_im_on_tormog = kF_revers_level7;
else
if (Frot_pid<-F_revers_level6)
n_iq_koef_im_on_tormog = kF_revers_level6;
else
if (Frot_pid<-F_revers_level5)
n_iq_koef_im_on_tormog = kF_revers_level5;
else
if (Frot_pid<-F_revers_level4)
n_iq_koef_im_on_tormog = kF_revers_level4;
else
if (Frot_pid<-F_revers_level3)
n_iq_koef_im_on_tormog = kF_revers_level3;
else
if (Frot_pid<-F_revers_level2)
n_iq_koef_im_on_tormog = kF_revers_level2;
else
if (Frot_pid<-F_revers_level1)
n_iq_koef_im_on_tormog = kF_revers_level1;
if (Frot_pid<-F_revers_level0)
n_iq_koef_im_on_tormog = kF_revers_level0;
else
n_iq_koef_im_on_tormog = kF_revers_level00;
}
else
if (direction==1 && fzad>=0)
{
// åäåì âïåğåä, çàäàíèå ñîâïàäàåò ñ íàïğàâëåíèåì âğàùåíèÿ
n_iq_koef_im_on_tormog = CONST_IQ_1;//_IQ(1.0);
}
else
{
// åñëè ğåêóïåğàöèÿ òî óìåíüøèì òîê â _iq_koef_im_on_tormog ğàç ìåíüøå îò çàäàííîãî
// mzz_zad = _IQmpy(mzz_zad, _iq_koef_im_on_tormog);
if (edrk.warnings.e9.bits.BREAK_TEMPER_ALARM == 1)
// åñòü ïåğåãğåâ àâàğèéíûé, ñíèæàåì ìîùíîñòü
n_iq_koef_im_on_tormog = _iq_koef_im_on_tormog_max_temper_break;
else
n_iq_koef_im_on_tormog = _iq_koef_im_on_tormog;
}
t_iq_koef_im_on_tormog = zad_intensiv_q(_iq_koef_im_on_tormog_add,
_iq_koef_im_on_tormog_dec,
t_iq_koef_im_on_tormog,
n_iq_koef_im_on_tormog);
mzz_zad = _IQmpy(mzz_zad, t_iq_koef_im_on_tormog);
simple_scalar1.mzz_zad_in2 = mzz_zad;
/* çàäàò÷èê èíòåíñèâíîñòè ìîìåíòà */
if (n_alg==1)
{
mzz_zad_int = zad_intensiv_q(simple_scalar1.mzz_add_2, simple_scalar1.mzz_add_2, mzz_zad_int, mzz_zad);
// if (Frot_pid>mzz_int_level1_on_F)
// mzz_zad_int = zad_intensiv_q(mzz_add_1, mzz_add_1, mzz_zad_int, mzz_zad);
// else
// mzz_zad_int = zad_intensiv_q(mzz_add_2, mzz_add_2, mzz_zad_int, mzz_zad);
}
if (n_alg==2)
mzz_zad_int = zad_intensiv_q(simple_scalar1.mzz_add_2, simple_scalar1.mzz_add_2, mzz_zad_int, mzz_zad);
// myq_temp = _IQdiv(mzz_zad, simple_scalar1.iq_mzz_max_for_fzad);
// myq_temp = _IQmpy( myq_temp, fzad_add_max);
// fzad_add = myq_temp;
fzad_int = zad_intensiv_q(fzad_add, fzad_add, fzad_int, fzad );
powerzad_int = zad_intensiv_q(simple_scalar1.powerzad_add, simple_scalar1.powerzad_add, powerzad_int, powerzad);
if (n_alg==1)
{
/* ğåãóëyòîğ ñêîğîñòè */
if (mzz_zad_int>=0)
{
dF = fzad_int - Frot_pid;//*direction;
////////// Power PI //////////////
//if (_IQabs(simple_scalar1.pidF.Out))
k_ogr_n = (_IQabs(power_pid) - _IQabs(powerzad_int));
// if (k_ogr_n<0) k_ogr_n = 0;
k_ogr_n = CONST_IQ_1 - _IQdiv(k_ogr_n, _IQabs(powerzad_int));
simple_scalar1.k_ogr_n = _IQsat(k_ogr_n,CONST_IQ_1,-CONST_IQ_1);
// íîâîå îãğàíè÷åíèÿ äëÿ pidP OutMax
new_pidP_OutMax = _IQabs(simple_scalar1.pidF.Out)+add_mzz_outmax_pidp;
new_pidP_OutMax = _IQsat(new_pidP_OutMax, mzz_zad_int, add_mzz_outmax_pidp ); // îò 100 äî ğåçóëüòàòà âûõîäà ğåãóëÿòîğà simple_scalar1.pidF.Out
// ñòàğûé âàğèàíò îãğàíè÷åíèÿ
// new_pidP_OutMax = mzz_zad_int;
simple_scalar1.pidPower.OutMax = new_pidP_OutMax;
simple_scalar1.pidPower.OutMin = 0;
// pidPower.Kp = _IQmpy( _IQdiv(iq_add_kp_dpower, _IQsat(mzz_zad,mzz_zad,MIN_MZZ_FOR_DPOWER)), pidPower_Kp);
// pidPower.Ki = _IQmpy( _IQdiv(iq_add_ki_dpower, _IQsat(mzz_zad,mzz_zad,MIN_MZZ_FOR_DPOWER)), pidPower_Ki);
// simple_scalar1.pidPower.Ki = _IQmpy(simple_scalar1.pidPower.Ki, simple_scalar1.k_freq_for_pid);
simple_scalar1.pidPower.Ref = _IQabs(powerzad_int); // òóò òîëüêî ïîëîæèòåëüíàÿ ìîùíîñòü
simple_scalar1.pidPower.Fdb = _IQabs(power_pid);
simple_scalar1.pidPower.calc(&simple_scalar1.pidPower);
// Saturate the integral output
if (simple_scalar1.pidPower.Ui > simple_scalar1.pidPower.OutMax)
simple_scalar1.pidPower.Ui = simple_scalar1.pidPower.OutMax;
else if (simple_scalar1.pidPower.Ui < simple_scalar1.pidPower.OutMin)
simple_scalar1.pidPower.Ui = simple_scalar1.pidPower.OutMin;
//////////////////////////////
//////////////////////////////
// îãğàíè÷åíèå ìàêñ. çíà÷èíèß âûõîäà ğåãóëßòîğà
// pidF.OutMax=mzz_zad_int;
// èëè òàê
pidFOutMax = zad_intensiv_q(simple_scalar1.mzz_add_3, simple_scalar1.mzz_add_1, pidFOutMax, simple_scalar1.pidPower.Out);
pidFOutMin = zad_intensiv_q(simple_scalar1.mzz_add_3, simple_scalar1.mzz_add_1, pidFOutMin, simple_scalar1.pidPower.Out);
// fzad
if (direction==-1 && fzad <= 0)
{
pidFOutMax = 0;
simple_scalar1.pidF.OutMax = 0;//simple_scalar1.pidPower.Out;
simple_scalar1.pidF.OutMin = -pidFOutMin;//-simple_scalar1.pidPower.Out;
}
else
if (direction==1 && fzad>=0)
{
pidFOutMin = 0;
simple_scalar1.pidF.OutMax = pidFOutMax;//simple_scalar1.pidPower.Out;
simple_scalar1.pidF.OutMin = 0;//-simple_scalar1.pidPower.Out;
}
else
{
simple_scalar1.pidF.OutMax = pidFOutMax;//simple_scalar1.pidPower.Out;
simple_scalar1.pidF.OutMin = -pidFOutMin;//-simple_scalar1.pidPower.Out;
}
/*
// pzad
if (direction==-1 && powerzad <= 0)
{
}
else
if (direction==1 && powerzad>=0)
{
}
else
{
}
*/
// pidF.OutMax = mzz_zad;
if (count_bs_work==2)
simple_scalar1.pidF.Kp = simple_scalar1.pidF_Kp;//_IQmpy( _IQdiv(simple_scalar1.iq_add_kp_df, _IQsat(mzz_zad,mzz_zad,simple_scalar1.min_mzz_for_df)), simple_scalar1.pidF_Kp);
else
simple_scalar1.pidF.Kp = _IQmpy2(simple_scalar1.pidF_Kp);
simple_scalar1.pidF.Ki = simple_scalar1.pidF_Ki;//_IQmpy( _IQdiv(simple_scalar1.iq_add_ki_df, _IQsat(mzz_zad,mzz_zad,simple_scalar1.min_mzz_for_df)), simple_scalar1.pidF_Ki);
simple_scalar1.pidF.Ki = _IQmpy(simple_scalar1.pidF.Ki,simple_scalar1.k_freq_for_pid);
/////////////////////////
// if (_IQabs(dF)<iq_dF_min1)
// {
// dF=iq_dF_min1;
// m.m1.bit.w_rotor_ust = 1;
// }
//
// if (_IQabs(dF)>iq_dF_min2)
// {
// m.m1.bit.w_rotor_ust = 0;
// }
//////////////////////////////////
// áåç êîğğåêöèé dF
//fzad_int =
simple_scalar1.pidF.Ref = _IQmpy(fzad_int, iqKoefOgran);
simple_scalar1.pidF.Fdb = Frot_pid;//*direction;
simple_scalar1.pidF.calc(&simple_scalar1.pidF);
// Saturate the integral output
if (simple_scalar1.pidF.Ui > simple_scalar1.pidF.OutMax)
simple_scalar1.pidF.Ui = simple_scalar1.pidF.OutMax;
else if (simple_scalar1.pidF.Ui < simple_scalar1.pidF.OutMin)
simple_scalar1.pidF.Ui = simple_scalar1.pidF.OutMin;
/////////////////////////////////////
mzz = _IQabs(simple_scalar1.pidF.Out); // òóò ìîäóëü!!!
///////////////////////////////////////
// çàäàò÷èê èíòåíñèâíîñòè íà òîê
// mzz = zad_intensiv_q(mzz_add_2, mzz_add_2, mzz, pidF.Out);
// mzzi = zad_intensiv_q(mzz_add_2, mzz_add_2, mzzi, mzz);
// îãğàíè÷èëè äèàïàçîí mzz
mzz = _IQsat(mzz,mzz_zad_int,0);
}
else
{
mzz = 0;
}
}
if (n_alg==2)
{
mzz = mzz_zad_int;
}
if (master == MODE_SLAVE)
{
mzz = Izad_from_master;
// îãğàíè÷èëè äèàïàçîí mzz
mzz = _IQsat(mzz,mzz_zad_int,0);
}
*Izad_out = mzz;
/* ïğè ïğåâûøåíèè òîêà íåêîòîğîãî ïîğîãîâîãî çíà÷åíèy I_PREDEL_LEVEL1
íà÷èíàåì ëèíåéíî ñáàâëyòü ìîìåíò */
/*
pred_Ip = (filter.I_3+filter.I_6)-I_PREDEL_LEVEL1;
if (pred_Ip<0)
Kpred_Ip=0.0; // âñå â íîğìå
else
{
// ïîğà îãğàíè÷èâàòü ìîìåíò
if (pred_Ip>=(I_PREDEL_LEVEL2-I_PREDEL_LEVEL1))
Kpred_Ip=1;
else
Kpred_Ip = pred_Ip/(I_PREDEL_LEVEL2-I_PREDEL_LEVEL1);
}
// à âîò è îãğàíè÷åíèå
Izad = mzz * (1-Kpred_Ip);
*/
Izad = _IQmpy(mzz, simple_scalar1.iqKoefOgranIzad);
// if ((n_alg==1) || (n_alg==2))
// {
//
// Im1 = iqIm_1;
// Im2 = iqIm_2;
//
// if (n_wind_pump==0) // ğàáîòà ïî äâóì îáìîòêàì
// {
//
// halfIm1 = Im1 >> 1;
// halfIm2 = Im2 >> 1;
//
// if (Im1>halfIm2) //if (Im1>IQdiv(Im2,_IQ(2.0)))
// {
// Im_regul=Im1;
// simple_scalar1.UpravIm1=1;
// simple_scalar1.UpravIm2=0;
// }
// else
// {
// if (Im2>halfIm1)
// {
// Im_regul=Im2;
// simple_scalar1.UpravIm2=1;
// simple_scalar1.UpravIm1=0;
// }
// else
// {
// Im_regul=Im1; //Im1
// simple_scalar1.UpravIm1=1;//1
// simple_scalar1.UpravIm2=0;//0
// }
// }
// }
//
// if (n_wind_pump==1) // íà íàñîñå 1 îáìîòêà çíà÷èò áåğåì òîê ñ 2-îé
// {
// Im_regul=Im2;
// simple_scalar1.UpravIm1=0;
// simple_scalar1.UpravIm2=1;
// }
//
// if (n_wind_pump==2) // íà íàñîñå 2 îáìîòêà çíà÷èò áåğåì òîê ñ 1-îé
// {
// Im_regul=Im1;
// simple_scalar1.UpravIm1=1;
// simple_scalar1.UpravIm2=0;
// }
Im_regul = iqIm;
simple_scalar1.Im_regul = Im_regul;
simple_scalar1.Izad = Izad;
dI1 = (Izad - Im_regul );
simple_scalar1.pidIm1.Ki = simple_scalar1.pidIm_Ki;
simple_scalar1.pidIm1.Ki = _IQmpy(simple_scalar1.pidIm1.Ki,simple_scalar1.k_freq_for_pid);
simple_scalar1.pidIm1.Ref = _IQdiv(Izad,iqUin);
simple_scalar1.pidIm1.Fdb = _IQdiv(Im_regul,iqUin);
simple_scalar1.pidIm1.calc(&simple_scalar1.pidIm1);
Uz_t1 = simple_scalar1.pidIm1.Out;
// îãğàíè÷åíèå ñïàäà Km
if (Uz_t1<Uze_t1)
{
Uze_ogr =_IQmpy(Uze_t1, simple_scalar1.iq_spad_k);
if (Uze_ogr>Uz_t1) Uze_t1 = Uze_ogr;
else
Uze_t1 = Uz_t1;
}
else
{
Uze_t1 = Uz_t1;
}
Uze_t1 = _IQsat(Uze_t1,simple_scalar1.pidIm1.OutMax, simple_scalar1.pidIm1.OutMin);
// }
/* ğåçóëüòàòû îòäàåì íàğóæó */
*Uz1 = Uze_t1;
*Uz2 = Uze_t1;
bpsi = bpsi_const + simple_scalar1.add_bpsi;
// ñêîëüæ. ~ ìîìåíòó
// bpsi = _IQmpy(koef_bpsi,mzz);
bpsi = _IQsat(bpsi,simple_scalar1.max_bpsi, simple_scalar1.min_bpsi);
#ifdef BAN_ROTOR_REVERS_DIRECT
// èñïîëüçóåì çàùèòó îò íåïğàâèëüíîãî âğàùåíèß
if (analog.filter_direct_rotor==-1)
// êğóòèìñß â äğóãóş ñòîğîíó, ïîıòîìó ïûòàåìñß ïåğåéòè íà ñêîëüæåíèè â íîğìàëüíîå âğàùåíèå
*Fz = bpsi;
else
// âñå íîğìàëüíî, íàïğàâëåíèå ïğàâèëüíîå
*Fz = _IQmpy(Frot,IQ_POLUS) + bpsi;
#else
if (simple_scalar1.pidF.Out < 0)
{
bpsi_filter = exp_regul_iq(koefBpsi, bpsi_filter, -bpsi);
}
else
if (simple_scalar1.pidF.Out > 0)
{
bpsi_filter = exp_regul_iq(koefBpsi, bpsi_filter, bpsi);
}
else
bpsi_filter = exp_regul_iq(koefBpsi, bpsi_filter, 0);
// *Fz = _IQmpy(Frot*direction,simple_scalar1.poluses) + bpsi_filter;
*Fz = _IQmpy(Frot, simple_scalar1.poluses) + bpsi_filter;
simple_scalar1.bpsi_curent = bpsi_filter;
#endif
simple_scalar1.mzz_zad_int = mzz_zad_int;
simple_scalar1.Uze_t1 = Uze_t1;
simple_scalar1.iqKoefOgran = iqKoefOgran;
simple_scalar1.Fz = *Fz;
simple_scalar1.direction = direction;
simple_scalar1.fzad_int = fzad_int;
// if (n_alg==2)
// {
//
// *Fz = fzad_provorot;
// /* bpsi - ñêîëüæåíèå, áåğåì ïîêà
// êîíñòàíòîé õîòy òîæå äîëæåí ğåãóëèğîâàòüñy */
// }
}