Hinder att undvika Robot V2 (4 / 5 steg)
Steg 4: programmering
KODEN:
/*
> Designa ett hinder att undvika robot med en Arduino NANO och Parallax PING))) ultraljud avstånd Sensor
> Version 2
> Zoran M.
*/
#include //include servo bibliotek
CONST int RForward = 0;
CONST int RBackward = 180;
CONST int LForward = RBackward;
CONST int LBackward = RForward;
CONST int RNeutral = 90.
CONST int LNeutral = 90. konstanter för varvtal
CONST int pingPin = 7.
CONST int dangerThresh = 10; tröskeln för hinder (i cm)
int leftDistance, rightDistance; avstånd på vardera sidan
Servo panMotor;
Servo leftMotor;
Servo rightMotor; deklarera motorer
lång varaktighet; tid det tar att ta emot PING))) signal
void setup()
{
rightMotor.attach(11);
leftMotor.attach(10);
panMotor.attach(6); bifoga motorer till rätt stift
panMotor.write(90); Set PING))) pan att centrera
}
void loop()
{
int distanceFwd = ping();
om (distanceFwd > dangerThresh) //if sökväg är klart
{
leftMotor.write(LForward);
rightMotor.write(RForward); gå framåt
Delay(100);
}
annat //if väg är blockerad
{
leftMotor.write(LNeutral);
rightMotor.write(RNeutral);
panMotor.write(0);
Delay(500);
rightDistance = ping(); Skanna till höger
Delay(500);
panMotor.write(180);
Delay(700);
leftDistance = ping(); Skanna till vänster
Delay(500);
panMotor.write(90); tillbaka till center
Delay(100);
compareDistance();
}
}
void compareDistance()
{
om (leftDistance > rightDistance) //if vänster hindras mindre
{
leftMotor.write(LBackward);
rightMotor.write(RForward); Sväng vänster
Delay(500);
}
annars om (rightDistance > leftDistance) //if höger hindras mindre
{
leftMotor.write(LForward);
rightMotor.write(RBackward); Sväng höger
Delay(500);
}
annat //if de hindras lika
{
leftMotor.write(LForward);
rightMotor.write(RBackward); vända 180 grader
Delay(1000);
}
}
lång ping()
{
Skicka ut PING))) signalen puls
pinMode (pingPin, produktionen);
digitalWrite (pingPin, låg);
delayMicroseconds(2);
digitalWrite (pingPin, hög);
delayMicroseconds(5);
digitalWrite (pingPin, låg);
Få tid det tar för att få eko
pinMode (pingPin, ingång);
längd = pulseIn (pingPin, hög);
Konvertera varaktighet till avstånd
returnera varaktighet / 29 / 2;
}
Fortsätt för ytterligare bilder och ingående.