#include <AccelStepper.h>

// Định nghĩa chân GPIO cho các động cơ bước
#define X_STEP_PIN 19
#define X_DIR_PIN  18

#define Y_STEP_PIN 13
#define Y_DIR_PIN  12

#define Z_STEP_PIN 5
#define Z_DIR_PIN  4

// Tạo các đối tượng AccelStepper cho các trục X, Y, Z
AccelStepper stepperX(1, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepperY(1, Y_STEP_PIN, Y_DIR_PIN);
AccelStepper stepperZ(1, Z_STEP_PIN, Z_DIR_PIN);

// Khai báo chân kết nối nút bấm
#define NUT1 34

bool trangThaiNutTruocDo = HIGH;

void setup() {
  //khai báo là cổng vào
  pinMode(NUT1, INPUT_PULLUP);

  // Đặt tốc độ và gia tốc mặc định cho các động cơ
  stepperX.setMaxSpeed(2000); // Tốc độ tối đa (steps per second)
  stepperX.setAcceleration(1000); // Gia tốc (steps per second per second)
  
  stepperY.setMaxSpeed(2000);
  stepperY.setAcceleration(1000);
  
  stepperZ.setMaxSpeed(500);
  stepperZ.setAcceleration(100);
}

void loop() {
  //Đọc trạng thái nút bấm
  bool trangThaiNut = digitalRead(NUT1);

  // Kiểm tra nút có được ấn ko. Phải so sánh với trạng thái trước 
  // đó để chỉ thực hiện lệnh 1 lần khi bấm tránh thực hiện nhiều 
  // lần khi bấm giữ hoặc tốc độ đọc nhanh hơn nhiều lần tốc độ bấm
  if (trangThaiNut == LOW) && (trangThaiNutTruocDo == HIGH) {
    chayMotor1();
    // Cập nhật trạng thái nút
    trangThaiNutTruocDo = trangThaiNut;
  }

  // Cập nhật trạng thái nút
  trangThaiNutTruocDo = trangThaiNut;

  // Di chuyển các động cơ bước theo hướng và khoảng cách mong muốn
  stepperY.moveTo(2000);
  stepperZ.moveTo(-3000);

  // Thực hiện di chuyển
  stepperY.run();
  stepperZ.run();
}
void chayMotor1() {
  stepperX.moveTo(1000);
  stepperX.run();
}