int ELEMYO::BandStop (int sensorValue, int f, int BW)
{
//Вычисление времени с момента предыдущего обращения
short DT = micros()-T;
//Новый отсчет времени
T = micros();
//-----Вычисление коэффициентов фильтра-------------------
float r = 1 - 0.00000314*BW*DT;
float b1 = -2*cos(0.000006283*f*DT);
float a1 = r*b1;
float K = (1+a1+r*r)/(2+b1);
//------------------------------------------------------------------------
//Новое значение сигнала после фильтрации
Y[2]=K*(sensorValue+b1*X[1]+X[0])-a1*Y[1]-r*r*Y[0];
//----Сохранение значений x(n-1), x(n-2), y(n-1), y(n-2)
Y[0]=Y[1];
Y[1]=Y[2];
X[0]=X[1];
X[1]=sensorValue;
//--------------------------------------------------------------------
return (int) Y[2];
}