// Aisya Rafa Maharani - 2006484210

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

// Motor Servo
#define servo_pin 21

Servo servoMotor;

int servo_start = 0;                 
int servo_target = 180;          
int servo_pos;                    

// Motor Stepper
#define STEP1 14
#define DIR1 12
#define STEP2 19
#define DIR2 18
#define motorInterfaceType 1

AccelStepper stepperMotor1(motorInterfaceType, STEP1, DIR1);
AccelStepper stepperMotor2(motorInterfaceType, STEP2, DIR2);

float stepper1_start = 0;
int stepper1_target_degree = 180;
int stepper1_target_step = 100;
int stepper1_pos;

float stepper2_start = 0;                 
int stepper2_target_degree = -180;
int stepper2_target_step = -100;
int stepper2_pos;

// Sensor Ultrasonic
#define trigPin 27
#define echoPin 26

// Kecepatan suara dalam satuan cm/uS
#define SOUND_SPEED 0.034
#define CM_TO_INCH 0.393701

long duration;
float distance_cm;
float distance_inch;


unsigned long startTime, realTime;            
int durationTime = 5000;

void setup(){
  Serial.begin(115200);

  pinMode(trigPin, OUTPUT);    
  pinMode(echoPin, INPUT);     

  stepperMotor1.setMaxSpeed(1000);
  stepperMotor2.setMaxSpeed(1000);

  stepperMotor1.setAcceleration(1000); 
  stepperMotor2.setAcceleration(1000);

  servoMotor.attach(servo_pin); 
  servoMotor.write(servo_pin);

  startTime = millis();
}

void distance(){
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  duration = pulseIn(echoPin, HIGH);
  
  distance_cm = duration * SOUND_SPEED / 2;
}

void loop(){
  distance();

  delay(10);
  if (distance_cm <= 80){
    realTime = millis();
    while(millis() - realTime <= durationTime + 10 && distance_cm <= 80 && servo_pos != servo_target){
      distance();
      stepper1_pos  = map(millis() - startTime, 0, durationTime, stepper1_pos, stepper1_target_step);
      stepper2_pos  = map(millis() - startTime, 0, durationTime, stepper2_pos, stepper2_target_step);
      servo_pos     = map(millis() - startTime, 0, durationTime, servo_pos   , servo_target);

      stepperMotor1.moveTo(stepper1_pos);
      stepperMotor2.moveTo(stepper2_pos);

      stepperMotor1.run();
      stepperMotor2.run();

      servoMotor.write(servo_pos);
    
      Serial.print("Waktu: ");
      Serial.println(millis() - startTime);
      Serial.print("Motor Servo: ");
      Serial.println(servo_pos);
      Serial.print("Motor Stepper 1: ");
      Serial.println(stepper1_pos * 360 / 200);
      Serial.print("Motor Stepper 2: ");
      Serial.println(stepper2_pos * 360 / 200);
      Serial.print("Jarak (cm): ");
      Serial.println(distance_cm);
    }

    stepper1_start = stepper1_pos;
    stepper2_start = stepper2_pos;
    stepperMotor1.stop();
    stepperMotor1.stop();
  }
  
  else{
    Serial.print("Jarak (cm): ");
    Serial.println(distance_cm);
  }
}