Sökvägen Clearing Wall-E (4 / 4 steg)
Steg 4: Koden
#include
CONST int left1 = 11; / / vänster motor stift 1
CONST int left2 = 10; / / vänster motor stift 2
CONST int right1 = 13. rätt motor stift 1
CONST int right2 = 12; //right motor 2
Servo handServo;
CONST int Trig = 4; / / HC-SR04 ultraljud sensorns trigger pin
CONST int Echo = 3; / / och det är läsa pin
void setup() {
Ange motor stiften
pinMode (left1, produktionen);
pinMode (left2, produktionen);
pinMode (right1, produktionen);
pinMode (right2, produktionen);
pinMode (Echo, indata);
pinMode (Trig, OUTPUT);
handServo.attach(5); / / Anslut servo till stift 5
}
void loop() {
lång varaktighet, cm; / / set variabel för varaktigheten av ping och avståndsenheter
gör motorerna rotera i sync (robot går framåt)
handServo.write (60), //initialize hand position
digitalWrite (left1, hög);
digitalWrite (left2, låg);
digitalWrite (right1, hög);
digitalWrite (right2, låg);
Delay(10000);
digitalWrite (left2, hög);
digitalWrite (left1, låg);
digitalWrite (right1, hög);
digitalWrite (right2, låg);
Delay(1000);
digitalWrite (left1, hög);
digitalWrite (left2, låg);
digitalWrite (right1, hög);
digitalWrite (right2, låg);
Delay(10000);
utseende till se om theres något framme
digitalWrite (Trig, låg), //give en låg på den utlösande faktorn för en ren hög
delayMicroseconds(2);
digitalWrite (Trig, hög); //now skicka en hög puls
delayMicroseconds(5);
digitalWrite (Trig, låg); //and låg igen
längd = pulseIn (Echo, hög); //read echo PIN-koden för en hög
ultrasonic sensorn jag använde (HC-SR04) har 4 stift två för makt och marken,
en för att skicka en puls och den sista för att läsa puls skickas
du beräknar avståndet ett objekt är på genom att beräkna tiden
skillnaden mellan puls skickas och när det blev läst.
cm = microsecondsToCentimeters(duration);
om puls läsa var under en viss gräns flytta servon
om (cm < 18) {
handServo.write(180);
Delay(30);
}
annat {
handServo.write (60), //hands går tillbaka upp
}
}
lång microsecondsToCentimeters(long microseconds)
{
ljud hastighet är 29 mikrosekunder per centimeter.
pulsen gör en resa, så avståndet är delat med 2
återvända mikrosekunder / 29 / 2;
}