/* TUGAS SISTEM ROBOTIKA
  KENDALI MOTOR DENGAN SENSOR ULTRASONIK
  1 motor servo dan 2 motor stepper berbarengan dalam 5 detik
  mtr1_servo   : sudut  (180)
  mtr2_stepper : +sudut (180)
  mtr3_stepper : -sudut (180)
*/

#include <AccelStepper.h>
#include <ESP32Servo.h>

// Definisi makro pin motor
#define servoPin 34
#define stepper1_DIR 18
#define stepper1_STEP 19
#define stepper2_DIR 16
#define stepper2_STEP 17

// Definisi makro pin sensor ultrasonik
#define echoPin 33
#define trigPin 32

// Konfigurasi motor stepper
AccelStepper stepper1(AccelStepper::DRIVER, stepper1_STEP, stepper1_DIR);
AccelStepper stepper2(AccelStepper::DRIVER, stepper2_STEP, stepper2_DIR);

#define DEG_PER_STEP 1.8       // 200 step per revolution
const int stepper_DEG = 180;   // target motor stepper (derajat)

int initStepper1 = 0, initStepper2 = 0;         // sudut awal motor stepper 1 dan 2
int targetStep1 = stepper_DEG / DEG_PER_STEP;   // target motor stepper 1 (step)
int targetStep2 = -stepper_DEG / DEG_PER_STEP;  // taeget motor stepper 2 (step)

int setStep_stepper1;
int setStep_stepper2;

// Konfigurasi motor servo
Servo servo;

int initServo = 0;      // sudut awal servo
int targetServo = 180;  // target motor servo (derajat)

int setPos_servo;

// Konfigurasi sensor ultrasonik
#define SOUND_SPEED 0.034
//#define CM_TO_INCH 0.393701

long durationPulse;
float distanceCm;

int startTime, errorTime, elapsed, durasi = 5000;   // durasi motor bergerak
int state = 0, state2 = 0;

// ============================= SETUP CODE ==============================
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

  pinMode(trigPin, OUTPUT);    
  pinMode(echoPin, INPUT); 
  
  stepper1.setMaxSpeed(1000);     // kecepatan maksimal
  stepper1.setAcceleration(1000); // akselerasi
  stepper2.setMaxSpeed(1000);
  stepper2.setAcceleration(1000);

  servo.attach(servoPin); 
  servo.write(initServo);

  startTime = millis();
}


// ============================= LOOP CODE ===============================
void loop() {
  // Clear trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  
  // Pengukuran sensor ultrasonik
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  durationPulse = pulseIn(echoPin, HIGH);
  distanceCm = durationPulse * SOUND_SPEED / 2;
  //distanceInch = distanceCm * CM_TO_INCH;

  if(distanceCm <= 50 && state == 0) {
    errorTime = millis();
    state = 1;
  }

  delay(10);

  elapsed = millis() - errorTime;

  if(millis() - errorTime <= durasi && state == 1) {
    setStep_stepper1 = map(elapsed, 0, durasi, initStepper1, targetStep1);
    setStep_stepper2 = map(elapsed, 0, durasi, initStepper2, targetStep2);
    setPos_servo     = map(elapsed, 0, durasi , initServo, targetServo);
    
    stepper1.moveTo(setStep_stepper1);
    stepper2.moveTo(setStep_stepper2);

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

    servo.write(setPos_servo);

    Serial.print("Time taken : ");
    if (millis() - startTime - errorTime >= durasi) { 
      Serial.print(5000); 
    } else { 
        Serial.print(millis() - startTime - errorTime); 
    }
    Serial.print(" | Sudut stepper 1 : ");
    Serial.print(setStep_stepper1 * DEG_PER_STEP);
    Serial.print(" | Sudut stepper 2 : ");
    Serial.print(setStep_stepper2 * DEG_PER_STEP);
    Serial.print(" | Servo angle : ");
    Serial.print(setPos_servo);
    Serial.print(" | Disctance (cm) : ");
    Serial.println(distanceCm);

    if(setPos_servo >= 180) {
      state2 = 1;
    }

  }
  
  if(state2 == 1) {
    stepper1.stop();
    stepper2.stop();
    servo.detach();
  }

}