AM
Size: a a a
AM
P
P
r
r
r
r
P
/* Пид-регулятор */
static float yi = 0;
void PID_U_RESET()
{
yi = 0;
}
float PID_U(float delta)
{
/* константы */
#define kp (1)
#define ki (10)
#define kd (1)
#define Td (1)
#define hd (1e-3) /*Шаг счета - изменяется обратно пропорционально частоте вызова регулятора */
/*пропорциональная часть - не нуждается в статической переменной */
#define yp (kp*delta)
static float yd = 0, delta_last = 0;
if (delta > 80) delta = 80;
/* интегральная часть */
yi = yi + hd*ki*delta;
if(yi < 40) yi = 40;
else if(yi > 55) yi = 55;
/* дифференциальная часть */
yd = yd + hd*( ( kd*(delta-delta_last)/hd - yd )/Td );
delta_last = delta;
return (yp + yi + yd) ;
#undef kp
#undef ki
#undef kd
#undef Td
#undef hd
#undef yp
}
P
AM
P
AM
P
AM
P
P
P
AM
P
A