// **************************************************************************** // * FIRMWARE VIDIROB ARDUINO * // * * // * Plate forme : ARDUINO UNO * // * Version : 2.1 * // * Auteur : Yann CLEMENT (yann.clement@online.fr) * // * * // * Description : Firmware du robot. * // * Celui-ci est attiré par la lumière et évite les obstacles. * // * Cette version permet de déclencher plusieurs actions à la * // * suite pour chaque capteur. * // * * // * Journal : * // * - 04/02/14 : première version * // * - 19/02/14 : correction inversion G2/D2 * // * ajustement manoeuvre dégagement * // * * // **************************************************************************** // **************************************************************************** // * LISTE DES CODES ACTION : * // * - 0 : n'existe pas * // * - 1 à 9 : initialisation * // * - 10 à 19 : D1 : dégagement par la gauche * // * - 20 à 29 : G1 : dégagement par la droite * // * - 30 à 39 : D2 : évitement par la gauche * // * - 40 à 49 : G2 : évitement par la droite * // * - 50 à 54 : LUMID : lumière à droite * // * - 55 à 59 : LUMIG : lumière à gauche * // * - 60 à 69 : n'est pas utilisée * // * - 70 à 79 : procédure d'arrêt * // * - 80 à 89 : procédure normale * // * - 90 à -> : n'est pas utilisé * // **************************************************************************** // **************************************************************************** // * CONSTANTES * // **************************************************************************** // ** DIVERS ** // broche D13 : LED de la carte const byte led_arduino = 13; // broche D10 : buzzer (à câbler) const byte buzzer = 10; // vitesse PWM du robot const byte vitesse_bot = 110; // ** ODOMETRIE ** // broche D2 : capteur odométrique droit (ODOD) const byte capteur_odometrie_droit = 2; // broche D3 : capteur odométrique gauche (ODOG) const byte capteur_odometrie_gauche = 3; // ** CAPTEURS LUMIERE ** // broche D12 : capteur lumière droit (LUMID) const byte capteur_lumiere_droit = 12; // broche D11 : capteur lumière gauche (LUMIG) const byte capteur_lumiere_gauche = 11; // ** PONT EN H ** // broche D8 : moteur gauche, pont en H IN2 const byte in2_moteur = 8; // broche D9 : moteur gauche, pont en H IN1 const byte in1_moteur = 9; // broche D4 : moteur droit, pont en H IN4 const byte in4_moteur = 4; // broche D7 : moteur droit, pont en H IN3 const byte in3_moteur = 7; // broche D5 : PWM moteur gauche, pont en H ENB const byte pwm_moteur_gauche = 5; // broche D6 : PWM moteur droit, pont en H ENA const byte pwm_moteur_droit = 6; // valeur d'ajustement du PWM pour la resynchro des moteurs const byte ajustement_pwm = 7; // ** CAPTEURS OBSTACLES ** // broche A5 : capteur obstacle droit 1 (D1) const byte capteur_obstacle_droit_1 = 18; // broche A4 : capteur obstacle droit 2 (D2) const byte capteur_obstacle_droit_2 = 19; // broche A2 : capteur obstacle gauche 1 (G1) const byte capteur_obstacle_gauche_1 = 17; // broche A3 : capteur obstacle gauche 2 (G2) const byte capteur_obstacle_gauche_2 = 16; // **************************************************************************** // * 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 // mémorise le PWM demandé au moteur droit byte pwm_courant_droit = 0; // PWM demandé au moteur gauche // mémorise le PWM demandé au moteur gauche byte pwm_courant_gauche = 0; // équilibrage pont en H // flag servant à indiquer si les deux moteurs 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; // flag pour demander une pause moteur entre deux action // une pause est parfois nécessaire à cause de l'inertie du robot boolean pause_moteur = false; // ** CAPTEURS LUMIERE ** // état du capteur de lumière droit boolean etat_lumiere_droit = true; // état du capteur de lumière gauche boolean etat_lumiere_gauche = true; // ** CAPTEURS OBSTACLES ** // état du capteur d'obstacle droit 1 boolean etat_obstacle_droit_1 = true; // état du capteur d'obstacle droit 2 boolean etat_obstacle_droit_2 = true; // état du capteur d'obstacle gauche 1 boolean etat_obstacle_gauche_1 = true; // état du capteur d'obstacle gauche 2 boolean etat_obstacle_gauche_2 = true; // indique un obstacle de niveau 2 boolean obstacle_niveau_2 = false; // indique un obstacle de niveau 1 boolean obstacle_niveau_1 = false; // ** DIVERS ** // permet de savoir quelle est l'action en cours byte action_courante = 0; // code d'action courante // numéro de l'action du switch action qui sera ou a été exécutée int code_action = 0; // code d'action précédent // numéro de l'action précédemment exécutée int code_action_precedent = 0; // **************************************************************************** // * 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); // initialisation du PWM pour le buzzer pinMode(buzzer, 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); // initialisation obstacle droit 1 comme entrée (D1) pinMode(capteur_obstacle_droit_1, INPUT); // initialisation obstacle droit 2 comme entrée (D2) pinMode(capteur_obstacle_droit_2, INPUT); // initialisation obstacle gauche 1 comme entrée (G1) pinMode(capteur_obstacle_gauche_1, INPUT); // initialisation obstacle gauche 2 comme entrée (G2) pinMode(capteur_obstacle_gauche_2, INPUT); // initialisation lumière droit comme entrée (LUMID) pinMode(capteur_lumiere_droit, INPUT); // initialisation lumière gauche comme entrée (LUMIG) pinMode(capteur_lumiere_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); // ** LIAISON SERIE ** // initialise la liaison série pour le debuggage Serial.begin(9600); // ** DEMARRAGE ** // toutes les sorties à l'état bas // code action passe à 1 robot_stop(); // on émet un bip analogWrite(buzzer, 128); // pause de 500 ms delay(500); // on stop le bip analogWrite(buzzer, 0); // pause de 500 ms delay(500); // DEBUG Serial.print(code_action); Serial.println(":DEBUT"); } // **************************************************************************** // * BOUCLE * // **************************************************************************** // fonction principale void loop() { // **************************************************************************** // * CAPTEURS * // **************************************************************************** // les actions en dessous de 30 ne peuvent être interrompues if (code_action >= 30) { // ** LECTURE DES CAPTEURS ** // OBSTACLES : // niveau 1, les deux capteurs le plus proche du centre // capteurs actifs à l'état bas etat_obstacle_droit_1 = digitalRead(capteur_obstacle_droit_1); etat_obstacle_gauche_1 = digitalRead(capteur_obstacle_gauche_1); if (!etat_obstacle_droit_1 || !etat_obstacle_gauche_1) obstacle_niveau_1 = true; else obstacle_niveau_1 = false; // niveau 2, les deux capteurs les plus proche des cotés // capteurs actifs à l'état bas etat_obstacle_droit_2 = digitalRead(capteur_obstacle_droit_2); etat_obstacle_gauche_2 = digitalRead(capteur_obstacle_gauche_2); if (!etat_obstacle_droit_2 || !etat_obstacle_gauche_2) obstacle_niveau_2 = true; else obstacle_niveau_2 = false; // LUMIERE : // capteurs actifs à l'état bas etat_lumiere_droit = digitalRead(capteur_lumiere_droit); etat_lumiere_gauche = digitalRead(capteur_lumiere_gauche); // si il y a détection de lumière, on émet un bip if (!etat_lumiere_gauche || !etat_lumiere_droit) analogWrite(buzzer, 128); else analogWrite(buzzer, 0); // ** ALGORITHME DE DEDUCTION D'ACTION ** // si l'un des capteurs de niveau 1 est actif if (obstacle_niveau_1) { // si obstacle uniquement sur le capteur 1 droit if (!etat_obstacle_droit_1 && etat_obstacle_gauche_1) { // DEBUG Serial.print(code_action); Serial.println(":D1->10"); // on se branche sur l'action obstacle code_action = 10; // si obstacle uniquement sur le capteur 1 gauche } else if (etat_obstacle_droit_1 && !etat_obstacle_gauche_1) { // DEBUG Serial.print(code_action); Serial.println(":G1->20"); // on se branche sur l'action obstacle code_action = 20; // sinon il y a obstacle sur les capteurs 1 gauche et 1 droit } else { // si le côté gauche est libre if (!etat_obstacle_droit_2 && etat_obstacle_gauche_2) { // DEBUG Serial.print(code_action); Serial.println(":G1&D1->20"); // on se branche sur l'action obstacle code_action = 20; // sinon, quoi qu'il arrive, on fait l'inverse } else { // DEBUG Serial.print(code_action); Serial.println(":G1&D1->10"); // on se branche sur l'action obstacle code_action = 10; } } // les obstacles de niveau 2 ne sont traités que si il n'y a pas d'obstacle de niveau 1 } else if (obstacle_niveau_2) { // si obstacle uniquement sur le capteur 2 droit (D2) if (!etat_obstacle_droit_2 && etat_obstacle_gauche_2) { // le coté gauche est donc dégagé // si lumière seulement à gauche if (!etat_lumiere_gauche && etat_lumiere_droit) { // rejoindre lumière vers la gauche // DEBUG Serial.print(code_action); Serial.println(":LUMIG->55"); // on se branche sur l'action lumière code_action = 55; // sinon, on évite simplement l'obstacle } else { // éviter l'obstacle à droite en virant à gauche // DEBUG Serial.print(code_action); Serial.println(":D2->30"); // on se branche sur l'action évitement code_action = 30; } // si obstacle uniquement sur le capteur 2 gauche (G2) } else if (etat_obstacle_droit_2 && !etat_obstacle_gauche_2) { // le côté droit est donc dégagé // si lumière seulement à droite if (etat_lumiere_gauche && !etat_lumiere_droit) { // rejoindre lumière vers la droite // DEBUG Serial.print(code_action); Serial.println(":LUMID->50"); // on se branche sur l'action lumière code_action = 50; // sinon, on éviter simplement l'obstacle } else { // éviter l'obstacle à gauche en virant à droite // DEBUG Serial.print(code_action); Serial.println(":G2->40"); // on se branche sur l'action évitement code_action = 40; } // si obstacle sur G2 et D2 } else if (!etat_obstacle_droit_2 && !etat_obstacle_gauche_2) { // on passe à une action équivalente à G1&D1 // DEBUG Serial.print(code_action); Serial.println(":G2D2->10"); // on se branche sur l'action obstacle code_action = 10; } // si il n'y a pas d'obstacle, le robot cherche la lumière } else { // si détection de lumière sur le capteur droit if (!etat_lumiere_droit && etat_lumiere_gauche) { // DEBUG Serial.print(code_action); Serial.println(":LUMID->50"); // on se branche sur l'action lumière code_action = 50; // si détection de lumière sur le capteur gauche } else if (!etat_lumiere_gauche && etat_lumiere_droit) { // DEBUG Serial.print(code_action); Serial.println("LUMIG->55"); // on se branche sur l'action lumière code_action = 55; // sinon, si les deux capteurs de lumière ou aucun n'est // actif, le résultat est le même, le robot va tout droit } else { // DEBUG Serial.print(code_action); Serial.println(":AUCUN->80"); // on passe en mode normal code_action = 80; } } } // **************************************************************************** // * MOTEURS SYNCHRONISES * // **************************************************************************** // si les moteurs sont syncho et donc actifs tous les deux if (moteur_gauche_actif && moteur_droit_actif && synchro_pont_H) { // si on a le nombre d'impulsions demandées à gauche ou à droite if ((compteur_odometrie_gauche > nombre_impulsions_gauche) || (compteur_odometrie_droit > nombre_impulsions_droit)) { // ** ARRET DES MOTEURS ** // 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; // baisse le flag de synchro synchro_pont_H = false; // l'action est terminée action_courante = 0; // DEBUG Serial.print(code_action); Serial.println(":SYNC_MOT_STOP"); // on passe au code action suivant code_action = code_action + 1; // sinon, le trajet n'est pas encore complet } else { // ** TEST SYNCHRO MOTEURS ** // 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 { // ** RESYNCHRO MOTEURS ** // 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 < (254 - ajustement_pwm)) analogWrite(pwm_moteur_gauche, (pwm_courant_gauche + ajustement_pwm)); } // 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 < (254 - ajustement_pwm)) analogWrite(pwm_moteur_droit, (pwm_courant_droit + ajustement_pwm)); } } } // sinon, pas de synchro moteurs } else { // **************************************************************************** // * MOTEURS NON SYNCHRONISES * // **************************************************************************** // si on a le nombre d'impulsions demandées à gauche et que le moteur est actif if ((compteur_odometrie_gauche > nombre_impulsions_gauche) && moteur_gauche_actif) { // ** ARRET MOTEUR GAUCHE ** // PWM à 0 analogWrite(pwm_moteur_gauche, 0); // frein moteur digitalWrite(in3_moteur, HIGH); digitalWrite(in4_moteur, HIGH); // moteur n'est plus actif moteur_gauche_actif = false; // l'action est terminée action_courante = 0; // DEBUG Serial.print(code_action); Serial.println(":MOT_G_STOP"); // on passe au code action suivant code_action = code_action + 1; } // si on a le nombre d'impulsions demandées à droite et que le moteur est actif if ((compteur_odometrie_droit > nombre_impulsions_droit) && moteur_droit_actif) { // ** ARRET MOTEUR DROIT ** // PWM à 0 analogWrite(pwm_moteur_droit, 0); // frein moteur digitalWrite(in1_moteur, HIGH); digitalWrite(in2_moteur, HIGH); // moteur n'est plus actif moteur_droit_actif = false; // DEBUG Serial.print(code_action); Serial.println(":MOT_D_STOP"); // on passe au code action suivant code_action = code_action + 1; // l'action est terminée action_courante = 0; } } // ** PAUSE MOTEUR ** // si une pause moteur est demandée et que l'action courante est terminée if (pause_moteur && action_courante == 0) { delay(350); pause_moteur = false; } // **************************************************************************** // * SWITCH ACTION * // **************************************************************************** // si le code action est différent du précédent if (code_action != code_action_precedent) { // code action précédent mis à jour code_action_precedent = code_action; //Serial.print(code_action); //Serial.println(":SWITCH ACTION"); switch (code_action) { // NOTES : // - 1 impulsion = environ 1 cm // utiliser pour cela robot_avance(PWM, LIMITE) et robot_recule(PWM, LIMITE) // - environ 34 impulsions pour un demi-tour en utilisant un seul moteur // - environ 15 impulsions pour un demi-tour en utilisant deux moteurs // utiliser pour cela robot_pivote_droite(PWM, LIMITE) et robot_pivote_gauche(PWM, LIMITE) // ** INIT ** case 1: // 1 impulsions égale environ 0,85 cm // donc pour 25 cm = 25 impulsions robot_avance(120, 25); pause_moteur = true; // DEBUG Serial.println("1:(INIT)AVANCE"); break; case 2: // il faut environ 15 impulsions pour un // demi-tour en utilisant deux moteurs robot_pivote_gauche(120, 15); pause_moteur = true; // DEBUG Serial.println("2:(INIT)PIV_G"); break; case 3: robot_pivote_droite(120, 15); pause_moteur = true; // DEBUG Serial.println("3:(INIT)PIV_D"); break; case 4: robot_recule(120, 25); // DEBUG Serial.println("4:(INIT)RECULE"); break; case 5: // on passe en mode normal code_action = 80; // DEBUG Serial.println("5:(INIT)->80"); break; // ** D1 ** case 10: robot_recule(vitesse_bot, 5); // DEBUG Serial.println("10:RECULE"); break; case 11: robot_pivote_gauche(vitesse_bot, 7); // DEBUG Serial.println("10:PIV_G"); break; case 12: code_action = 80; // DEBUG Serial.println("12:->80"); break; // ** G1 ** case 20: robot_recule(vitesse_bot, 5); // DEBUG Serial.println("20:RECULE"); break; case 21: robot_pivote_droite(vitesse_bot, 7); // DEBUG Serial.println("21:PIV_D"); break; case 22: code_action = 80; // DEBUG Serial.println("22:->80"); break; // ** D2 ** case 30: // il faut environ 34 impulsions pour un // demi-tour en utilisant un seul moteur robot_vire_gauche(vitesse_bot, 34); // DEBUG Serial.println("30:VIR_G"); break; case 31: code_action = 80; // DEBUG Serial.println("31:->80"); break; // ** G2 ** case 40: // il faut environ 34 impulsions pour un // demi-tour en utilisant un seul moteur robot_vire_droite(vitesse_bot, 34); // DEBUG Serial.println("40:VIR_D"); break; case 41: code_action = 80; // DEBUG Serial.println("41:->80"); break; // ** LUMID ** case 50: // il faut environ 34 impulsions pour un // demi-tour en utilisant un seul moteur robot_vire_droite(vitesse_bot, 34); // DEBUG Serial.println("50:VIR_D"); break; case 51: code_action = 80; // DEBUG Serial.println("51:->80"); break; // ** LUMIG ** case 55: // il faut environ 34 impulsions pour un // demi-tour en utilisant un seul moteur robot_vire_gauche(vitesse_bot, 34); // DEBUG Serial.println("55:VIR_G"); break; case 56: code_action = 80; // DEBUG Serial.println("56:->80"); break; // ** STOP PROGRAMME ** case 70: // on stoppe le robot robot_stop(); // on éteind la LED digitalWrite(led_arduino, LOW); // DEBUG Serial.println("70:FIN"); // boucle infinie while (true) { // PROGRAMME STOPPE } // ** NORMAL ** case 80: robot_avance(vitesse_bot, 100); // DEBUG Serial.println("80:AVANCE"); break; case 81: // on boucle code_action = 80; // DEBUG Serial.println("81:->80"); break; // ** INCONNU ** default: // DEBUG Serial.print(code_action); Serial.println(":INCONNU->80"); // on repart sur un code normal code_action = 80; break; } } } // **************************************************************************** // * FONCTIONS * // **************************************************************************** // INTERRUPTION ODOMETRIE DROIT // fonction appelée par l'interruption 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'interruption 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; } // --- // PAUSE // temporisation en millisecondes void pause(int temps) { // tempo delay(temps); // code d'action suivant code_action = code_action + 1; } // --- // ROBOT AVANCE // fait avancer le robot void robot_avance(int pwm, int limite) { // si cette action est déjà en cours, on sort directement if (action_courante == 1) return; // 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 et doivent être synchronisés pwm_courant_gauche = pwm_courant_droit = pwm; synchro_pont_H = true; // é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; // AVANCE = 1 action_courante = 1; } // --- // ROBOT RECULE // fait reculer le robot void robot_recule(int pwm, int limite) { // si cette action est déjà en cours, on sort directement if (action_courante == 2) return; // 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 et doivent être synchronisés pwm_courant_gauche = pwm_courant_droit = pwm; synchro_pont_H = true; // écriture sur la broche 2 du pont en H d'un état HAUT digitalWrite(in2_moteur, HIGH); // écriture sur la broche 4 du pont en H d'un état BAS digitalWrite(in4_moteur, LOW); // écriture sur la broche 1 du pont en H d'un état BAS digitalWrite(in1_moteur, LOW); // écriture sur la broche 3 du pont en H d'un état HAUT digitalWrite(in3_moteur, HIGH); // é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; // RECULE = 2 action_courante = 2; } // --- // 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; // STOP = fin d'action = 0 action_courante = 0; // code d'action suivant code_action = code_action + 1; } // --- // ROBOT PIVOTE GAUCHE // fait pivoter le robot sur lui-même vers la gauche void robot_pivote_gauche(int pwm, int limite) { // si cette action est déjà en cours, on sort directement if (action_courante == 3) return; // 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 et doivent être synchronisés pwm_courant_gauche = pwm_courant_droit = pwm; synchro_pont_H = true; // moteur droit (in1 et in2) avance et moteur gauche (in3 et in 4) recule // é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 BAS digitalWrite(in4_moteur, LOW); // é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 HAUT digitalWrite(in3_moteur, HIGH); // écrit la valeur du PWM analogWrite(pwm_moteur_droit, pwm); // écrit la valeur du PWM analogWrite(pwm_moteur_gauche, 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; // PIV_G = 3 action_courante = 3; } // --- // ROBOT PIVOTE DROITE // fait pivoter le robot sur lui-même vers la droite void robot_pivote_droite(int pwm, int limite) { // si cette action est déjà en cours, on sort directement if (action_courante == 4) return; // 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 et doivent être synchronisés pwm_courant_gauche = pwm_courant_droit = pwm; synchro_pont_H = true; // moteur droit (in1 et in2) recule et moteur gauche (in3 et in 4) avance // écriture sur la broche 2 du pont en H d'un état HAUT digitalWrite(in2_moteur, HIGH); // é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 BAS digitalWrite(in1_moteur, LOW); // é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_droit, pwm); // écrit la valeur du PWM analogWrite(pwm_moteur_gauche, 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; // PIV_D = 4 action_courante = 4; } // --- // ROBOT VIRE GAUCHE // fait virer le robot vers la gauche void robot_vire_gauche(int pwm, int limite) { // si cette action est déjà en cours, on sort directement if (action_courante == 5) return; // compteurs odométriques à 0 compteur_odometrie_droit = 0; // nombre d'impulsions limite pour les deux moteurs nombre_impulsions_droit = limite; // les PWM sont identique et doivent être synchronisés pwm_courant_droit = pwm; // ** STOP MOTEUR GAUCHE ** // valeur du PWM à 0 analogWrite(pwm_moteur_gauche, 0); // écriture sur la broche A du pont en H d'un état BAS digitalWrite(in4_moteur, LOW); // écriture sur la broche B du pont en H d'un état BAS digitalWrite(in3_moteur, LOW); // ** MOTEUR DROIT AVANCE ** // écriture sur la broche 2 du pont en H d'un état BAS digitalWrite(in2_moteur, LOW); // écriture sur la broche 1 du pont en H d'un état HAUT digitalWrite(in1_moteur, HIGH); // écrit valeur du PWM analogWrite(pwm_moteur_droit, pwm); // passe les moteurs en actif moteur_droit_actif = true; moteur_gauche_actif = false; // met à jour le PWM courant pwm_courant_droit = pwm; // VIR_G = 5 action_courante = 5; } // --- // ROBOT VIRE DROITE // fait virer le robot vers la droite void robot_vire_droite(int pwm, int limite) { // si cette action est déjà en cours, on sort directement if (action_courante == 6) return; // compteurs odométriques à 0 compteur_odometrie_gauche = 0; // nombre d'impulsions limite pour les deux moteurs nombre_impulsions_gauche = limite; // les PWM sont identique et doivent être synchronisés pwm_courant_gauche = pwm; // ** STOP MOTEUR DROIT ** // valeur du PWM à 0 analogWrite(pwm_moteur_droit, 0); // écriture sur la broche A du pont en H d'un état BAS digitalWrite(in2_moteur, LOW); // écriture sur la broche B du pont en H d'un état BAS digitalWrite(in1_moteur, LOW); // ** MOTEUR GAUCHE AVANCE ** // écriture sur la broche 4 du pont en H d'un état HAUT digitalWrite(in4_moteur, HIGH); // écriture sur la broche 3 du pont en H d'un état BAS digitalWrite(in3_moteur, LOW); // écrit valeur du PWM analogWrite(pwm_moteur_gauche, pwm); // active le moteur gauche moteur_gauche_actif = true; moteur_droit_actif = false; // met à jour le PWM courant pwm_courant_gauche = pwm; // VIR_D = 6 action_courante = 6; }