#include <ESP32Servo.h>

// Servo myservo;  // create servo object to control a servo
// // twelve servo objects can be created on most boards

// int pos = 0;    // variable to store the servo position

// void setup() {
//   myservo.attach(18);  // attaches the servo on pin 13 to the servo object
// }

// void loop() {
//   for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
//     // in steps of 1 degree
//     myservo.write(pos);              // tell servo to go to position in variable 'pos'
//     delay(15);                       // waits 15ms for the servo to reach the position
//   }
//   for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
//     myservo.write(pos);              // tell servo to go to position in variable 'pos'
//     delay(15);                       // waits 15ms for the servo to reach the position
//   }
//   myservo.write(pos);
// }


Servo myservo;
const int targetPosition = 180;  // Your desired target position (in degrees)
const int servoPin = 18;        // Connect the servo signal wire to GPIO 2
const float dt = 0.02;         // Time step (adjust as needed)
const float Kp = 0.5;          // Proportional gain
const float Ki = 0.1;          // Integral gain
const float Kd = 0.01;         // Derivative gain

float integral = 0;
float previousError = 0;

void setup() {
  myservo.attach(servoPin);
  myservo.write(0);  // Initialize the servo to the middle position
  delay(500);
}

void loop() {
  float currentPosition = myservo.read();  // Read the current position of the servo
  float error = targetPosition - currentPosition;
  
  // Calculate the PID components
  float proportional = Kp * error;
  integral += Ki * error * dt;
  float derivative = Kd * (error - previousError) / dt;
  
  // Calculate the control output
  float output = proportional + integral + derivative;
  
  // Apply the control output to the servo
  myservo.write(currentPosition + output);
  previousError = error;
  
  delay(dt * 1000);  // Convert time step to milliseconds and wait
}