977 lines
28 KiB
C
977 lines
28 KiB
C
/*
|
|
* 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 */
|
|
// }
|
|
|
|
|
|
}
|
|
|
|
|
|
|