#include <AccelStepper.h>
#include <PID_v1.h>

AccelStepper stepper = AccelStepper(AccelStepper::DRIVER, 4, 7); // Assuming DRIVER type

double Setpoint;
double Input;
double Output;

// PID parameters
double Kp = 1, Ki = 0.1, Kd = 0.1; // Adjust these values based on your system

// Create PID instance
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(9600);
  Setpoint = 0; // set point value
  myPID.SetMode(AUTOMATIC);
  myPID.SetTunings(Kp, Ki, Kd);
  stepper.setMaxSpeed(5000);
  stepper.setAcceleration(100);
}

void loop() {
  if (Serial.available() > 0) {
    Input = Serial.parseFloat();
  }
  myPID.Compute();
  moveStepper(Output);
  Serial.print(Input);
  Serial.print(" ");
  Serial.println(Output);
  Serial.print(" ");
  Serial.println(Setpoint);
}

void moveStepper(double position) {
  //Serial.println("Moving stepper motor");
  stepper.moveTo(position);
  while (stepper.distanceToGo() != 0) {
    stepper.run();
  }
}