//this is a practical example of implementing digital filters on the arduino
//you can use the slider to move both of the servos at the same time
//servo1 (upper one) shows the unfiltered signal
//servo2 (lower one) shows the filtered signal
//note how servo1 takes much longer to get to the desired position

#include <Servo.h> //fist we include the Servo library to make our life easier

Servo servo1; //initialize the two servos
Servo servo2;

float fc = 0.1; //cut-off frequency in Hz, play with the value and see how it affects the signal
float fn = 100.0; //sampling frequency, I measured the sampling with the logic-analyser and got about 95Hz

float wc = fc*2*3.14; 
float T = 1/fn; 
float x[] = {0,0}; 
float y[] = {0,0}; 
float a1 = -(1-(2/(T*wc)))/(1+(2/(T*wc))); 
float b1 = 1/(1+(2/(T*wc)));
float b0 = b1;

void setup() {
  servo1.attach(9);
  servo2.attach(10);
}

void loop() {
  delay(T*1000.0); 
  x[0] = analogRead(A0)+random(-50,50); 
  y[0] = b0*x[0] + b1*x[1] + a1*y[1]; 
  x[1] = x[0]; 
  y[1] = y[0];
  servo1.write(map(y[0], 0, 1023, 0, 180));  
  servo2.write(map(x[0], 0, 1023, 0, 180));
}