// Coding By Wira
// Robot 4 Wd


const int R1 = 2;
const int R2 = 3;
const int R3 = 4;
const int R4 = 5;

const int trig = 10;
const int echo = 11;

int PWM = 125;// PWM motor 0-255

long waktu;
int jarak;

void setup() {
  pinMode(R1, OUTPUT);
  pinMode(R2, OUTPUT);
  pinMode(R3, OUTPUT);
  pinMode(R4, OUTPUT);

  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);

  Serial.begin(9600);

}

void loop() {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  waktu = pulseIn(echo, HIGH, 30000);

  jarak = waktu * 0.034 / 2;

  if (jarak == 0) {
    jarak = 99;
  }

  if (jarak <= 25) {
    gerak_acak();
  } else {
    Maju();
  }
  delay(20);
}

void gerak_acak() {
  int acak = random(0, );

  switch (acak) {
    case 1:
      belok_U_Maju_Kanan();
    break:

    case 2:
      belok_U_Maju_Kiri();
    break:

    case 3:
      belok_U_Mundur_Kanan();
    break:

    case 4:
      belok_U_Mundur_Kiri();
    break:

    case 5:
      belok_Maju_Kanan();
    break:

    case 6:
      belok_Maju_Kiri();
    break:

    case 7:
      belok_Mundur_Kanan();
    break:

    case 8:
      belok_Mundur_Kiri();
    break:

    case 7:
      putar_balik_kanan();
    break:

    case 8:
      putar_balik_kiri();
    break:
  }
}

//  void belok U maju

void belok_U_Maju_Kanan() {
  Stop();
  delay(750);

  Mundur(); 
  delay(1150);

  Stop();
  delay(750);

  digitalWrite(R1, HIGH);
  digitalWrite(R2, LOW);
  digitalWrite(R3, LOW);
  digitalWrite(R4, LOW);
  Stop();
  delay(750);

}

void belok_U_Maju_Kiri() {
  Stop();
  delay(750);

  Mundur(); 
  delay(1150);

  Stop();
  delay(750);

  digitalWrite(R1, LOW);
  digitalWrite(R2, LOW);
  digitalWrite(R3, HIGH);
  digitalWrite(R4, LOW);
  Stop();
  delay(750);

}

//  void belok U mundur

void belok_U_Mundur_Kanan() {
  Stop();
  delay(750);

  Mundur(); 
  delay(1150);

  Stop();
  delay(750);

  digitalWrite(R1, LOW);
  digitalWrite(R2, HIGH);
  digitalWrite(R3, LOW);
  digitalWrite(R4, LOW);
  Stop();
  delay(750);

} 

void belok_U_Mundur_Kiri() {
  Stop();
  delay(750);

  Mundur(); 
  delay(1150);

  Stop();
  delay(750);

  digitalWrite(R1, LOW);
  digitalWrite(R2, LOW);
  digitalWrite(R3, LOW);
  digitalWrite(R4, HIGH);

  Stop();
  delay(750);

}

//  void belok maju

void belok_Maju_Kanan() {
  Stop();
  delay(750);

  Mundur(); 
  delay(1150);

  Stop();
  delay(750);

  digitalWrite(R1, HIGH);
  digitalWrite(R2, LOW);
  digitalWrite(R3, LOW);
  digitalWrite(R4, LOW);

  Stop();
  delay(750);

}

void belok_Maju_Kiri() {
  Stop();
  delay(750);

  Mundur(); 
  delay(1150);

  Stop();
  delay(750);

  digitalWrite(R1, LOW);
  digitalWrite(R2, LOW);
  digitalWrite(R3, HIGH);
  digitalWrite(R4, LOW);

  Stop();
  delay(750);

}

//  void belok mundur

void belok_Mundur_Kanan() {
  Stop();
  delay(750);

  Mundur(); 
  delay(1150);

  Stop();
  delay(750);

  digitalWrite(R1, LOW);
  digitalWrite(R2, HIGH);
  digitalWrite(R3, LOW);
  digitalWrite(R4, LOW);
  Stop();
  delay(750);

} 

void belok_Mundur_Kiri() {

  Stop();
  delay(750);

  Mundur(); 
  delay(1150);

  Stop();
  delay(750);

  digitalWrite(R1, LOW);
  digitalWrite(R2, LOW);
  digitalWrite(R3, LOW);
  digitalWrite(R4, HIGH);
  Stop();
  delay(750);
}

// void maju, mundur, dan stop

void Maju() {
  digitalWrite(R1, PWM);
  digitalWrite(R2, LOW);
  digitalWrite(R3, PWM);
  digitalWrite(R4, LOW);
}

void Stop() {
  digitalWrite(R1, LOW);
  digitalWrite(R2, LOW);
  digitalWrite(R3, LOW);
  digitalWrite(R4, LOW);
}

void Mundur() {
  digitalWrite(R1, LOW);
  digitalWrite(R2, PWM);
  digitalWrite(R3, LOW);
  digitalWrite(R4, PWM);
} 

void putar_balik_kanan() {
  Stop();
  delay(750);

  Mundur(); 
  delay(1150);

  Stop();
  delay(750);

  digitalWrite(R1, LOW);
  digitalWrite(R2, PWM);
  digitalWrite(R3, PWM);
  digitalWrite(R4, LOW);
  Stop();
  delay(750);
} 

void putar_balik_kiri() {
  Stop();
  delay(750);

  Mundur(); 
  delay(1150);

  Stop();
  delay(750);

  digitalWrite(R1, PWM);
  digitalWrite(R2, LOW);
  digitalWrite(R3, LOW);
  digitalWrite(R4, PWM);

  Stop();
  delay(750);
} 

// void acak delay

//belok U delay

void belok_U() {
  int belokU = random(0, 3);

  switch (belokU) {
    case 1:
      delay(3000);
      break;
    case 2:
      delay(1350);
      break;
    case 3:
      delay(2750);
      break;
  }
}

// void belok delay

void belok() {
  int belok = random(0, 3);

  switch (belok) {
    case 1:
      delay(2000);
      break;
    case 2:
      delay(1500);
      break;
    case 3:
      delay(1250);
      break;
  }
}

// void putar di tempat

void putar_di_tempatB() {
  Stop();
  delay(750);

  Mundur(); 
  delay(1150);

  Stop();
  delay(750);

  digitalWrite(R1, LOW);
  digitalWrite(R2, HIGH);
  digitalWrite(R3, HIGH);
  digitalWrite(R4, LOW);

  Stop();
  delay(750);

}

void putar_di_tempatA() {
  Stop();
  delay(750);

  Mundur(); 
  delay(1150);

  Stop();
  delay(750);

  digitalWrite(R1, HIGH);
  digitalWrite(R2, LOW);
  digitalWrite(R3, LOW);
  digitalWrite(R4, HIGH);

  Stop();
  delay(750);
}