#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;
}