// PID constants
float Kp = 2.0;    // Proportional constant
float Ki = 5.0;    // Integral constant
float Kd = 1.0;    // Derivative constant

// Variables
float setpoint = 900;   // Desired motor speed (RPM)
float input = 0;        // Actual motor speed (measured by a sensor)
float output = 0;       // Output to motor (PWM value)
float lastInput = 0;    // Last motor speed
float integral = 0;     // Integral sum
float error = 0;        // Difference between setpoint and actual speed
float lastError = 0;
unsigned long lastTime = 0;

void setup() {
  Serial.begin(9600);
  pinMode(9, OUTPUT); // PWM pin for motor control
  lastTime = millis();  // Initialize time
}

void loop() {
  // Read actual speed (input) from a sensor, such as an encoder or potentiometer
  input = analogRead(A0);  // Assume a value between 0 and 1023, scaled for motor speed
  input = map(input, 0, 1023, 0, 255);  // Map the sensor value to speed range (RPM)

  // Call the PID control function
  PIDControl();

  // Output the motor control value
  analogWrite(9, output);

  // Display the results
  Serial.print("Setpoint: ");
  Serial.println(setpoint);
  Serial.print(" RPM, Input: ");
  Serial.print(input);
  Serial.print(" RPM, Output: ");
  Serial.println(output);

  delay(2000);  // Loop every 100ms
}

void PIDControl() {
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - lastTime) / 1000.0;  // Time since last loop in seconds
  
  // Calculate the error
  error = setpoint - input;

  // Proportional term
  float proportional = Kp * error;

  // Integral term (summing up error over time)
  integral += error * deltaTime;
  float integralTerm = Ki * integral;

  // Derivative term (rate of change of error)
  float derivative = (input - lastInput) / deltaTime;
  float derivativeTerm = Kd * derivative;

  // Calculate the PID output
  output = proportional + integralTerm - derivativeTerm;

  // Constrain the output to a range that is valid for PWM (0-255)
  output = constrain(output, 0, 255);

  // Update the last input and time for the next loop
  lastInput = input;
  lastTime = currentTime;
}