#define SL_t 22
#define SL_e 34

#define SR_t 23
#define SR_e 35

#define SB_t 5
#define SF_t 5


#define M1 18
#define M2 19
#define M3 2
#define M4 4
int dl, dr, df, db;

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

  pinMode(SL_t, OUTPUT);
  pinMode(SL_e, INPUT);
  pinMode(SR_t, OUTPUT);
  pinMode(SR_e, INPUT);

  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
  pinMode(M3, OUTPUT);
  pinMode(M4, OUTPUT);
}

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

  dl = pulseIn(SL_e, HIGH) * 0.17;


  digitalWrite(SR_t, HIGH);
  delayMicroseconds(10);
  digitalWrite(SR_t, LOW);

  dr = pulseIn(SR_e, HIGH) * 0.17;

  if (dr <= 1000) {
    right();
    Serial.println("TERRAIN AUTO AVOIDANCE! MOVED LEFT");
    delay(1000);
  } else if (dl <= 1000) {
    left();
    Serial.println("TERRAIN AUTO AVOIDANCE! MOVED RIGHT");
    delay(1000);
  } else { all(); }
  
  delay(500);
}

void listen_usc(int pin) {
  digitalWrite(pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(pin, LOW);
}

void right () {
  digitalWrite(M1, HIGH);
  digitalWrite(M4, HIGH);  
  digitalWrite(M2, LOW);
  digitalWrite(M3, LOW); 
}

void left () {
  digitalWrite(M2, HIGH);
  digitalWrite(M3, HIGH); 
  digitalWrite(M1, LOW);
  digitalWrite(M4, LOW);  
}

void front () {
  digitalWrite(M1, HIGH);
  digitalWrite(M2, HIGH);
  digitalWrite(M3, LOW);
  digitalWrite(M4, LOW);   
}

void back () {
  digitalWrite(M3, HIGH);
  digitalWrite(M4, HIGH);  
  digitalWrite(M1, LOW);
  digitalWrite(M2, LOW);
}

void all () {
  digitalWrite(M1, HIGH); 
  digitalWrite(M2, HIGH); 
  digitalWrite(M3, HIGH); 
  digitalWrite(M4, HIGH); 
}