Étape 3: Organigramme et CODE
Tout d’abord que nous commençons avec le losange, le losange représente un chèque. Pour cette vérification, nous voyons s’il y a un objet moins de 30 cm pour notre robot. La vérification a deux issues possibles, oui ou non. Oui, ce qui signifie qu’il n’y a en effet certains objets moins 30 cm. Non, ce qui signifie qu’il n’y a aucun objet détecté moins de 30 cm.
S’il n’y a rien de moins de 30 cm, que le robot peut simplement se déplacer vers l’avant que la voie est libre. S’il y a quelque chose de moins de 30 cm le robot doit effectuer des manœuvres d’évitement obstacle.
La première étape de l’évitement d’obstacles est d’arrêter le robot ! Si vous n’arrêtez pas immédiatement le robot il va se planter !
Ouf ! C’est dramatique.
Après que le robot a arrêté il faut voir de quelle manière il faut aller. Il fait ceci en regardant les deux directions, beaucoup comme vous devriez lorsque vous traversez la route. Tout d’abord le robot tourne à gauche, prend une lecture, tourne à droit, prend une lecture.
Une autre vérification se produit pour voir quelle direction est la meilleure façon d’aller. Si à gauche est la voie à suivre il doit rebrousser chemin vers la gauche et puis aller de l’avant. Si à droite est le chemin à parcourir le robot se déplace simplement vers l’avant car il doit déjà faire face dans la bonne direction. Donc c’était assez facile n’était pas elle ? Tout ce que nous devons faire maintenant est de convertir ce diagramme dans le code de l’Arduino et c’est ça, le code d’évitement d’obstacle effectué !
#include
#include
////////////////////////////////////////////////servo motor and sonar sensor ////////////////////////////////////////////
const int dangerdistance = 30 ; distance de sécurité pour les obstacles (en cm) Servo panMotor ; Créez un objet servo #define TRIGGER_PIN 3 / / goupille de Arduino lié à déclencher la broche sur le capteur à ultrasons. #define ECHO_PIN 2 / / goupille de Arduino liée à la broche sur le capteur à ultrasons d’écho. #define MAX_distance 200 / / distance maximale que nous voulons faire un ping pour (en centimètres). Distance maximum sensor est évalué à 400-500 cm. NewPing sonar (TRIGGER_PIN, ECHO_PIN, MAX_distance) ; NewPing configuration des broches et distance maximale. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////// MOTOR //////////////////////////////////////////////////////////////////
*
Ce code effectue quelques manoeuvres simples pour illustrer les fonctions: - motorDrive (motorNumber, motorDirection, motorSpeed) - motorBrake(motorNumber) - motorStop(motorNumber) - motorsStandby
Connexions: - broche 5---> PW101 - broche 7---> AIN2 - Broche 8---> AIN1 - broche 11---> STBY - Pin 12---> BIN1 - Pin 13---> BIN2 - broche 6---> PWMB
-Moteur 1: A01 et A02 - moteur 2: B01 et B02
*
Et définir les broches
Moteur 1 int pinAIN1 = 8 ; Direction int pinAIN2 = 7 ; Direction int pinPWMA = 5 ; Vitesse
Moteur 2 int pinBIN1 = 12 ; Direction int pinBIN2 = 13 ; Direction int pinPWMB = 6 ; Vitesse
Veille int pinSTBY = 11 ;
Constantes pour aider à mémoriser la turnCW booléenne statique de paramètres = 0 ; motorDrive fonction public static boolean turnCCW = 1 ; motorDrive fonction public static boolean motor1 = 0 ; pour motorDrive, motorStop, frein moteur fonctionne public static boolean motor2 = 1 ; pour motorDrive, motorStop, fonctions de frein moteur
////////////////////////////////////////////////////////////////////////////////////////////////////////////////// unsigned long time; minuterie pour boucle do... tout en unsigned long timeplus; / / minuteur pour la boucle do... tandis que
distance de l’int ; int nous ; void setup() {//Set le pinMode moteur broche Modes (pinPWMA, sortie); pinMode (pinAIN1, sortie); pinMode (pinAIN2, sortie) ;
pinMode (pinPWMB, sortie) ; pinMode (pinBIN1, sortie) ; pinMode (pinBIN2, sortie) ; pinMode (pinSTBY, sortie) ; Serial.Begin(9600) ; panMotor.write(90) ; Delay(15000); / / attendre que je reçois panMotor.attach(4) prêt ; s’adapte sur le servo sur la broche 9 l’objet servo}
//////////////////////////////////////////////////////////LOOP//////////////////////////////////////////////////
void loop() {panMotor.write(90); / / mettez un capteur sonar à 90 degrés
Delay(500) ; capteur sonar ping de nous amener à distance int = sonar.ping() ; développantes int = uS / US_ROUNDTRIP_CM ; if(centerdistance==0) {développantes = 200;} Difficulté newping bibliothèque trompé
Si (développantes > dangerdistance) résulte //if chemin {/ / *** alors qu’aucun OBSTACLE à votre FRONT ne KEEP MOVING AND CHECK FOR OBSTACLE chaque MS 100 ***
/ / (capteur donnent moins de robot Gardez 30cm avancer) {int nous = sonar.ping() ; int développantes = uS / US_ROUNDTRIP_CM ; / / vérifier les capteurs delay(100); / / attente pour capteurs stabiliser le motorDrive (motor1, turnCW, 150); motorDrive (motor2, turnCW, 150);} tandis que (développantes < dangerdistance) ;
} / /---Pathé est bloqué---autre //if chemin est bloqué {motorBrake(motor1) ; motorBrake(motor2) ; si (développantes
panMotor.write(175); / / tourner à gauche delay(900) ; int leftdistance = sonar.ping () / US_ROUNDTRIP_CM ; numérisation vers la gauche delay(500) ; if(leftdistance==0) {leftdistance = 200;} Difficulté newping bibliothèque confondu avec / / / comparer DISTANCE droite et gauche / / / si (leftdistance > rightdistance & & leftdistance > dangerdistance) //if gauche est moins encombré et super que dangerdistance {//Stop Motor1, Motor2 motorStop(motor1) ; motorStop(motor2) ; time=millis() ; panMotor.write (175); //turn sonar à gauche « Préparez-vous vérifier le côté gauche du robot quand il tourne » delay(400) / / (tourner un robot vers la gauche tandis que le capteur de donner moins de 30cm et pas plus de temps alors 800 ms) {int nous = sonar.ping() ; distance = uS / US_ ROUNDTRIP_CM ; vérifier les capteurs delay(100) ; Attendez de capteurs stabiliser //Turn vers motor1 : arrêter Motor1, lent motorDrive de Motor2 (motor1, turnCCW, 180) ; motorDrive (motor2, turnCW, 180) ; timeplus=Millis() ; } tandis que (distance > dangerdistance & timeplus-temps < 800) ;
motorBrake(motor1) ; motorBrake(motor2) ; panMotor.write(90); / / tour sonar d’origine position delay(2000) ;
} ElseIf (rightdistance > leftdistance & & rightdistance > dangerdistance) //if droit est moins encombré et super que dangerdistance {//Stop Motor1, Motor2 motorStop(motor1) ; motorStop(motor2) ; panMotor.write(5) ; delay(400) ; time=millis() ; faire //(do turn robot to right while sensor give less than 30cm and time not more then 800 ms) {int nous = sonar.ping() ; distance = uS / US_ROUNDTRIP_CM; / / vérifier les capteurs delay(100); / / attente pour capteurs à stabiliser motorDrive (motor2 turnCCW, 180) ; motorDrive (motor1, turnCW, 180) ; timeplus=Millis() ; } tandis que (distance > dangerdistance & timeplus-temps < 800) ; motorBrake(motor1) ; motorBrake(motor2) ;
panMotor.write(90) ; Delay(2000) ; } else / / si aucune solution {/ / tout arrêter motorBrake(motor1) ; motorBrake(motor2) ; motorsStandby() ; panMotor.write(90) ; delay(9000);}} }
///////////////////////////// FUNCTIONS /////////////////////////////////////////
void motorDrive (boolean motorNumber, boolean motorDirection, int motorSpeed) {/ * cela actionne un moteur spécifié, dans une direction précise, à une vitesse spécifiée:-motorNumber : motor1 ou motor2---> 1 moteur ou moteur 2 - motorDirection : turnCW ou turnCCW---> dans le sens horaire ou antihoraire - motorSpeed : de 0 à 255---> 0 = arrêt / 255 = rapide * /
Boolean pinIn1 ; Se rapporte à AIN1 ou BIN1 (selon le numéro du moteur indiqué)
Spécifier le sens pour activer le moteur / / horaire : AIN1/BIN1 = haute et AIN2/BIN2 = faible / / Counter-horaire : AIN1/BIN1 = LOW et AIN2/BIN2 = haute si (motorDirection == turnCW) pinIn1 = élevé ; else pinIn1 = faible ;
Sélectionnez le moteur tourne et régler la direction et la vitesse if(motorNumber == motor1) {digitalWrite (pinAIN1, pinIn1); digitalWrite (pinAIN2,! pinIn1); //This est à l’opposé de la AIN1 analogWrite (pinPWMA, motorSpeed);} else {digitalWrite (pinBIN1, pinIn1); digitalWrite (pinBIN2,! pinIn1); //This est à l’opposé de la BIN1 analogWrite (pinPWMB, motorSpeed);}
Enfin, assurez-vous que STBY est désactivé - tirez-le haute digitalWrite (pinSTBY, HIGH) ;
}
frein moteur Sub (boolean motorNumber) {/ * ce « Frein » s le moteur spécifié, en définissant la vitesse à zéro * /
Si (motorNumber == motor1) analogWrite (pinPWMA, 0) ; d’autre analogWrite (pinPWMB, 0) ; }
void motorStop (boolean motorNumber) {/ * cela arrête le moteur spécifié en affectant les deux broches IN LOW * / if (motorNumber == motor1) {digitalWrite (pinAIN1, LOW); digitalWrite (pinAIN2, basse);} else {digitalWrite (pinBIN1, LOW); digitalWrite (pinBIN2, basse);}}
void motorsStandby() {/ * cela met les moteurs en Mode veille * / digitalWrite (pinSTBY, LOW);}
