#include <AccelStepper.h>
#include <PID_v1_bc.h>

// Định nghĩa các chân động cơ bước
#define STEP_PIN 12
#define DIR_PIN 13

// Đối tượng AccelStepper
AccelStepper stepper(1, STEP_PIN, DIR_PIN);

// Biến PID
double Setpoint = 100; // Giá trị đích mà bạn muốn đạt được (số bước)
double Input;          // Số bước hiện tại của stepper motor
double Output;

// Các hằng số PID
double Kp = 0.5;
double Ki = 0.1;
double Kd = 0.01;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  // Khởi tạo cổng Serial
  Serial.begin(9600);

  // Thiết lập tốc độ và các thông số khác của động cơ bước
  stepper.setMaxSpeed(1000);
  stepper.setAcceleration(500);

  // Khởi tạo PID
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-400, 400); // Giới hạn giá trị Output

  // Bắt đầu điều khiển động cơ bước
  stepper.setSpeed(0);
  stepper.moveTo(Setpoint);
}

void loop() {
  // Đọc số bước hiện tại của stepper motor
  Input = stepper.currentPosition();

  // Tính toán PID
  myPID.Compute();

  // Điều khiển động cơ bước bằng giá trị Output từ PID
  stepper.setSpeed(Output);
  Serial.print(Input);
  Serial.print(" ");
  Serial.println(Output);
  // Kiểm tra nếu đạt được giá trị đích
  if (stepper.distanceToGo() == 0) {
    // Khi đạt được giá trị đích, dừng stepper motor
    stepper.stop();
  }

  // Bước động cơ
  stepper.run();
}