// TOUMA · Junior Inventeur · robot qui évite les obstacles
// Robot TOUMA : avance si le chemin est libre (>= 20 cm), sinon s'arrête et tourne à droite.
const int ENA = 5, ENB = 6, IN1 = 2, IN2 = 3, IN3 = 4, IN4 = 7;
const int TRIG = 9, ECHO = 10;

long distanceCm() {
  digitalWrite(TRIG, LOW); delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  long d = pulseIn(ECHO, HIGH, 30000);   // attendre l'écho
  return d == 0 ? 400 : d / 58;          // rien vu = loin
}
void moteurs(int a, int b, int c, int d, int vitesse) {
  digitalWrite(IN1, a); digitalWrite(IN2, b);
  digitalWrite(IN3, c); digitalWrite(IN4, d);
  analogWrite(ENA, vitesse); analogWrite(ENB, vitesse);
}
void avancer()       { moteurs(HIGH, LOW, HIGH, LOW, 110); }
void tournerDroite() { moteurs(HIGH, LOW, LOW, HIGH, 110); }
void arreter()       { moteurs(LOW, LOW, LOW, LOW, 0); }

void setup() {
  int sorties[] = {ENA, ENB, IN1, IN2, IN3, IN4, TRIG};
  for (int i = 0; i < 7; i++) pinMode(sorties[i], OUTPUT);
  pinMode(ECHO, INPUT);
  arreter();
  delay(2000);                            // 2 s pour poser le robot
}
void loop() {
  if (distanceCm() >= 20) {
    avancer();
  } else {
    arreter(); delay(300);
    tournerDroite(); delay(400);
  }
  delay(60);
}