#include <Servo.h>

#define trigPin 9
#define echoPin 10

Servo myservo;

int sp = 150;

float kp = 1.18;
float ki = 0.67;
float kd = 0.15;

float p,i,d,suhu,pid,pidx;
float error,errorx,sumerr;
long duration, distance;

void setup() {
  myservo.attach(3);
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  }

void loop() {

  error = sp - distance;
  p = error * kp;
  sumerr = error + errorx;
  i = ki * sumerr;
  d = error - errorx;
  pid = p + i + d;
  pid = 250.0 - pid;
 
  pidx = sp - pid;
 
  myservo.write(pidx);

  if(pidx < 1){
  pidx = 0;
  }
 
  if(pidx > 250){
  pidx = 250;
  }


  digitalWrite(trigPin, LOW);  // Added this line
  delayMicroseconds(2); // Added this line
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); // Added this line
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2) / 29.1;
  Serial.print(distance);
  Serial.print(" cm");
  Serial.print("PID= ");
  Serial.println(pidx);
  delay(200);

 errorx = error;

}