Stap 4: codering
Tot slot, hier is de code die we gebruikt:#include "Servo.h"
#include < SoftwareSerial.h >
Const int Trig_pinRight = 7; Triggers Pulse voor rechts ultrasone Sensor
Const int Echo_pinRight = 13; Recevies Echo voor rechts ultrasone Sensor
Const int Trig_pinLeft = 12; Pulse triggers voor links ultrasone Sensor
Const int Echo_pinLeft = 8; Echo ontvangt voor links ultrasone Sensor
lange durationRight; Tijd die nodig is voor pulse om terug te stuiteren rechts ultrasone sensor
lange durationLeft; Tijd die nodig is voor pulse om terug te stuiteren op de ultrasone Sensor links
Servo motorLeft; LINKS VEX Motor
Servo motorRight; RECHTS VEX Motor
VOID Setup {}
Serial.begin(9600); Instellen van seriële bibliotheek 9600 bps
Serial.println("Initializing..."); Bezig met initialiseren... afdrukken te bevestigen code werkt met seriële bibliotheek
motorLeft.attach(11); LINKS Motor is in pin 11
motorRight.attach(10); RECHT Motor is in pin 10
Serial.println ("vanaf");
Initialiseer de puls-pin als output:
pinMode (Trig_pinRight, uitvoer);
Initialiseer de puls-pin als output:
pinMode (Trig_pinLeft, uitvoer);
Initialiseer de echo_pin pin als input:
pinMode (Echo_pinRight, INPUT);
Initialiseer de echo_pin pin als input:
pinMode (Echo_pinLeft, INPUT);
}
void loop
{
Pols en Echo voor rechts ultrasone Sensor; Proces voor het bepalen en het afdrukken van de duur voor recht ultrasone Sensor
digitalWrite (Trig_pinRight, laag);
delayMicroseconds(2);
digitalWrite (Trig_pinRight, hoge);
delayMicroseconds(5);
digitalWrite (Trig_pinRight, laag);
durationRight = pulseIn(Echo_pinRight,HIGH);
Serial.println ("durationRight:");
Serial.println (durationRight, DEC);
Pols en Echo voor links ultrasone Sensor; Proces voor het bepalen en afdrukken van de duur voor de ultrasone Sensor links
digitalWrite (Trig_pinLeft, laag);
delayMicroseconds(2);
digitalWrite (Trig_pinLeft, hoge);
delayMicroseconds(5);
digitalWrite (Trig_pinLeft, laag);
durationLeft = pulseIn(Echo_pinLeft,HIGH);
Serial.println ("durationLeft:");
Serial.println (durationLeft, DEC);
Als (durationRight > 0 & & durationRight < 4000 & & durationRight > 0 & & durationLeft < 4000) / / STOP als mens is te dicht bij beide sensoren
{
motorLeft.write(90);
motorRight.write(90);
}
Als (durationRight > 4000 & & durationRight < 10000 & & durationLeft > 4000 & & durationLeft < 10000) / / verplaatsen naar voren wanneer menselijke is equadistant aan beide sensoren
{
motorLeft.write(113);
motorRight.write(68);
}
Als (durationLeft - 1000 > durationRight & & durationRight > 4000) / / RECHTSAF wanneer mens dicht bij de juiste sensor ligt
{
motorLeft.write(120);
motorRight.write(100);
delay(500);
motorLeft.write(120);
motorRight.write(75);
delay(2000);
}
Als (durationRight - 1000 > durationLeft & & durationLeft > 4000) / / LINKSAF als mens dichter bij linker sensor
{
motorLeft.write(100);
motorRight.write(75);
delay(500);
motorLeft.write(120);
motorRight.write(75);
delay(5000);
}
Als (durationRight > 10000 & & durationLeft > 10000) / / verplaatsen van FAST FORWARD wanneer menselijke is te ver weg maar nog steeds equadistant
{
motorLeft.write(135);
motorRight.write(45);
}
}