#include <Servo.h>
#include <NewPing.h>

// Define HC-SR04 ultrasonic sensor pins
#define TRIGGER_PIN 11
#define ECHO_PIN 12

// Maximum distance we want to ping for (in centimeters).
#define MAX_DISTANCE 400

// Initialize NewPing object with the pins and maximum distance
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

Servo myservo;

int servoPin = 9;
int targetAngle = 90;
int currentAngle = 90;
float Kp = 2.0;
float Ki = 0.0;
float Kd = 0.5;
float previousError = 0;
float integral = 0;
bool movingForward = true;
bool objectDetected = false;

void setup() {
  myservo.attach(servoPin);
  Serial.begin(9600);
  myservo.write(currentAngle); // Start at 90 degrees
}

void loop() {
  // Measure distance using ultrasonic sensor
  delay(50);
  unsigned int uS = sonar.ping();
  float distance = uS / US_ROUNDTRIP_CM;

  // Print distance
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  if (distance < 30) {
    objectDetected = true;
    targetAngle = 90;  // Stop at 90 degrees first
  } else if (objectDetected && currentAngle == 90) {
    // Once the object is detected and the servo stops at 90 degrees, change direction to 0
    targetAngle = 0;
    objectDetected = false;
  } else if (!objectDetected) {
    // Default condition to move from 90 to 180 degrees
    if (movingForward) {
      targetAngle = 180;
    } else {
      targetAngle = 90;
    }

    // Change direction if target reached
    if (currentAngle == 180) {
      movingForward = false;
    } else if (currentAngle == 90 && !movingForward) {
      movingForward = true;
    }
  }

  // PID controller to move the servo
  float error = targetAngle - currentAngle;
  integral += error;
  float derivative = error - previousError;
  float output = Kp * error + Ki * integral + Kd * derivative;

  currentAngle += output;
  currentAngle = constrain(currentAngle, 0, 180);

  myservo.write(currentAngle);
  previousError = error;

  delay(50); // Adjust delay for smoother movement
}