#include <Servo.h>              //Using the Arduino as the controller device,the library enables to control the servo motor
Servo sg90;                     //initialize a servo object to control a servo 
#define PROPORTIONAL 15         //Initial Proportional Gain
#define INTEGRAL 0.02           //Initial Integral Gain
#define DERIVATIVE 10           //Intitial Derivative Gain

float Error1 = 0;               // define variable to calculate error 
float Error2 = 0;               //define variable to calculate error 

void setup() {
  pinMode(2, OUTPUT);           //ultasonic trig pin is connected to pin 2 of Arduino
  pinMode(3, INPUT);            //ultasonic echo pin is connected to pin 3 of Arduino
  sg90.attach(9);               // servo pin is connected to the pin 8 of arduino
  Serial.begin(9600);           // Initiate a serial communication with baud rate of 9600
  sg90.write(50);               // initial position of the servo motor 

}
void loop() {
  PID();                         // calling the function to run the code 
 
}


// function is used to calculate the position of the ball

double Total_length () {                
  digitalWrite(2, LOW);
  delayMicroseconds(4);
  digitalWrite(2, HIGH);
  delayMicroseconds(10);
  digitalWrite(2, LOW);

  double times = pulseIn(3, HIGH);
  double centimeters = times / 29 / 2;    // formula to calculate the position of the ball
  return centimeters;
}

void PID() {                       
  int distance = Total_length  ();       // the maximum position for the ball


  int Mid_Point = 15;                    //initializing the mid point 
  float fault = Mid_Point - distance;     // calculting the fault
   

//formulas to calculate pid values
 
  float P = fault * PROPORTIONAL;   
  float I = Error2 * INTEGRAL;
  float D = (fault - Error1) * DERIVATIVE;

  float total_PID = P + I + D;
  Error1 = fault;
  Error2 = fault+1;
  Serial.println(total_PID);
  int a = (int)total_PID;


a = map(a, -135, 135, 135, 0);    // to plot the values on the graph    


  if (a< 0) {
    a = 0;
  }
  if (a > 135) {
    a = 135;
  }

  sg90.write(a);
}