Le Robot américain paresseux (4 / 4 étapes)

Étape 4: codage

#include « Servo.h »
#include < SoftwareSerial.h >

const int Trig_pinRight = 7 ;   Impulsion de déclencheurs pour capteur à ultrasons à droite
const int Echo_pinRight = 13 ;     Echo reçoit pour capteur à ultrasons à droite
const int Trig_pinLeft = 12 ;   Déclenche l’impulsion pour LEFT capteur à ultrasons
const int Echo_pinLeft = 8 ;     Reçoit l’écho laissé par ultrasons capteur
long durationRight ;            Temps mis par l’impulsion de rebondir à droite capteur ultrasonique
long durationLeft ;             Durée de l’impulsion de rebondir à gauche capteur ultrasonique

Servo motorLeft ;               VEX gauche moteur
Servo motorRight ;              VEX droite moteur

void setup() {}

Serial.Begin(9600) ;           Mettre en place une bibliothèque série à 9600 bps

Serial.println("Initializing...") ;  Initialisation de l’impression... pour confirmer le code travaille en collaboration avec la bibliothèque série

motorLeft.attach(11) ;         Le moteur gauche est sur la broche 11
motorRight.attach(10) ;        Le moteur droit est sur la broche 10


Serial.println (« départ ») ;
initialiser la goupille d’impulsion en sortie :
pinMode (Trig_pinRight, sortie) ;
initialiser la goupille d’impulsion en sortie :
pinMode (Trig_pinLeft, sortie) ;
initialiser la broche echo_pin comme intrant :
pinMode (entrée, Echo_pinRight) ;
initialiser la broche echo_pin comme intrant :
pinMode (entrée, Echo_pinLeft) ;

}

void loop()
{

Légumineuses et Echo pour capteur à ultrasons à droite ; Processus de détermination et les durées d’impression pour capteur à ultrasons droit

digitalWrite (Trig_pinRight, basse) ;
delayMicroseconds(2) ;
digitalWrite (Trig_pinRight, élevé) ;
delayMicroseconds(5) ;
digitalWrite (Trig_pinRight, basse) ;
durationRight = pulseIn(Echo_pinRight,HIGH) ;
Serial.println ("durationRight:") ;
Serial.println (durationRight, DEC) ;

Légumineuses et Echo pour laissé par ultrasons capteur ; Processus de détermination et les durées d’impression pour capteur à ultrasons gauche

digitalWrite (Trig_pinLeft, basse) ;
delayMicroseconds(2) ;
digitalWrite (Trig_pinLeft, élevé) ;
delayMicroseconds(5) ;
digitalWrite (Trig_pinLeft, basse) ;
durationLeft = pulseIn(Echo_pinLeft,HIGH) ;
Serial.println ("durationLeft:") ;
Serial.println (durationLeft, DEC) ;

Si (durationRight > 0 & & durationRight < 4000 & & durationRight > 0 & & durationLeft < 4000) / / STOP lorsque l’homme est trop proche pour les deux capteurs
{
motorLeft.write(90) ;
motorRight.write(90) ;
}
Si (durationRight > 4000 & & durationRight < 10000 & & durationLeft > 4000 & & durationLeft < 10000) / / déplacer vers l’avant lorsque l’homme est equadistant pour les deux capteurs
{
motorLeft.write(113) ;
motorRight.write(68) ;
}
Si (durationLeft - 1000 > durationRight & & durationRight > 4000) / / tourner à droite lorsque l’homme se trouve à proximité de la sonde droite
{
motorLeft.write(120) ;
motorRight.write(100) ;
Delay(500) ;

motorLeft.write(120) ;
motorRight.write(75) ;
Delay(2000) ;
}
Si (durationRight - 1000 > durationLeft & & durationLeft > 4000) / / tourner à gauche lorsque l’homme est plus proche du capteur gauche
{
motorLeft.write(100) ;
motorRight.write(75) ;
Delay(500) ;

motorLeft.write(120) ;
motorRight.write(75) ;
Delay(5000) ;
}
Si (durationRight > 10000 & & durationLeft > 10000) / / déplacer vers l’avant rapide lorsque l’homme est trop loin mais encore equadistant
{
motorLeft.write(135) ;
motorRight.write(45) ;
}
}