#include <Servo.h>
#define servoPin 22

Servo myservo;
const int targetPosition = 90;  // desired target position      
const float dt = 0.02;         
const float Kp = 0.5;          
const float Ki = 0.2;          
const float Kd = 0.02;         

float integral = 0;
float previousError = 0;

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

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);
  Serial1.print(currentPosition);
  
  Serial1.println(output);
  previousError = error;
  
  delay(dt * 1000);  // Convert time step to milliseconds and wait
}