112 lines
3.5 KiB
C
112 lines
3.5 KiB
C
/*!
|
|
Copyright 2017 ÀÎ "ÍÈÈÝÒ" è ÎÎÎ "ÍÏÔ ÂÅÊÒÎÐ"
|
|
|
|
Licensed under the Apache License, Version 2.0 (the "License");
|
|
you may not use this file except in compliance with the License.
|
|
You may obtain a copy of the License at
|
|
|
|
http://www.apache.org/licenses/LICENSE-2.0
|
|
|
|
Unless required by applicable law or agreed to in writing, software
|
|
distributed under the License is distributed on an "AS IS" BASIS,
|
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
See the License for the specific language governing permissions and
|
|
limitations under the License.
|
|
|
|
\file V_pid_reg3.c
|
|
\brief ÏÈÄ-ðåãóëÿòîð (ñì. TPidReg3)
|
|
\author ÎÎÎ "ÍÏÔ Âåêòîð". http://motorcontrol.ru
|
|
\version v 2.0 25/03/2016
|
|
|
|
\addtogroup V_pid_reg3
|
|
@{*/
|
|
|
|
#include "V_IQmath.h" // Include header for IQmath library
|
|
#include "V_pid_reg3.h"
|
|
#include "stdlib.h"
|
|
#include "main.h"
|
|
//! Ôóíêöèÿ ðàñ÷åòà ÏÈÄ ðåãóëÿòîðà
|
|
//! Âîîáùå ýòî ÷óòü äîðàáîòàííàÿ âåðñèÿ òåêñàñîâñêîãî ðåãóëÿòîðà
|
|
//! Äîêóìåíòàöèþ ýòîé ìàãèè ëó÷øå âñåãî ïîñìîòðåòü, çàãóãëèâ pid_reg3.pdf
|
|
|
|
//! Íà âõîäå pid_ref_reg3 - çàäàíèå, pid_fdb_reg3 - îáðàòíàÿ ñâÿçü
|
|
//! Íà âûõîäå pid_out_reg3.
|
|
|
|
//! \memberof TPidReg3
|
|
void pid_reg3_calc(TPidReg3 *v) {
|
|
v->e_reg3 = v->pid_ref_reg3 - v->pid_fdb_reg3;//îøèáêà - çàäàíèå ìèíóñ îáðàòíàÿ ñâÿçü
|
|
v->e_reg3Dz=v->e_reg3;//îøèáêà ïîñëå êîððåêöèè áëîêà ìåðòâîé çîíû
|
|
|
|
if (v->DeadZone!=0){//åñëè åñòü óñòàâêà çîíû íå÷óâñòâèòåëüíîñòè
|
|
if (v->e_reg3Dz>0){//îøèáêà â ïëþñ
|
|
v->e_reg3Dz=v->e_reg3Dz-v->DeadZone;//âû÷èòàåì ìåðòâóþ çîíó
|
|
if (v->e_reg3Dz<0)//íî òàê, ÷òîáû îøèáêà íå ñòàëà îòðèöàòåëüíîé
|
|
v->e_reg3Dz=0;
|
|
}
|
|
if (v->e_reg3Dz<0){
|
|
v->e_reg3Dz=v->e_reg3Dz+v->DeadZone;
|
|
if (v->e_reg3Dz>0)
|
|
v->e_reg3Dz=0;
|
|
}
|
|
}
|
|
|
|
v->up_reg3 = _IQmpy(v->Kp_reg3, v->e_reg3Dz);
|
|
|
|
v->uprsat_reg3 = v->up_reg3 + v->ui_reg3 + v->ud_reg3;
|
|
|
|
if (v->uprsat_reg3 > v->pid_out_max)
|
|
v->pid_out_reg3 = v->pid_out_max;
|
|
else if (v->uprsat_reg3 < v->pid_out_min)
|
|
v->pid_out_reg3 = v->pid_out_min;
|
|
else
|
|
v->pid_out_reg3 = v->uprsat_reg3;
|
|
|
|
v->saterr_reg3 = v->pid_out_reg3 - v->uprsat_reg3 + v->saterr_reg3Add;
|
|
|
|
//èñïîëüçîâàòü ëè ôèëüòð äëÿ äèôô. ÷àñòè. Åñëè êôèëüòðà íîëü, òî íåò.
|
|
if (v->Kf_d == 0){
|
|
v->e_reg3_filterOut = v->e_reg3;//âûõîä ôèëüòðà
|
|
}
|
|
else{//èíà÷å ñ÷èòàåì ôèëüòð
|
|
v->e_reg3_filterOut = v->e_reg3_filterOut
|
|
+ _IQmpy(v->Kf_d, (v->e_reg3 - v->e_reg3_filterOut));
|
|
}
|
|
|
|
if ((v->DiffCounter++ + 1) >= v->DiffDelim) {//êàæäûå ñêîëüêî âûçîâîâ ñ÷èòàòü äèôô. ÷àñòü
|
|
if (v->KdFilterInitFlag==1){//ýòî ïåðâûé òàêò ðàñ÷åòà ðåãóëÿòîðà
|
|
v->e_reg3_filterOut = v->e_reg3;//âûõîä ôèëüòðà îøèáêè èíèöèàëèçèðóåì îøèáêîé
|
|
v->up1_reg3 = v->e_reg3_filterOut;//ïðîèçâîäíàÿ íîëü
|
|
v->KdFilterInitFlag=0;//èíèöèàëèçàöèÿ çàâåðøåíà
|
|
}
|
|
v->ud_reg3 = _IQmpy(v->Kd_reg3, (v->e_reg3_filterOut - v->up1_reg3)<<6);//äèôôåðåíöèàëüíàÿ ÷àñòü
|
|
v->up1_reg3 = v->e_reg3_filterOut;
|
|
v->DiffCounter = 0;
|
|
}
|
|
|
|
if (v->Ki_reg3 != 0)//åñòü èíòåãðàëüíàÿ ñîñòàâëÿþùàÿ
|
|
v->ui_reg3 = v->ui_reg3
|
|
+ _IQmpy(v->Ki_reg3,
|
|
v->up_reg3) + _IQmpy(v->Kc_reg3,v->saterr_reg3);
|
|
else
|
|
v->ui_reg3 = 0;
|
|
|
|
if (v->Kc_reg3 == 0) {
|
|
if (v->ui_reg3 > v->pid_out_max)
|
|
v->ui_reg3 = v->pid_out_max;
|
|
else if (v->ui_reg3 < v->pid_out_min)
|
|
v->ui_reg3 = v->pid_out_min;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
//! \memberof TPidReg3
|
|
void pid_reg3_reset(TPidReg3 *v) {
|
|
v->pid_fdb_reg3=0;
|
|
v->pid_ref_reg3=0;
|
|
v->ui_reg3=0;
|
|
v->KdFilterInitFlag=1;
|
|
}
|
|
|
|
/*@}*/
|