//------------------------------------------------------
// Programme de base 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;

// 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 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);
}

//------------------------------------------------------
// 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 fait une marche avant
    motor_left.run(BACKWARD);
    motor_right.run(BACKWARD);
  }
  else {
    // Il n'y a pas d'obstacle
    // On fait une marche avant
    motor_left.run(FORWARD);
    motor_right.run(FORWARD);
  }
}