const int TRIGPIN = 27;          
const int ECHOPIN = 26;

const int PIN_ENABLE_A = 19; //pin enable motor 1
const int PIN_IN1 = 18; //pin direction1 motor 1
const int PIN_IN2 = 5; //pin direction2 motor 1
const int PIN_IN3 = 17; //pin direction1 motor 2
const int PIN_IN4 = 16; //pin direction2 motor 2
const int PIN_ENABLE_B = 4; //pin enable motor 2

long timer;
int jarak;

void setup(){
  Serial.begin(9600);
  pinMode(ECHOPIN, INPUT);
  pinMode(TRIGPIN, OUTPUT);

  pinMode(PIN_ENABLE_A, OUTPUT);
  pinMode(PIN_IN1, OUTPUT);
  pinMode(PIN_IN2, OUTPUT);
  pinMode(PIN_IN3, OUTPUT);
  pinMode(PIN_IN4, OUTPUT);
  pinMode(PIN_ENABLE_B, OUTPUT);
}

void loop(){
  
  motor(-127, 127);
  // motor(255, -255);

  int jarakTengah = bacaSensorJarakTengah();

  Serial.print("Jarak = ");
  Serial.print(jarakTengah);
  Serial.print(" cm");
  Serial.println();

  if(jarakTengah < 20){
    Serial.println("Deteksi Halangah");
    delay(200);
  }
  // // delay(1000);
}


int bacaSensorJarakTengah(){
  digitalWrite(TRIGPIN, LOW);                   
  delayMicroseconds(2);
  digitalWrite(TRIGPIN, HIGH);                  
  delayMicroseconds(10);
  digitalWrite(TRIGPIN, LOW);                   

  timer = pulseIn(ECHOPIN, HIGH);
  jarak = timer/58;

  return jarak;
}

void motor(int SPEED_L, int SPEED_R){
  //motor 1 atau motor kiri ketika IN1 = HIGH maka motor maju
  if(SPEED_L > 0){
    analogWrite(PIN_ENABLE_A, SPEED_L); //ini untuk atur kecepatan
    digitalWrite(PIN_IN1, HIGH);
    digitalWrite(PIN_IN2, LOW);
  } else{
    analogWrite(PIN_ENABLE_A, -SPEED_L);
    digitalWrite(PIN_IN1, LOW);
    digitalWrite(PIN_IN2, HIGH);
  }

  //motor 2 atau motor kanan ketika IN3 = HIGH maka motor maju
  if(SPEED_R > 0){
    analogWrite(PIN_ENABLE_B, SPEED_R);
    digitalWrite(PIN_IN3, LOW);
    digitalWrite(PIN_IN4, HIGH);
  } else{
    analogWrite(PIN_ENABLE_B, -SPEED_R);
    digitalWrite(PIN_IN3, HIGH);
    digitalWrite(PIN_IN4, LOW);
  }
}

void stop(){
  analogWrite(PIN_ENABLE_B, 255);
  digitalWrite(PIN_IN3, HIGH);
  digitalWrite(PIN_IN4, HIGH); 
  analogWrite(PIN_ENABLE_B, 255);
  digitalWrite(PIN_IN3, HIGH);
  digitalWrite(PIN_IN4, HIGH);
}

void disableMotor(){
  analogWrite(PIN_ENABLE_B, 0);
  digitalWrite(PIN_IN3, HIGH);
  digitalWrite(PIN_IN4, HIGH); 
  analogWrite(PIN_ENABLE_B, 0);
  digitalWrite(PIN_IN3, HIGH);
  digitalWrite(PIN_IN4, HIGH);
}