La Féte du Nombre PI

/*

Ce programme permet de faire fonctionner automatiquelent la petite voiture en

– déviant la trajectoire quand un obstacle est détecté à 50cm à gauche ou a droite et infléchir la direction dans le sens opposé à l’obstacle

– détectant un obstacle proche (10cm) et en declenchant une manoeuvre de recul et de rotation pour repartir

– ajout de 2 détecteurs ultrasons arrière pour quand la voiture recule

J’ai transféré sur le module I2C certaines commandes de moteur pour utiliser les IO de l’arduino pour les détectuers ultrasons supplémentaires

connexion série via le module bluetooth HC06 (branché sur port 0(RX) et 1 TX (arduino) en croisé (TX HC06 => RX arduino (1), RX HC06 => TX Arduino((1)

*/

#include <NewPing.h> // librairie pour les détecteurs ultrasons

#include <PCF8574.h> // librairie pour le module d’extension I2C

PCF8574 expander1 ; // créé une instance expander1 à partir de la classe PCF8574 (module d’extension I2C)

// adresse du bus 8574A avec les 3 cavaliers A0,A1 et A2 sur position « -« , valeur 56 en hexadecimal soit 0x38 en hexa, valeur utilisée dans la commande init

// Pins utilisées (utilisation de l’expander1 pour les moteurs

// allocation des pins PWM arduino utilisées pour connecter le controle de vitesse moteur (enA et enB)

// les roues d’un même coté sont branchées sur le même moteur

// moteur un

int enA = 5; // permet de définir la vitesse

int exp_in1 = 0; // en jumelage avec int2 permet de définir le sens de rotation (avant in1= HIGH, in2= LOW), connexion sur expander

int exp_in2 = 1; // en jumelage avec int1 permet de définir le sens de rotation (arriere in1= LOW, in2 = HIGH , connexion sur expander

// moteur deux

int enB = 6;

int exp_in3 = 2; // connexion sur expander

int exp_in4 = 3; // connexion sur expander

int nbImpulsion_d = 0; // sert à compter les impulsions lues par la fourche

int nbImpulsion_g = 0; // roues gauches: idem que pour roues droites

int memoire_impulsions_d = 0; // sert à accumuler les impulsions lues par la fourche (permet de correler à la vitesse et à la distance parcourue)

int memoire_impulsions_g = 0; // idem pour roues gauche

int temps_reference_d =0;

int temps_reference_g =0;

int avance = 0; // vitesse d’avance de la voiture (commande moteur)

int droitegauche = 0; // permet de controler le virage à droite (si positif) ou à gauche (si négatif)

int rouesdroites = 0; // calcul de la vitesse des roues droites en fonction de l’avance et du virage éventuel (le virage se fait en accélérant les roues d’un coté par rapport à l’autre)

int rouesgauches = 0; // calcul de la vitesse des roues droites en fonction de l’avance et du virage éventuel

int distance_obstacle_ad = 0; //distance obstacle avant droit

int distance_obstacle_ag = 0; //distance obstacle avant gauche

int distance_obstacle_ard = 0; //distance obstacle arrière droit

int distance_obstacle_arg = 0; //distance obstacle arrière gauche

const int fourchePin_d = 2; //Sortie de la fourche droite (mesure vitesse roue droite)

const int fourchePin_g = 3; //Sortie de la fourche gauche (mesure vitesse roue gauche)

const int TURNDISTANCE = 50; // définit la distance mini de détection d’un obstacle pour lancer la procédure d’évitemment

const int MINDISTANCE = 10; // distance minimum pour enclancher l’arrêt de la voiture et le recul

const int LOWSPEED = 70; // vitesse lente pour manoeuvre lors de la détection d’obstacle

const int HIGHSPEED = 100; // vitesse rapide quand aucun obstacle n’est détecté

// 1er détecteur

int TRIGGER_PIN_ad = 10; // Arduino trigger pin avant-droit pour le senseur ultrasons (envoi d’impulsion)

int ECHO_PIN_ad = 11; // Arduino Echo pin avant-droit pour le senseur ultrasons (lecture retour d’impulsion)

int MAX_DISTANCE_ad = 200 ; // Distance Maximum de ping en centimetres. (donc on ne tient pas compte des obstacles détectés au delà de cette valeur) (valeur pour ce parametre et ce type de senseur 400-500cm.)

// 2e détecteur

int TRIGGER_PIN_ag = 12; // idem au dessus pour avant gauche.

