// http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
/*working variables*/
unsigned long lastTime;
double Input, Output, Setpoint;
double errSum, lastErr;
double kp, ki, kd, eps;
void Compute()
{
/*How long since we last calculated*/
unsigned long now = millis();
double timeChange = (double)(now - lastTime);
 
/*Compute all the working error variables*/
double error = Setpoint - Input;

if(abs(error)<2)
{
  error=0;
  errSum=0;
  lastErr=0;

}
errSum += (error * timeChange);
double dErr = (error - lastErr) / timeChange;
 
/*Compute PID Output*/
Output = kp * error + ki * errSum + kd * dErr;
 
/*Remember some variables for next time*/
lastErr = error;
lastTime = now;
}
 
void SetTunings(double Kp, double Ki, double Kd)
{
kp = Kp;
ki = Ki;
kd = Kd;
}


void setup() {
  // put your setup code here, to run once:
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(3, OUTPUT);
 Serial.begin(115200);
 Serial.println("Servo PID");
 SetTunings(1.1,0.5,0.6);
}

void loop() {
  // put your main code here, to run repeatedly:
  Setpoint=analogRead(A0);
  Input=analogRead(A1);
  
  Compute();
  l
  analogWrite(3,constrain(Setpoint, 0, 255););
  delay(500);
  Serial.println(Output);
}