#include <AccelStepper.h>
#include <Encoder.h>

// Stepper Motor Pins
#define STEP_PIN 9
#define DIR_PIN 10

// Encoder Pins
#define ENCODER_PIN_A 2
#define ENCODER_PIN_B 3

AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);
Encoder encoder(ENCODER_PIN_A, ENCODER_PIN_B);

double input = 2; // Desired RPM input
double e = 0, u = 0, ehestory = 0, uhestory = 0;
long previousMillis = 0;
const long interval = 100;  // Interval for calculations
double RPM, output, pe;

void setup() {
  Serial.begin(9600);
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  stepper.setMaxSpeed(1000);
  stepper.setAcceleration(500);
}

void loop() {
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    long encoderValue = encoder.read();
    RPM = (encoderValue / 80160.0) * (60000.0 / interval);
    double Vrps = RPM * (2 * 3.1415) / 60;
    encoder.write(0);

    output = Vrps;
    e = input - output;
    pe = (e / input) * 100;
    u = uhestory + (e + ehestory) * 0.01057 + 0.0040 * (e - ehestory);

    if (u > 12) u = 12;
    if (u < 0) u = 0;

    ehestory = e;
    uhestory = u;

    stepper.setSpeed(u * 83.33); // Convert voltage to steps/sec (assuming 12V is max speed)
    stepper.runSpeed();

    Serial.print("input=");
    Serial.println(input);
    Serial.print("output=");
    Serial.println(output);
    Serial.print("%error=");
    Serial.println(pe);
    Serial.print("input voltage to the motor=");
    Serial.println(u);
    Serial.print("speed in RPM=");
    Serial.println(RPM);
  }
}