int ECHO_PIN_ag = 13; // idem au dessus pour avant gauche.

int MAX_DISTANCE_ag = 200 ; // idem au dessus pour avant gauche.

// 3e détecteur

int TRIGGER_PIN_ard = 4; // idem au dessus pour arriere droit.

int ECHO_PIN_ard = 7; // idem au dessus pour arriere droit.

int MAX_DISTANCE_ard = 50 ; // idem au dessus pour arriere droit.

// 4e détecteur

int TRIGGER_PIN_arg = 8; /// idem au dessus pour arriere gauche.

int ECHO_PIN_arg = 9; // idem au dessus pour arriere gauche.

int MAX_DISTANCE_arg = 50 ; // idem au dessus pour arriere gauche.

NewPing sonar_ad(TRIGGER_PIN_ad, ECHO_PIN_ad, MAX_DISTANCE_ad); // création de l’instance sonar pour le détecteur avant droit

NewPing sonar_ag(TRIGGER_PIN_ag, ECHO_PIN_ag, MAX_DISTANCE_ag); // création de l’instance sonar pour le détecteur avant gauche

NewPing sonar_ard(TRIGGER_PIN_ard, ECHO_PIN_ard, MAX_DISTANCE_ard); // création de l’instance sonar pour le détecteur arrière droit

NewPing sonar_arg(TRIGGER_PIN_arg, ECHO_PIN_arg, MAX_DISTANCE_arg); // création de l’instance sonar pour le détecteur arrière gauche

void comptageDUneImpulsion_d() {

nbImpulsion_d ++; // procédure d’incrémentation du compteur d’impulsions droit

}

void comptageDUneImpulsion_g() {

nbImpulsion_g ++; // procédure d’incrémentation du compteur d’impulsions gauche

}

// ce programme envoi des PING toutes les 50ms afin de détecter les obstacles éventuels

// en cas de détection il retourne les distances en cm des obstacles détectés

void detection_obstacle() {

delay(50); // PING toutes les 50ms (environ 20 pings/sec).

unsigned int uS1 = sonar_ad.ping(); // envoi du ping, récupère le temps d’écho en microseconds (uS1).

distance_obstacle_ad = uS1 / US_ROUNDTRIP_CM; // detection coté avant droit, calcul de la distance en fonction du temps de retour d’écho (valeur = 0 si en dehors de la zone de détection)

unsigned int uS2 = sonar_ag.ping(); // idem ci-dessus pour détecteur avant gauche (uS2).

distance_obstacle_ag = uS2 / US_ROUNDTRIP_CM; // idem ci-dessus pour détecteur avant gauche

unsigned int uS3 = sonar_ard.ping(); // idem ci-dessus pour détecteur arriere droit (uS3).

distance_obstacle_ard = uS3 / US_ROUNDTRIP_CM; // idem ci-dessus pour détecteur arriere droit

unsigned int uS4 = sonar_arg.ping(); // idem ci-dessus pour détecteur arriere gauche (uS4).

distance_obstacle_arg = uS4 / US_ROUNDTRIP_CM; // idem ci-dessus pour détecteur arriere gauche

/*

Serial.print(« Ping: « ); //(code utilisé pour vérifier le fonctionnement des détecteurs)

Serial.print(« distance obstacle avant droit:  » );

Serial.print(distance_obstacle_ad);

Serial.println( » cm »);

Serial.print(« distance obstacle avant gauche:  » );

Serial.print(distance_obstacle_ag);

Serial.println( » cm »);

Serial.print(« distance obstacle arriere droit:  » );

Serial.print(distance_obstacle_ard);

Serial.println( » cm »);

Serial.print(« distance obstacle arriere gauche:  » );

Serial.print(distance_obstacle_arg);

Serial.println( » cm »);

*/

}

/* programme de commande des moteurs, avance (vitesse avant si positif, vitesse de recul si négatif), droitegauche (positif pour tourner à fdroite, négatif pour tourner à gauche)

– avance peut varier de -255 à +255

– droitegauche peut varier de -40 à +40

*/

