#include <PID_v1.h>

// Define the input, output, and setpoint variables
double Setpoint, Input, Output;

// Specify the PID tuning parameters
double Kp = 2.0, Ki = 5.0, Kd = 1.0;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

// Motor control pins
const int motorPin1 = 18;
const int motorPin2 = 19;

// Encoder input pins
const int encoderPin1 = 34;
const int encoderPin2 = 35;

// Variables for encoder
volatile int encoderPos = 0;
int encoderPosLast = 0;

// Timer for PID sampling
unsigned long lastTime = 0;
unsigned long sampleTime = 100; // 100ms

void setup() {
  // Set motor control pins as outputs
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);

  // Set encoder pins as inputs
  pinMode(encoderPin1, INPUT);
  pinMode(encoderPin2, INPUT);

  // Attach interrupts for encoder
  attachInterrupt(digitalPinToInterrupt(encoderPin1), readEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPin2), readEncoder, CHANGE);

  // Initialize PID controller
  Setpoint = 200; // Desired RPM
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-255, 255);

  Serial.begin(115200);
}

void loop() {
  // Read the current speed from the encoder
  Input = readRPM();

  // Run the PID computation if sample time has passed
  if (millis() - lastTime >= sampleTime) {
    lastTime = millis();
    myPID.Compute();
    
    // Control the motor based on PID output
    if (Output > 0) {
      analogWrite(motorPin1, Output);
      analogWrite(motorPin2, 0);
    } else {
      analogWrite(motorPin1, 0);
      analogWrite(motorPin2, -Output);
    }

    Serial.print("RPM: ");
    Serial.println(Input);
  }
}

// Encoder reading function
void readEncoder() {
  int state1 = digitalRead(encoderPin1);
  int state2 = digitalRead(encoderPin2);
  if (state1 == state2) {
    encoderPos++;
  } else {
    encoderPos--;
  }
}

// Function to calculate RPM
double readRPM() {
  int deltaPos = encoderPos - encoderPosLast;
  encoderPosLast = encoderPos;
  return (deltaPos / 20.0) * 60.0; // Convert to RPM
}