#include<Servo.h>
#include<PID_v1.h>


const int servoPin = 9;                                               //Servo Pin

float Kp = .08;                                                    //Initial Proportional Gain
float Ki = 0.55;                                                      //Initial Integral Gain
float Kd = 0;                                                    //Intitial Derivative Gain
double Setpoint, Input, Output, ServoOutput;

PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, REVERSE);           //Initialize PID object, which is in the class PID.

Servo myServo;                                                       //Initialize Servo.


void setup() {


  Serial.begin(9600);                                                //Begin Serial
  myServo.attach(servoPin);                                          //Attach Servo
  pinMode(7, INPUT);
  Input = pulseIn(7, HIGH);                                            //Calls function readPosition() and sets the balls

  myPID.SetMode(AUTOMATIC);                                          //Set PID object myPID to AUTOMATIC
  myPID.SetOutputLimits(-15, 15);
}

float a1, a2, a3, a4, a5, runAvg;
unsigned long duration;
unsigned long lastDuration = 0;

void loop()
{
  if(Serial.available()){
    Kp   = Serial.readString().toFloat();
  }

  Serial.print("Kp: ");
  Serial.print(Kp);
  Serial.print("\t");
  Serial.print("Kd: ");
  Serial.print(Kd);

   myPID.SetTunings(Kp, Kd, Ki);

  //Code for running average:
  duration = pulseIn(7, HIGH);
  long diff = abs(duration - lastDuration);
  if (diff <= 180) {
    a5 = a4;
    a4 = a3;
    a3 = a2;
    a2 = a1;
    a1 = duration;

    runAvg = (a1 + a2 + a3 + a4 + a5) / 5;
  }
  lastDuration = duration;

  Setpoint = 560;
  Input = runAvg;
  Serial.print("\tInput: ");
  Serial.print(Input);

  myPID.Compute();                                                   //computes Output in range of -80 to 80 degrees

  Serial.print("\tOutput: ");
  Serial.print(Output);



  ServoOutput = 110 + Output;                                        // 110 degrees is my horizontal
  Serial.print("\tServoInput: ");

  Serial.println(ServoOutput);



  myServo.write(ServoOutput);                                        //Writes value of Output to servo


}