#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 = 300; // 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 = 1;
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);
  //
  pinMode(26, INPUT_PULLUP);
  // Thiết lập tốc độ và các thông số khác của động cơ bước
  stepper.setMaxSpeed(500);
  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() {
  bool check = false;
  if (analogRead(26) == 0) check = true;
  if (check == true) {
    //Đọ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);

    // 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();
  }
}