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

Servo myServo;

double Setpoint, Input, Output;

double Kp = 2.0, Ki = 1.0, Kd = 0.0;

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

const int servoPin = 9;

int readServoPosition() {

  return myServo.read();
}

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

  // Initialize the setpoint (desired position)
  Setpoint = 90; // Example: 90 degrees

  // 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() {
  // Update the input value
  Input = readServoPosition();

  // Compute the PID output
  myPID.Compute();

  // Adjust the servo position based on PID output
  myServo.write(Output);

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

  delay(50);
}