#include <Arduino.h>
#include <PID_v1.h>
// Define motor control pins
const int motorPin1 = 2;  // Motor control pin 1
const int motorPin2 = 3;  // Motor control pin 2

// Setpoint and variables for PID control
double setpoint = 100.0;  // Target speed for the motor
double input, output;     // Estimated speed of the motor and PID output

// Define PID parameters
double Kp = 1.0;  // Proportional gain
double Ki = 0.1;  // Integral gain
double Kd = 0.01; // Derivative gain

// Create PID controller object
PID motorController(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  // Initialize motor control pins
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);

  // Set motor direction (you may need to adjust this based on your motor)
  digitalWrite(motorPin1, HIGH);
  digitalWrite(motorPin2, LOW);

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

  // Set PID sample time (in milliseconds)
  motorController.SetSampleTime(100);  // Adjust as needed

  // Set PID output limits (you may need to adjust these)
  motorController.SetOutputLimits(0, 255);

  // Enable the PID controller
  motorController.SetMode(AUTOMATIC);
}

void loop() {
  // In the absence of direct speed feedback, you can estimate the speed based on the PWM input
  input = map(analogRead(A0), 0, 1023, 0, 255);  // Adjust as needed

  // Compute the PID control output
  motorController.Compute();

  // Apply the PID control output to the motor
  analogWrite(motorPin1, int(output));  // Assuming you control speed with PWM

  // Print values for debugging
  Serial.print("SP: "); Serial.print(setpoint);
  Serial.print(" Input: "); Serial.print(input);
  Serial.print(" Output: "); Serial.print(output);
  Serial.println();

  // Add a delay to control loop execution
  delay(100);  // Adjust as needed
}