Étape 10 : Programmer votre Robot
Maintenant que le robot est terminé, vous aurez envie de programmer c' est un comportement.
Découvrez le code pour ce robot.
#include
créer des objets de servo Servo leftMotor ; Servo rightMotor ;
const int serialPeriod = 250 ; n’imprimer vers la console série, chaque 1/4 seconde unsigned long timeSerialDelay = 0 ;
const int loopPeriod = 20 ; une période de 20ms = une fréquence de 50Hz unsigned long timeLoopDelay = 0 ;
Spécifiez la trig & echo épingles utilisées pour les capteurs à ultrasons const int ultrasonic2TrigPin = 8 ; const int ultrasonic2EchoPin = 9 ;
int ultrasonic2Distance ; int ultrasonic2Duration ;
Définissez les États #define DRIVE_FORWARD 0 #define TURN_LEFT 1
État int = DRIVE_FORWARD ; 0 = le lecteur vers l’avant (par défaut), 1 = tourner à gauche
void setup() {Serial.begin(9600); / / capteur à ultrasons pin configurations pinMode (ultrasonic2TrigPin, sortie); pinMode (ultrasonic2EchoPin, INPUT); leftMotor.attach(13) ; rightMotor.attach(12);}
void loop() {debugOutput(); / / impressions débogage des messages de la console série if(millis() - timeLoopDelay > = loopPeriod) {readUltrasonicSensors(); / / lire et stocker les distances mesurées stateMachine() ; timeLoopDelay = millis();}}
void stateMachine() {if(state == DRIVE_FORWARD) / / aucun obstacle ne détecté {si (ultrasonic2Distance > 6 || ultrasonic2Distance < 0) / / si il n’y a rien en face de nous (Remarque : ultrasonicDistance sera négatif pour certains ultrason si il n’y a rien dans la gamme) {/ / drive avant rightMotor.write(180) ; leftMotor.write(0);} d’autre / / il existe un objet devant nous {État = TURN_LEFT;}} d’autre if(state == TURN_LEFT) / / obstacle détecté--tourner à gauche {unsigned long timeToTurnLeft = 1100; / / il faut environ 1,1 secondes pour tourner à 90 degrés unsigned long turnStartTime = millis(); / / save le temps qui nous a commencé à tourner
while((Millis()-turnStartTime) < timeToTurnLeft) / / séjour dans cette boucle, jusqu'à ce que soit écoulé timeToTurnLeft (1,1 seconde) {/ / tourner à gauche rightMotor.write(180) ; leftMotor.write(180);} Etat = DRIVE_FORWARD ; } }
void readUltrasonicSensors() {/ / ultrasons 2 digitalWrite (ultrasonic2TrigPin, HIGH); delayMicroseconds(10); / / doit garder l’axe de triglycéride élevé pour au moins 10us digitalWrite (ultrasonic2TrigPin, LOW); ultrasonic2Duration = pulseIn (ultrasonic2EchoPin, HIGH); ultrasonic2Distance = (ultrasonic2Duration/2) / 29;}
void debugOutput() {if((millis() - timeSerialDelay) > serialPeriod) {Serial.print ("ultrasonic2Distance:") ; Serial.Print(ultrasonic2Distance) ; Serial.Print("cm") ; Serial.println() ; timeSerialDelay = millis() ; } }