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

// Define the Servo object
Servo myServo;

// Define PID variables
double Setpoint, Input, Output;

// PID constants
double Kp = 2, Ki = 0, Kd = 2;

// Create PID controller
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

// Servo pin
const int servoPin = 9;

// Ultrasonic sensor pins
const int trigPin = 7;
const int echoPin = 6;

// Variables for ultrasonic sensor
long duration;
int distance;

// Function to read the current position of the servo
int readServoPosition() {
  return myServo.read();
}

// Function to measure distance using ultrasonic sensor
int measureDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;

  return distance;
}

void setup() {
  // Attach the servo to the pin
  myServo.attach(servoPin);

  // Ultrasonic sensor setup
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Initialize the input (current position)
  Input = readServoPosition();

  // Initialize the PID controller
  myPID.SetMode(AUTOMATIC);
   myPID.SetOutputLimits(0, 180);// Servo angles between 0 and 180 degrees

  Serial.begin(9600); // Initialize serial communication for debugging
}

void loop() {
  // Measure distance from ultrasonic sensor
  distance = measureDistance();

  // Check distance condition
  if (distance >= 30) {
    myPID.SetOutputLimits(90, 90);
    myServo.write(90); // Move servo to 'pos' position
      delay(500);
      myServo.write(90); // Move servo to 'pos' position
      delay(500);
  } else {
    myPID.SetOutputLimits(0, 270);
    myServo.write(180); // Move servo to 'pos' position
      delay(500);
      myServo.write(0); // Move servo to 'pos' position
      delay(500);
  }

  // Debugging: Print the values to the Serial Monitor
  
  
  Serial.print(" Input: ");
  Serial.print(Input);
  Serial.print(" Output: ");
  Serial.print(Output);
  Serial.print(" Distance: ");
  Serial.println(distance);

  // Small delay to allow the servo to move or PID to stabilize
  delay(500);
}