Étape 4: programmation
Voici notre exemple de code de navigation à l’aide de tableaux 2D. À l’intérieur de la grille de tableau 2D, nous avons d’entrée un 0 ou un 1. Un 1 représente un objet ou un obstacle. Un espace libre de 0 représente. Le robot se déplace à travers les 0.
Navigation de la grille :
IL S’AGIT D’UN PROGRAMME POUR TRACER UNE ZONE AVEC DES TABLEAUX 2D ET ENSUITE NAVIGUER UNIQUEMENT À TRAVERS LA
ZÉROS PRÉSENTS. CAPTEURS À ULTRASONS NE SERVENT PAS BIEN QU’ILS SONT INITIALISÉS.
LES CODEURS NE SONT ÉGALEMENT PAS UTILISÉS MAIS SONT INITIALISÉS
Pranav
#include « Servo.h »
Moteurs
Servo motor1 ;
Servo motor2 ;
Encodeurs rotatifs
int encoderAPin = 4 ; Gauche / / épingle pour l’encoderA rotatif
int encoderBPin = 2 ; Droit / / épingle pour l’encoderB rotatif
int encoderAPosition = 0 ; compteur de position pour encoderA
int encoderBPosition = 0 ; compteur de position pour encoderB
int signalAInput1 ; signal d’entrée 1 pour encoderA
int signalAInput2 ; entrée de signal 2 pour encoderA
int signalBInput1 ; signal d’entrée 1 pour encoderB
int signalBInput2 ; entrée de signal 2 pour encoderB
service de localisation de robot
0 est en place
1 a raison
2 est en panne
3 est laissé
int robotDirection = 2 ;
Il s’agit des coordonnées dans la grille du où le robot est
C’est aussi le x et y des index dans le tableau.
n’oubliez pas que le tableau commence à l’index 0.
int xcoordinate = 2 ;
int ycoordinate = 1 ;
épingles à ultrasons
const int Trig_pin = 5 ; broche pour déclencher les impulsions d’entrée
const int Echo_pin = 6 ; broche pour recevoir l’écho OUPUT
longue durée ; Combien de temps il faut pour le son de rebondir
broches de moteurs
const int Motor1Pin = 9 ; Côté gauche
const int Motor2Pin = 10 ; côté droit
le tableau qu’il suit avec
Il peut s’agir d’un tableau de n’importe quelle taille
Assurez-vous juste que le robot a un espace libre pour se déplacer à partir de sa position initiale.
int arraything [6] [6] =
{
{1,1,1,1,1,1}
,
{1,1,0,1,0,1}
,
{1,1,0,1,0,1}
,
{1,1,0,1,0,1}
,
{1,1,0,1,1,1}
,
{1,1,1,0,1,1}
};
annuler le programme d’installation () {}
Encodeur rotatif
pinMode (encoderAPin, entrée) ;
pinMode (encoderBPin, entrée) ;
Serial.Begin(9600) ;
Serial.println ("dès à présent...") ;
\
par ultrasons
pinMode (Trig_pin, sortie) ; initialiser la goupille d’impulsion en sortie :
pinMode (entrée, Echo_pin) ; initialiser la broche echo_pin comme intrant :
motorsmmmmmmmmm
motor1.Attach(Motor1Pin) ;
motor2.Attach(Motor2Pin) ;
}
En anglais, ce programme voit si il y a quelque chose en face d’elle.
Si non, il se déplace tout droit. Dans l’affirmative, il vérifie son droit.
Si son droit est libre, il tourne comme ça, sinon, il vérifie sa gauche.
Si la gauche est libre, il tourne comme ça, sinon, il tourne à 180 degrés
et remonte la façon dont il est venu.
//
Près do est à encorporate début et destination comme conclusion de la trajectoire optimale
//
Dernière consiste à utiliser les ultrasons avec la grille.
void loop () {}
alors que (1 == 1) {}
Si (isFrontOpen() == true) {}
moveForward() ;
retarder (2000) ;
}
d’autre
Si (isRightOpen() == true) {}
turnRight() ;
retarder (2000) ;
}
d’autre
Si (isLeftOpen() == true) {}
turnLeft() ;
retarder (2000) ;
}
else {}
turnAround() ;
retarder (2000) ;
}
}
}
Vérifie si il y a quelque chose à droite en face de lui à l’aide de grilles
Boolean isFrontOpen () {}
int nextNumber = getFrontNumber() ;
Si (nextNumber == 0) {}
retourne la valeur true ;
}
else {}
retourne la valeur false ;
}
}
Vérifie si il y a quelque chose pour le droit à l’aide de grilles
Boolean isRightOpen() {}
int nextNumber = getRightNumber() ;
Si (nextNumber == 0) {}
retourne la valeur true ;
}
else {}
retourne la valeur false ;
}
}
Vérifie si il y a quelque chose à gauche à l’aide de grilles
Boolean isLeftOpen() {}
int nextNumber = getLeftNumber() ;
Si (nextNumber == 0) {}
retourne la valeur true ;
}
else {}
retourne la valeur false ;
}
}
Se déplace vers l’avant.
void moveForward () {}
motor1.Write(180) ;
motor2.Write(0) ;
Serial.println("Forward") ;
Si (robotDirection == 0)
yCoordinate = ycoordinate - 1 ;
Si (robotDirection == 1)
xCoordinate = xcoordinate + 1 ;
Si (robotDirection == 2)
yCoordinate = ycoordinate + 1 ;
Si (robotDirection == 3)
xCoordinate = xcoordinate - 1 ;
retard (100) ;
/ * Serial.print("xcoordinate") ;
Serial.Print(xCoordinate) ;
retard (500) ;
Serial.Print ("ycoordinate") ;
Serial.Print(yCoordinate) ;
retard (500) ;
Serial.Print ("direction du robot:") ;
Serial.Print(robotDirection) ;
Delay(500) ;
Serial.println () ;
Delay(1000) ;
*/
Delay(800) ;
}
Tours de 90 degrés vers la droite
tournedroite Sub () {}
motor1.Write (60) ;
motor2.Write (60) ;
Delay(178) ;
motor2.Write(95) ;
Delay(65) ;
motor1.Write(90) ;
Serial.println("Right") ;
Si (robotDirection == 0)
robotDirection = 1 ;
ElseIf (robotDirection == 1)
robotDirection = 2 ;
ElseIf (robotDirection == 2)
robotDirection = 3 ;
ElseIf (robotDirection == 3)
robotDirection = 0 ;
retard (500) ;
Serial.Print("xCoordinate") ;
Serial.Print(xCoordinate) ;
retard (500) ;
Serial.Print ("ycoordinate") ;
Serial.Print(yCoordinate) ;
retard (500) ;
Serial.Print ("direction du robot:") ;
Serial.Print(robotDirection) ;
retard (500) ;
Serial.println() ;
Delay(1000) ;
}
Tours de 90 degrés vers la gauche
tournegauche Sub () {}
motor1.Write(120) ;
motor2.Write(120) ;
Delay(325) ;
motor2.Write(95) ;
Delay(65) ;
motor1.Write(90) ;
Serial.println("left") ;
Si (robotDirection == 0)
robotDirection = 3 ;
ElseIf (robotDirection == 1)
robotDirection = 0 ;
ElseIf (robotDirection == 2)
robotDirection = 1 ;
ElseIf (robotDirection == 3)
robotDirection = 2 ;
retard (500) ;
Serial.Print("xCoordinate") ;
Serial.Print(xCoordinate) ;
retard (500) ;
Serial.Print ("ycoordinate") ;
Serial.Print(yCoordinate) ;
retard (500) ;
Serial.Print ("direction du robot:") ;
Serial.Print(robotDirection) ;
Delay(500) ;
Serial.println() ;
Delay(1000) ;
}
Tours de 180 degrés
Revirement Sub () {}
Delay(1000) ;
Serial.println("Around") ;
Si (robotDirection == 0)
robotDirection = 2 ;
ElseIf (robotDirection == 1)
robotDirection = 3 ;
ElseIf (robotDirection == 2)
robotDirection = 0 ;
ElseIf (robotDirection == 3)
robotDirection = 1 ;
retard (500) ;
Serial.Print("xCoordinate") ;
Serial.Print(xCoordinate) ;
retard (500) ;
Serial.Print ("ycoordinate") ;
Serial.Print(yCoordinate) ;
retard (500) ;
Serial.Print ("direction du robot:") ;
Serial.Print(robotDirection) ;
Delay(500) ;
Serial.println() ;
Delay(1000) ;
}
Obtient le nombre sur la grille du droit spatial en face d’elle.
int getFrontNumber() {}
Si (robotDirection == 0) {}
retour arraything [ycoordinate - 1] [xcoordinate] ;
}
Si (robotDirection == 1) {}
retour arraything [ycoordinate] [xcoordinate + 1] ;
}
Si (robotDirection == 2) {}
retour arraything [ycoordinate + 1] [xcoordinate] ;
}
Si (robotDirection == 3) {}
retour arraything [ycoordinate] [xcoordinate - 1] ;
}
}
Obtient le nombre sur la grille de l’espace à droite de celui-ci.
int getRightNumber() {}
Si (robotDirection == 0) {}
retour arraything [ycoordinate] [xcoordinate + 1] ;
}
Si (robotDirection == 1) {}
retour arraything [ycoordinate + 1] [xcoordinate] ;
}
Si (robotDirection == 2) {}
retour arraything [ycoordinate] [xcoordinate - 1] ;
}
Si (robotDirection == 3) {}
retour arraything [ycoordinate - 1] [xcoordinate] ;
}
}
Obtient le nombre sur la grille de l’espace à gauche de celui-ci.
int getLeftNumber() {}
Si (robotDirection == 0) {}
retour arraything [ycoordinate] [xcoordinate - 1] ;
}
Si (robotDirection == 1) {}
retour arraything [ycoordinate - 1] [xcoordinate] ;
}
Si (robotDirection == 2) {}
retour arraything [ycoordinate] [xcoordinate + 1] ;
}
Si (robotDirection == 3) {}
retour arraything [ycoordinate + 1] [xcoordinate] ;
}
}
Voici un exemple de programme pour les capteurs à ultrasons. Ce programme permettra juste d’éviter tout ce qui est dans la manière.
Inclus dans le programme est de programmation pour un servo supplémentaire simplement tourner en arrière sur le dessus du robot. Si vous ne faites pas cela sur votre robot, ignorer cette partie du code, il ne devrait avoir aucun effet sur votre programme.
CE CODE ÉVITE LES OBSTACLES
IL PEUT RENCONTRER SOIT UN OBSTACLE À DROITE OU À GAUCHE
PAR CONSÉQUENT, NOUS PROGRAMMÉ À TOUR VERS LA GAUCHE SI ELLE A RENCONTRÉ UN OBSTACLE SUR LA DROITE
ET À DROITE S’IL RENCONTRÉES SUR LA GAUCHE
Pranav
/*
1 par ultrasons
Broche 5 déclenche le pouls (fil jaune)
Broche 6 reçoit l’écho (fil Orange)
2 par ultrasons
Broche 7 déclenche l’impulsion (fil jaune)
Broche 8 reçoit l’écho (fil Orange)
*/
#include « Servo.h »
Servo motorR ;
Servo motorL ;
Servo-moteurs ;
épingles à ultrasons
Droit à ultrasons
const int Trig1_pin = 5 ; broche pour déclencher les impulsions d’entrée
const int Echo1_pin = 6 ; broche pour recevoir l’écho OUPUT
longue durée1 ; Combien de temps il faut pour le son de rebondir
Gauche à ultrasons
const int Trig2_pin = 7 ;
const int Echo2_pin = 8 ;
longue Durée2 ;
broches de moteurs
const int MotorLPin = 2 ; Gauche
const int MotorRPin = 13 ; Droit
rotation moteur ultrasonique
const int MotorSpinPin = 3 ;
int spinCounter = 0 ;
détection ou non
isARightWallThere booléen = false ;
isALeftWallThere booléen = false ;
quel moyen de transformer
booléenne droite = true ;
void setup() {}
Serial.Begin(9600) ;
Serial.println (« départ ») ;
1 par ultrasons
pinMode (Trig1_pin, sortie) ; initialiser la goupille d’impulsion en sortie :
pinMode (entrée, Echo1_pin) ; initialiser la broche echo_pin comme intrant :
2 par ultrasons
pinMode (7, sortie) ; initialiser la goupille d’impulsion en sortie :
pinMode (entrée, Echo2_pin) ; initialiser la broche echo_pin comme intrant :
moteurs
motorR.attach(MotorRPin) ;
motorL.attach(MotorLPin) ;
motorS.attach(MotorSpinPin) ;
}
void loop() {}
alors que (1 == 1) {}
Si (spinCounter < 2) {}
motorS.write(160) ;
}
else {}
motorS.write(20) ;
}
Si (spinCounter > = 3) {}
spinCounter = 0 ;
}
spinCounter ++ ;
checkTheRightWall() ;
checkTheLeftWall() ;
Si (isARightWallThere == false & & isALeftWallThere == false) {}
motorR.write(120) ;
motorL.write(72) ;
}
else {}
Si (isARightWallThere == true & & isALeftWallThere == true) {}
motorR.write(0) ;
motorL.write(180) ;
Delay(300) ;
motorR.write(80) ;
motorL.write(80) ;
Delay(500) ;
motorR.write(0) ;
motorL.write(180) ;
}
else {}
Si (isARightWallThere == false & & isALeftWallThere == true) {}
motorR.write(50) ;
motorL.write(90) ;
}
else {}
Si (isARightWallThere == true & & isALeftWallThere == false) {}
motorR.write(90) ;
motorL.write(140) ;
}
}
}
}
}
}
void checkTheRightWall () {}
digitalWrite (Trig1_pin, basse) ;
delayMicroseconds(2) ;
digitalWrite (Trig1_pin, élevé) ;
delayMicroseconds(10) ;
digitalWrite (Trig1_pin, basse) ;
Durée1 = pulseIn(Echo1_pin,10) ;
Serial.println (« Durée1: ") ;
Serial.println (Durée1, DEC) ;
Si ((Durée1 > 4000 || Durée1 == 0)) {}
isARightWallThere = false ;
}
else {}
isARightWallThere = true ;
}
}
void checkTheLeftWall() {}
digitalWrite (7, faible) ;
delayMicroseconds(2) ;
digitalWrite (7, HIGH) ;
delayMicroseconds(10) ;
digitalWrite (7, faible) ;
Durée2 = pulseIn(Echo2_pin,10) ;
Serial.println ("Durée2:") ;
Serial.println (Durée2, DEC) ;
Si ((Durée2 > 4000 || Durée2 == 0)) {}
isALeftWallThere = false ;
}
else {}
isALeftWallThere = true ;
}
}
Si vous avez le temps, découvrez l’une des pages de mon ami. C’est lui qui a fait toute la programmation pour nous.
Voici le lien vers sa page :
La fin
Étincelle Industries,
