Étape 4: programmation
LE CODE :
/*
> Concevoir un obstacle en évitant le robot en utilisant un Arduino NANO et parallaxe PING))) capteur de Distance à ultrasons
> Version 2
> M Zoran.
*/
bibliothèque de #include //include servo
const int RForward = 0 ;
const int RBackward = 180 ;
const int LForward = RBackward ;
const int LBackward = RForward ;
const int RNeutral = 90 ;
const int LNeutral = 90 ; constantes de vitesse du moteur
const int pingPin = 7 ;
const int dangerThresh = 10 ; seuil d’obstacles (en cm)
int leftDistance, rightDistance ; distance de chaque côté
Servo panMotor ;
Servo leftMotor ;
Servo rightMotor ; déclarer des moteurs
longue durée ; temps nécessaire pour recevoir PING))) signal
void setup()
{
rightMotor.attach(11) ;
leftMotor.attach(10) ;
panMotor.attach(6) ; fixer les moteurs aux broches appropriées
panMotor.write(90) ; jeu de PING))) pan vers le centre
}
void loop()
{
int distanceFwd = ping() ;
Si (distanceFwd > dangerThresh) //if chemin est clair
{
leftMotor.write(LForward) ;
rightMotor.write(RForward) ; aller de l’avant
Delay(100) ;
}
d’autre //if chemin est bloqué
{
leftMotor.write(LNeutral) ;
rightMotor.write(RNeutral) ;
panMotor.write(0) ;
Delay(500) ;
rightDistance = ping() ; Scan vers la droite
Delay(500) ;
panMotor.write(180) ;
Delay(700) ;
leftDistance = ping() ; numérisation vers la gauche
Delay(500) ;
panMotor.write(90) ; retourner au centre
Delay(100) ;
compareDistance() ;
}
}
Sub compareDistance()
{
Si (leftDistance > rightDistance) //if gauche est moins obstruée
{
leftMotor.write(LBackward) ;
rightMotor.write(RForward) ; tourner à gauche
Delay(500) ;
}
ElseIf (rightDistance > leftDistance) //if droit est moins obstruée
{
leftMotor.write(LForward) ;
rightMotor.write(RBackward) ; tourner à droite
Delay(500) ;
}
d’autre //if qu'ils sont tout aussi obstrués
{
leftMotor.write(LForward) ;
rightMotor.write(RBackward) ; virage de 180 degrés
Delay(1000) ;
}
}
ping() longue
{
Impulsion de signal envoyer des PING)))
pinMode (pingPin, sortie) ;
digitalWrite (pingPin, basse) ;
delayMicroseconds(2) ;
digitalWrite (pingPin, HIGH) ;
delayMicroseconds(5) ;
digitalWrite (pingPin, basse) ;
Obtenir la durée qu’il faut pour recevoir l’écho
pinMode (pingPin, entrée) ;
durée = pulseIn (pingPin, HIGH) ;
Convertir la durée en distance
durée de retour / 29 / 2 ;
}
Continuer sur pour des photos supplémentaires et la conclusion.