// **************************************************************************** // * TEST ASSERVISSEMENT VIDIROB * // * * // * Plateforme : ARDUINO UNO * // * Version : 0.1 * // * Auteur : Yann CLEMENT (yann.clement@online.fr) * // * * // * Description : Test de l'asservissement du robot. * // * Celui-ci parcours 1 mètre 50 en ligne droite en utilisant * // * les capteurs odométrique. * // * Les autres capteurs ne sont pas gérés. * // * * // * Journal : * // * - 02/02/14 : première version * // * * // **************************************************************************** // **************************************************************************** // * CONSTANTES * // **************************************************************************** // ** DIVERS ** // broche D13 : LED de la carte const int led_arduino = 13; // vitesse PWM du robot const byte vitesse_bot = 95; // ** ODOMETRIE ** // broche D2 : capteur odométrique droit (ODOD) const int capteur_odometrie_droit = 2; // broche D3 : capteur odométrique gauche (ODOG) const int capteur_odometrie_gauche = 3; // ** PONT EN H ** // broche D8 : moteur gauche, pont en H IN2 const int in2_moteur = 8; // broche D9 : moteur gauche, pont en H IN1 const int in1_moteur = 9; // broche D4 : moteur droit, pont en H IN4 const int in4_moteur = 4; // broche D7 : moteur droit, pont en H IN3 const int in3_moteur = 7; // broche D5 : PWM moteur gauche, pont en H ENB const int pwm_moteur_gauche = 5; // broche D6 : PWM moteur droit, pont en H ENA const int pwm_moteur_droit = 6; // **************************************************************************** // * VARIABLES * // **************************************************************************** // ** ODOMETRIE ** // compteur odométrique droit // chaque impulsion de l'odométrie droite est comptée dans cette variable int compteur_odometrie_droit = 0; // compteur odométrique gauche // chaque impulsion de l'odométrie gauche est comptée dans cette variable int compteur_odometrie_gauche = 0; // nombre d'impulsions demandées au moteur droit // le moteur droit passe automatiquement à l'arrêt après le nombre indiqué dans cette variable int nombre_impulsions_droit = 0; // nombre d'impulsions demandées au moteur gauche // le moteur gauche passe automatiquement à l'arrêt après le nombre indiqué dans cette variable int nombre_impulsions_gauche = 0; // différence d'impulsions odométrique entre moteur gauche et droite // utilisé pour l'asservissement, ne pas modifier int difference_impulsions_odometrie = 0; // ** PONT EN H ** // PWM demandé au moteur droit // sert au firmware à mémoriser le PWM demandé au moteur droit byte pwm_courant_droit = 0; // PWM demandé au moteur gauche // sert au firmware à mémoriser le PWM demandé au moteur gauche byte pwm_courant_gauche = 0; // equilibrage pont en H // flag servant à indiquer si les deux moteur doivent tourner à la même vitesse boolean synchro_pont_H = false; // flag d'activité sur le moteur droit // sert au firmware pour savoir si le moteur est actif boolean moteur_droit_actif = false; // flag d'activité sur le moteur gauche // sert au firmware pour savoir si le moteur est actif boolean moteur_gauche_actif = false; // **************************************************************************** // * INITIALISATION * // **************************************************************************** // initialisation de la carte void setup() { // ** SORTIES ** // initialisation de la broche 13 (vers LED) comme sortie pinMode(led_arduino, OUTPUT); // initialisation moteur droit (vers IN4) comme sortie pinMode(in4_moteur, OUTPUT); // initialisation moteur droit (vers IN3) comme sortie pinMode(in3_moteur, OUTPUT); // initialisation moteur gauche (vers IN2) comme sortie pinMode(in2_moteur, OUTPUT); // initialisation moteur gauche (vers IN1) comme sortie pinMode(in1_moteur, OUTPUT); // initialisation du PWM droit comme sortie (vers ENA) pinMode(pwm_moteur_droit, OUTPUT); // initialisation du PWM gauche comme sortie (vers ENB) pinMode(pwm_moteur_gauche, OUTPUT); // ** ENTREES ** // initialisation odométrie droit comme entrée (ODOD) pinMode(capteur_odometrie_droit, INPUT); // initialisation odométrie gauche comme entrée (ODOG) pinMode(capteur_odometrie_gauche, INPUT); // ** INTERRUPTIONS ** // on déclenche une interruption (sur front montant) sur le capteur odométrique droit (broche 2) attachInterrupt(0, interruption_odometrie_droit, RISING); // on déclenche une interruption (sur front montant) sur le capteur odométrique gauche (broche 3) attachInterrupt(1, interruption_odometrie_gauche, RISING); // ** LED ** // écriture en sortie (LED broche 13) d'un état HAUT digitalWrite(led_arduino, HIGH); // ** DEMARRAGE ** // toutes les sorties à l'état bas robot_stop(); // pause d'une seconde delay(1000); // NOTES : // 1 impulsion = environ 1 cm // pour 1 mètre 50 = 150 robot_avance(100, 150); } // **************************************************************************** // * BOUCLE * // **************************************************************************** // fonction principale void loop() { // **************************************************************************** // * MOTEURS SYNCHRONISES * // **************************************************************************** // si l'odométrie gauche ou l'odométrie droite sont complétées if ((compteur_odometrie_gauche > nombre_impulsions_gauche) || (compteur_odometrie_droit > nombre_impulsions_droit)) { // on met son PWM droit à 0 analogWrite(pwm_moteur_droit, 0); pwm_courant_droit = 0; // on met son PWM gauche à 0 analogWrite(pwm_moteur_gauche, 0); pwm_courant_gauche = 0; // frein moteurs digitalWrite(in1_moteur, HIGH); digitalWrite(in3_moteur, HIGH); digitalWrite(in2_moteur, HIGH); digitalWrite(in4_moteur, HIGH); // les moteurs ne sont plus actifs moteur_gauche_actif = false; moteur_droit_actif = false; // sinon, le trajet n'est pas encore complet } else { // compare le moteur droit au moteur gauche difference_impulsions_odometrie = compteur_odometrie_droit - compteur_odometrie_gauche; // si les impulsions sont coordonnées if (difference_impulsions_odometrie == 0) { // les moteur sont synchro, on modifie ou confirme leurs PWM analogWrite(pwm_moteur_gauche, pwm_courant_gauche); analogWrite(pwm_moteur_droit, pwm_courant_droit); // sinon } else { // si la différence est positive, le moteur droit va trop vite if (difference_impulsions_odometrie > 0) { // on met son PWM à 0 analogWrite(pwm_moteur_droit, 0); // on augmente légèrement le PWM de l'autre moteur si il n'est pas déjà trop haut if (pwm_moteur_gauche < 248) analogWrite(pwm_moteur_gauche, (pwm_courant_gauche + 7)); } // si la différence est négative, le moteur gauche va trop vite if (difference_impulsions_odometrie < 0) { // on met son PWM à 0 analogWrite(pwm_moteur_gauche, 0); // on augmente légèrement le PWM de l'autre moteur si il n'est pas déjà trop haut if (pwm_moteur_droit < 248) analogWrite(pwm_moteur_droit, (pwm_courant_droit + 7)); } } } } // **************************************************************************** // * FONCTIONS SECONDAIRES * // **************************************************************************** // INTERRUPTION ODOMETRIE DROIT // fonction appelée par l'interuption sur le capteur odométrique droit void interruption_odometrie_droit() { // on incrémente le compteur odométrique droit compteur_odometrie_droit = compteur_odometrie_droit + 1; } // --- // INTERRUPTION ODOMETRIE GAUCHE // fonction appelée par l'interuption sur le capteur odométrique gauche void interruption_odometrie_gauche() { // on incrémente le compteur odométrique gauche compteur_odometrie_gauche = compteur_odometrie_gauche + 1; } // --- // ROBOT AVANCE // fait avancer le robot void robot_avance(int pwm, int limite) { // compteurs odométriques à 0 compteur_odometrie_droit = 0; compteur_odometrie_gauche = 0; // nombre d'impulsions limite pour les deux moteurs nombre_impulsions_droit = limite; nombre_impulsions_gauche = limite; // les PWM sont identique pwm_courant_gauche = pwm_courant_droit = pwm; // écriture sur la broche 2 du pont en H d'un état BAS digitalWrite(in2_moteur, LOW); // écriture sur la broche 4 du pont en H d'un état HAUT digitalWrite(in4_moteur, HIGH); // écriture sur la broche 1 du pont en H d'un état HAUT digitalWrite(in1_moteur, HIGH); // écriture sur la broche 3 du pont en H d'un état BAS digitalWrite(in3_moteur, LOW); // écrit la valeur du PWM analogWrite(pwm_moteur_gauche, pwm); // écrit la valeur du PWM analogWrite(pwm_moteur_droit, pwm); // passe les moteurs en actif moteur_droit_actif = true; moteur_gauche_actif = true; // met à jour le pwm courant pwm_courant_droit = pwm; pwm_courant_gauche = pwm; } // --- // ROBOT STOP // stop le robot, passe les PWM automatiquement à 0 void robot_stop() { // initialise les variables pwm_courant_droit = 0; pwm_courant_gauche = 0; // valeur du PWM à 0 analogWrite(pwm_moteur_droit, 0); // valeur du PWM à 0 analogWrite(pwm_moteur_gauche, 0); // écriture sur la broche 4 du pont en H d'un état BAS digitalWrite(in4_moteur, LOW); // écriture sur la broche 2 du pont en H d'un état BAS digitalWrite(in2_moteur, LOW); // écriture sur la broche 3 du pont en H d'un état BAS digitalWrite(in3_moteur, LOW); // écriture sur la broche 1 du pont en H d'un état BAS digitalWrite(in1_moteur, LOW); // désactive les moteurs moteur_droit_actif = false; moteur_gauche_actif = false; }