Construisez. Câblez. Programmez.

/*
  EDU BOT 1.0 - ESS Technologies
  Mode : Éviteur d'obstacles

  Matériel : Arduino UNO, L298N, 2 moteurs TT,
  HC-SR04, SG90 et LCD 16x2 I2C.
*/

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo servoUltrason;

// L298N
const byte PIN_ENA = 5;
const byte PIN_IN1 = 7;
const byte PIN_IN2 = 8;
const byte PIN_ENB = 6;
const byte PIN_IN3 = 9;
const byte PIN_IN4 = 10;

// HC-SR04
const byte PIN_TRIG = 12;
const byte PIN_ECHO = 11;

// SG90
const byte PIN_SERVO = 3;
const int ANGLE_CENTRE = 90;
const int ANGLE_GAUCHE = 150;
const int ANGLE_DROITE = 30;

// Réglages
const int VITESSE_AVANT = 145;
const int VITESSE_RECUL = 130;
const int VITESSE_ROTATION = 150;
const int DISTANCE_OBSTACLE_CM = 25;
const unsigned long DUREE_RECUL_MS = 300;
const unsigned long DUREE_ROTATION_MS = 500;

unsigned long dernierAffichage = 0;

void commanderMoteur(byte pinEnable, byte pin1, byte pin2, int vitesse) {
  vitesse = constrain(vitesse, -255, 255);

  if (vitesse > 0) {
    digitalWrite(pin1, HIGH);
    digitalWrite(pin2, LOW);
    analogWrite(pinEnable, vitesse);
  } else if (vitesse < 0) {
    digitalWrite(pin1, LOW);
    digitalWrite(pin2, HIGH);
    analogWrite(pinEnable, -vitesse);
  } else {
    digitalWrite(pin1, LOW);
    digitalWrite(pin2, LOW);
    analogWrite(pinEnable, 0);
  }
}

void commanderRobot(int vitesseGauche, int vitesseDroite) {
  commanderMoteur(PIN_ENA, PIN_IN1, PIN_IN2, vitesseGauche);
  commanderMoteur(PIN_ENB, PIN_IN3, PIN_IN4, vitesseDroite);
}

void avancer()        { commanderRobot(VITESSE_AVANT, VITESSE_AVANT); }
void reculer()        { commanderRobot(-VITESSE_RECUL, -VITESSE_RECUL); }
void tournerAGauche() { commanderRobot(-VITESSE_ROTATION, VITESSE_ROTATION); }
void tournerADroite() { commanderRobot(VITESSE_ROTATION, -VITESSE_ROTATION); }
void arreterRobot()   { commanderRobot(0, 0); }

long mesurerDistanceCm() {
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(3);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  const unsigned long duree = pulseIn(PIN_ECHO, HIGH, 30000UL);
  if (duree == 0) return 400;

  return (long)(duree * 0.0343 / 2.0);
}

long mesurerDistanceMoyenne() {
  long total = 0;
  byte mesuresValides = 0;

  for (byte i = 0; i < 3; i++) {
    const long distance = mesurerDistanceCm();
    if (distance > 1 && distance < 400) {
      total += distance;
      mesuresValides++;
    }
    delay(35);
  }

  return mesuresValides == 0 ? 400 : total / mesuresValides;
}

long regarderDirection(int angle) {
  servoUltrason.write(angle);
  delay(450);
  return mesurerDistanceMoyenne();
}

void afficherEtat(const char* action, long distance) {
  if (millis() - dernierAffichage < 120) return;
  dernierAffichage = millis();

  lcd.setCursor(0, 0);
  lcd.print("MODE: EVITEUR   ");
  lcd.setCursor(0, 1);
  lcd.print("                ");
  lcd.setCursor(0, 1);
  lcd.print(action);
  lcd.print(" ");

  if (distance >= 400) lcd.print("---");
  else lcd.print(distance);

  lcd.print("cm");
}

void eviterObstacle() {
  arreterRobot();
  afficherEtat("OBSTACLE", mesurerDistanceCm());
  delay(200);

  reculer();
  afficherEtat("RECULE", 0);
  delay(DUREE_RECUL_MS);
  arreterRobot();
  delay(150);

  const long distanceGauche = regarderDirection(ANGLE_GAUCHE);
  afficherEtat("GAUCHE", distanceGauche);
  delay(150);

  const long distanceDroite = regarderDirection(ANGLE_DROITE);
  afficherEtat("DROITE", distanceDroite);
  delay(150);

  servoUltrason.write(ANGLE_CENTRE);
  delay(300);

  Serial.print("Gauche: ");
  Serial.print(distanceGauche);
  Serial.print(" cm | Droite: ");
  Serial.print(distanceDroite);
  Serial.println(" cm");

  if (distanceGauche >= distanceDroite) {
    tournerAGauche();
    afficherEtat("TOURNE G", distanceGauche);
  } else {
    tournerADroite();
    afficherEtat("TOURNE D", distanceDroite);
  }

  delay(DUREE_ROTATION_MS);
  arreterRobot();
  delay(100);
}

void setup() {
  pinMode(PIN_ENA, OUTPUT);
  pinMode(PIN_IN1, OUTPUT);
  pinMode(PIN_IN2, OUTPUT);
  pinMode(PIN_ENB, OUTPUT);
  pinMode(PIN_IN3, OUTPUT);
  pinMode(PIN_IN4, OUTPUT);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

  arreterRobot();
  Serial.begin(9600);

  lcd.init();
  lcd.backlight();

  servoUltrason.attach(PIN_SERVO);
  servoUltrason.write(ANGLE_CENTRE);

  lcd.setCursor(0, 0);
  lcd.print("EDU BOT 1.0");
  lcd.setCursor(0, 1);
  lcd.print("Mode eviteur");
  delay(2000);
  lcd.clear();
}

void loop() {
  servoUltrason.write(ANGLE_CENTRE);
  const long distanceAvant = mesurerDistanceMoyenne();

  Serial.print("Distance avant: ");
  Serial.print(distanceAvant);
  Serial.println(" cm");

  if (distanceAvant > DISTANCE_OBSTACLE_CM || distanceAvant >= 400) {
    avancer();
    afficherEtat("AVANCE", distanceAvant);
  } else {
    eviterObstacle();
  }

  delay(50);
}