/* * alg_simple_scalar.c * * Created on: 26 èþí. 2020 ã. * Author: Yura */ #include #include //#include #include #include #include #include #include //#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=(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_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_t1Uz_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 */ // } }