void commande_moteur(int avance, int droitegauche) {

if (avance >= 0 ) { // moteur marche avant

expander1.digitalWrite(exp_in1, HIGH);

expander1.digitalWrite(exp_in2, LOW);

expander1.digitalWrite(exp_in3, HIGH);

expander1.digitalWrite(exp_in4, LOW);

}

else { // moteur marche arrière

expander1.digitalWrite(exp_in1, LOW);

expander1.digitalWrite(exp_in2, HIGH);

expander1.digitalWrite(exp_in3, LOW);

expander1.digitalWrite(exp_in4, HIGH);

}

if (droitegauche > 0) { //tourner à droite, les roues gauches (enA) vont plus vite

// vitesse basé sur le parametre « avance » (range 0-255) corrigé en fonction de la direction

rouesdroites = abs (avance);

rouesgauches = abs(avance) + abs(droitegauche);

}

else { //tourner à gauche, les roues droites (enB) vont plus vite

rouesdroites = abs(avance) + abs(droitegauche) ;

rouesgauches = abs (avance);

}

analogWrite(enA, rouesgauches);

analogWrite(enB, rouesdroites);

}

void setup() {

// définit les PINs moteur et détecteurs ultrasons en entrée ou sortie

pinMode(enA, OUTPUT); // commande moteur 1

pinMode(enB, OUTPUT); // commande moteur 2

pinMode (ECHO_PIN_ad, INPUT);

pinMode (TRIGGER_PIN_ad, OUTPUT);

pinMode (ECHO_PIN_ag, INPUT);

pinMode (TRIGGER_PIN_ag, OUTPUT);

pinMode (ECHO_PIN_ard, INPUT);

pinMode (TRIGGER_PIN_ard, OUTPUT);

pinMode (ECHO_PIN_arg, INPUT);

pinMode (TRIGGER_PIN_arg, OUTPUT);

pinMode(A4, INPUT); // Entrée bus I2C sur l’arduinio

pinMode(A5, INPUT); // Entrée bus I2Csur l’arduinio

digitalWrite(A4, LOW); // état démarrage bus I2C

digitalWrite(A5, LOW); // état démarrage bus I2C

Serial.begin(9600); //connection au port série pour debug

expander1.begin(0x38); // démarrage du bus I2C pour module d’extension

expander1.pinMode(exp_in1, OUTPUT); // commande moteur 1 en sortie

expander1.pinMode(exp_in2, OUTPUT); // commande moteur 1 en sortie

expander1.pinMode(exp_in3, OUTPUT); // commande moteur 2 en sortie

expander1.pinMode(exp_in4, OUTPUT); // commande moteur 2 en sortie

pinMode(fourchePin_d, INPUT); //fourche optique droite de mesure de vitesse en entrée

pinMode(fourchePin_g, INPUT); //fourche optique gauche de mesure de vitesse en entrée

Serial.println(« Fourche optique – detection de presence »);

attachInterrupt(digitalPinToInterrupt(2), comptageDUneImpulsion_d, CHANGE); // comptage d’impulsion droit via le mode interrupt

attachInterrupt(digitalPinToInterrupt(3), comptageDUneImpulsion_g, CHANGE); // comptage d’impulsion gauche via le mode interrupt

}

void loop() {

detection_obstacle();

if (distance_obstacle_ad <= MINDISTANCE && distance_obstacle_ad != 0 || distance_obstacle_ag <= MINDISTANCE && distance_obstacle_ag != 0 )

{ // arret quand on se rapproche trop près d’un obstacle

expander1.digitalWrite(exp_in1, LOW);

expander1.digitalWrite(exp_in2, LOW);

expander1.digitalWrite(exp_in3, LOW);

expander1.digitalWrite(exp_in4, LOW);

// on recule jusqu’à que l’on ne détecte plus l’obstacle et on tourne à droite

do {

detection_obstacle();

commande_moteur(-50, 200);

}

while (distance_obstacle_ad <= 20 || distance_obstacle_ag <= 20 || ( distance_obstacle_ard > 5 && distance_obstacle_arg > 5));

// on recule jusquà etre à 20cm de l’obstacle sauf si il y a un obstacle arriere à moins de 5cm

}

else {// on continue à avancer

detection_obstacle();

if (distance_obstacle_ad < TURNDISTANCE || distance_obstacle_ag < TURNDISTANCE ) {

if (distance_obstacle_ad < distance_obstacle_ag) {

commande_moteur(LOWSPEED, -100); // on inflechit la trajectoire vers la gauche }

else {

commande_moteur(LOWSPEED, 100); //on infléchit la trajectoire vers la droite}

}

else {

commande_moteur(HIGHSPEED, 00);}

}

Serial.print (« impulsions droite: « );

Serial.print (nbImpulsion_d);

Serial.print ( » impulsions gauche: « );

Serial.println (nbImpulsion_g);

}