RoboduLAB : Différence entre versions

/*
  Ce programme de démonstration permet de faire fonctionner le robot d'une manière autonome
*/

#include <Servo.h> // librairie pour servomoteur 

#define broche_servoA 3 // broche servo A 
#define broche_servoB 5 // broche servo B 
#define trig 2 // broche trig du capteur US HC-SR04
#define echo 4 // broche echo du capteur US HC-SR04

const int MAX_SENS2=1000; // largeur impulsion pour position ANGLE_MIN degrés du servomoteur
const int ARRET=1490; // largeur impulsion pour position ANGLE_MEDIANE degrés du servomoteur
const int MAX_SENS1=2000; // largeur impulsion pour position ANGLE_MAX degrés du servomoteur

// classiquement : centrage sur 1500 - maxi sens 1 = 1000 et maxi sens 2 = 2000

// --- Déclaration des constantes des broches E/S numériques ---

long lecture_echo = 0; // variable sur 4 octets mesure de distance
long cm = 0; // variable sur 4 octets pour la conversion en cm

//--- Création objet servomoteur 
Servo servoA;  // crée un objet servo pour contrôler le servomoteur A (en dessous du swich ON/OFF
Servo servoB;  // crée un objet servo pour contrôler le servomoteur B (à gauche du servo A)



// ////////////////////////// 2. FONCTION SETUP = Code d'initialisation ////////////////////////// 
// La fonction setup() est exécutée en premier et 1 seule fois, au démarrage du programme

void setup()   { // debut de la fonction setup()

// --- ici instructions à exécuter 1 seule fois au démarrage du programme --- 

  pinMode (broche_servoA,OUTPUT); // Broche broche_servo configurée en sortie
  pinMode (broche_servoB,OUTPUT); // Broche broche_servo configurée en sortie
  pinMode (trig,OUTPUT); // broche broche trig configurée en sortie
  digitalWrite(trig, LOW); // met un niveau logique , LOW (BAS) sur la broche trig
  pinMode(echo, INPUT);  // la broche echo est initialisée en entree
  Serial.begin(115200);  // initialisation de la liaison série à 115200 bauds
  delay (2000); // on attend 2s
} // fin de la fonction setup()
// ********************************************************************************

////////////////////////////////// 3. FONCTION LOOP = Boucle sans fin = coeur du programme //////////////////
// la fonction loop() s'exécute sans fin en boucle aussi longtemps que l'Arduino est sous tension

void loop(){ // debut de la fonction loop()

   Avant();
   digitalWrite(trig, HIGH); // met un niveau logique , HIGH (HAUT) sur la broche trig
   delayMicroseconds(10); // attente pendant 10 microsecondes
   digitalWrite(trig, LOW);  // met un niveau logique , LOW (BAS) sur la broche trig.
   lecture_echo = pulseIn(echo, HIGH); //  lit la durée du niveau HAUT appliqué sur la broche echo
   cm = lecture_echo / 58; // conversion de la distance en cm
   Serial.print("Distance en cm : "); // affiche le message : "Distance en cm" sur le moniteur série
   Serial.println(cm); // affiche la mesure en cm avec retour à la ligne
   if (cm < 20) // si mesure < 10 => tourner sens aléatoire
   {
     if (millis()%2 == 1)
     {
        Droite();
        delay (1000); 
     }
     else
     {
        Gauche();
        delay (1000); 
     }
   }
} // fin de la fonction loop() - le programme recommence au début de la fonction loop sans fin

void Stop() {
  if (servoA.attached()) servoA.detach(); // détache le servomoteur de la broche  = arret propre servomoteur
  if (servoB.attached()) servoB.detach(); // détache le servomoteur de la broche  = arret propre servomoteur
}

void Avant() {
  if (!servoA.attached()) servoA.attach(broche_servoA); // attache le servomoteur à la broche si pas attaché
  servoA.writeMicroseconds(MAX_SENS1); // crée impulsion - sens2
  if (!servoB.attached()) servoB.attach(broche_servoB); // attache le servomoteur à la broche si pas attaché
  servoB.writeMicroseconds(MAX_SENS2); // crée impulsion - sens1
}

void Arriere() {
  if (!servoA.attached()) servoA.attach(broche_servoA); // attache le servomoteur à la broche si pas attaché
  servoA.writeMicroseconds(MAX_SENS2); // crée impulsion - sens1
  if (!servoB.attached()) servoB.attach(broche_servoB); // attache le servomoteur à la broche si pas attaché
  servoB.writeMicroseconds(MAX_SENS1); // crée impulsion - sens2
}
void Gauche() {
  if (!servoA.attached()) servoA.attach(broche_servoA); // attache le servomoteur à la broche si pas attaché
  servoA.writeMicroseconds(MAX_SENS1); // crée impulsion - sens2
  if (!servoB.attached()) servoB.attach(broche_servoB); // attache le servomoteur à la broche si pas attaché
  servoB.writeMicroseconds(MAX_SENS1); // crée impulsion - sens1
}

void Droite() {
  if (!servoA.attached()) servoA.attach(broche_servoA); // attache le servomoteur à la broche si pas attaché
  servoA.writeMicroseconds(MAX_SENS2); // crée impulsion - sens1
  if (!servoB.attached()) servoB.attach(broche_servoB); // attache le servomoteur à la broche si pas attaché
  servoB.writeMicroseconds(MAX_SENS2); // crée impulsion - sens2
}