//------------------------------------------------------
// Programme pour utiliser le Robot avec capteur ultrasons
//------------------------------------------------------


// Librairie pour la carte shield Motor Controller
#include <AFMotor.h>

//------------------------------------------------------
// Définitions des constantes :
//------------------------------------------------------
// PINs pour les moteurs
// Numéro de connecteur Moteur GAUCHE
#define MOTOR_LEFT 1
// Numéro de connecteur Moteur DROITE
#define MOTOR_RIGHT 2
// PINs pour le capteur ultrasons
#define ULTRA_TRIG 14
#define ULTRA_ECHO 15
// Distance minimale pour tourner, en cm
#define MIN_DISTANCE 15


//------------------------------------------------------
// Définitions des variables utilisées dans le programme
//------------------------------------------------------
// Distance de l'obstacle, en cm
float distance_cm = 0;

// Nombre aléatoire pour les choix de direction ou vitesse
long rand_num = 0;

// Moteurs gauche et droite
AF_DCMotor motor_left(MOTOR_LEFT, MOTOR12_64KHZ);
AF_DCMotor motor_right(MOTOR_RIGHT, MOTOR12_64KHZ);

// Vitesse des 2 moteurs, à l'initial = 200
long motor_speed = 200;


//------------------------------------------------------
// Fonction de mesure de la distance
float measure_distance() {
  // Définition des variables utilisées dans la fonction
  // Largeur d'impulsion
  long largeur_impulsion;
  // Distance calculée à partir de la largeur d'impulsion
  float distance;
  
  // Emettre un pulse de 10µs sur TRIG
  digitalWrite(ULTRA_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(ULTRA_TRIG, LOW);

  // Mesurer la largeur d'impulsion sur ECHO
  largeur_impulsion = pulseIn(ULTRA_ECHO, HIGH);

  // Calcul de la distance, en considérant que la vitesse du son = 340 m/s
  distance = largeur_impulsion * 0.017;

  // Affichage de la distance sur le moniteur série
  Serial.print("Distance = ");
  Serial.print(distance);
  Serial.println(" cm");

  // Retourne la distance mesurée
  return distance;
}

//------------------------------------------------------
// Fonction qui permet de tourner à droite
void turn_right() {
  motor_left.run(FORWARD);
  motor_right.run(RELEASE);
}

//------------------------------------------------------
// Fonction qui permet de tourner à gauche
void turn_left() {
  motor_left.run(RELEASE);
  motor_right.run(FORWARD);
}

//------------------------------------------------------
// Fonction qui permet d'arrêter le robot
void stop_robot() {
  motor_left.run(RELEASE);
  motor_right.run(RELEASE);
}

//------------------------------------------------------
// Fonction d'initialisation du programme
void setup() {
  // Réglage des PINs en Sortie
  pinMode(MOTOR_LEFT, OUTPUT);
  pinMode(MOTOR_RIGHT, OUTPUT);
  pinMode(ULTRA_TRIG, OUTPUT);
  
  // Réglage des PINs en Entrée
  pinMode(ULTRA_ECHO, INPUT);

  // Initialisation du moniteur série
  Serial.begin(9600);
  // Attente que le moniteur série soit prêt
  while (!Serial) {
    ;
  }

  // Met la PIN TRIG à 0
  digitalWrite(ULTRA_TRIG, LOW);

  // Vitesse des moteurs
  motor_left.setSpeed(motor_speed);
  motor_right.setSpeed(motor_speed);

  // Arrêt du robot
  stop_robot();
  // Attente de 5 secondes
  delay(5000);
}

//------------------------------------------------------
// Fonction exécutée en boucle
void loop() {
  // Mesure de la distance
  distance_cm = measure_distance();

  // Si l'obstacle est à moins de MIN_DISTANCE cm
  if (distance_cm < MIN_DISTANCE) {
    // On arrête
    stop_robot();

    // On attend 1 seconde
    delay(1000);
    
    // On fait un tirage aléatoire entre 2 possibilités : 0 ou 1 pour déterminer la direction
    rand_num = random(2);

    // Si 0 -> Droite
    if (rand_num == 0) {
      turn_right();
    }
    else {
      // Si 1 -> Gauche
      turn_left();
    }

    // On change la vitesse aléatoirement, entre 150 et 255
    motor_speed = random(150, 256);
    motor_left.setSpeed(motor_speed);
    motor_right.setSpeed(motor_speed);

    // Attente aléatoire entre 2 secondes et 5 secondes
    rand_num = random(2000, 5000);
    delay(rand_num);
  }
  else {
    // Il n'y a pas d'obstacle
    // On fait une marche avant
    motor_left.run(FORWARD);
    motor_right.run(FORWARD);
  }
}