#include <AccelStepper.h>

#define motor1Step  16  // GPIO STEP Motor 1
#define motor1Dir   17  // GPIO DIR Motor 1
#define motor2Step  18  // GPIO STEP Motor 2
#define motor2Dir   19  // GPIO DIR Motor 2

// Konfigurasi stepper (Tipe: DRIVER)
AccelStepper stepper1(AccelStepper::DRIVER, motor1Step, motor1Dir);
AccelStepper stepper2(AccelStepper::DRIVER, motor2Step, motor2Dir);

const int stepsPerRevolution = 200;
const float degPerStep = 360.0 / stepsPerRevolution;

int targetSteps1 = (300 / degPerStep);  //step untuk 300 derajat
int targetSteps2 = (-150 / degPerStep); //step untuk -150 derajat (CCW)

const int durasi = 5000;
unsigned long startTime;
int lastSecondReported = -1;

void setup() {
    Serial.begin(115200);
    
    stepper1.setMaxSpeed(400); // Kecepatan maksimal
    stepper1.setAcceleration(200); // Akselerasi
    stepper2.setMaxSpeed(400);
    stepper2.setAcceleration(200);

    startTime = millis();
}

void loop() {
  unsigned long elapsedTime = millis() - startTime;
  int currentSecond = elapsedTime / 1000;

  if (currentSecond != lastSecondReported && currentSecond <= 5) {
    Serial.print("Detik ");
    Serial.print(currentSecond);
    Serial.print(": Stepper1: ");
    Serial.print(stepper1.currentPosition() * degPerStep);
    Serial.print("°, Stepper2: ");
    Serial.print(stepper2.currentPosition() * degPerStep);
    Serial.println("°");
    lastSecondReported = currentSecond;
  }

  float progress = (float)elapsedTime / durasi;
  int stepPos1 = progress * targetSteps1;
  int stepPos2 = progress * targetSteps2;

  stepper1.moveTo(stepPos1);
  stepper2.moveTo(stepPos2);

  stepper1.run();
  stepper2.run();
}