Obstakel te vermijden Robot V2 (4 / 5 stap)

Stap 4: programmeren

Het basisconcept van de code is voor de robot te worden voortdurend voor objecten voor het terwijl het bewegen. Dit wordt gedaan door de PING))) sensor waar het stuurt een 40 kHz Tjilpen die niet hoorbaar, en haalt de echo. Het programma krijgt dan de duur duurt voor de pols om terug te gaan en zet dit om in afstand. Zodra de robot een object waar de afstand naar voren groter is dan de drempel van gevaar - het pad is duidelijk ontdekt dus ons programma vertelt de robot om vooruit te gaan. Anders zal de robot stoppen dan pan links en rechts. Afhankelijk van welke weg is "beter", zal de robot draaien om die duidelijker weg en beginnen in die richting. De code wordt alleen op deze manier. Dat is eigenlijk de "theorie" achter de code. Als u van eventuele verbeteringen die kunnen worden gedaan om de code weet, zou ik graag uw feedback te horen! Doorgaan naar de laatste stap voor extra foto's en de sluiting.

DE CODE:

/*
> Ontwerp obstakel te vermijden robot met behulp van een Arduino NANO en Parallax PING))) ultrasone afstandssensor
> Versie 2
> Zoran M.
*/

#include //include servo bibliotheek

Const int RForward = 0;
Const int RBackward = 180;
Const int LForward = RBackward;
Const int LBackward = RForward;
Const int RNeutral = 90;
Const int LNeutral = 90; constanten voor de motorsnelheid
Const int pingPin = 7;
Const int dangerThresh = 10; drempel voor obstakels (in cm)
int leftDistance, rightDistance; afstanden aan weerszijden
Servo panMotor;
Servo leftMotor;
Servo rightMotor; verklaren van motoren
lange duur; de tijd het neemt om te ontvangen van PING))) signaal

VOID Setup
{
rightMotor.attach(11);
leftMotor.attach(10);
panMotor.attach(6); motoren koppelen aan de juiste pinnen
panMotor.write(90); set PING))) pan om te centreren
}

void loop
{
int distanceFwd = ping();
Als (distanceFwd > dangerThresh) //if pad is duidelijk
{
leftMotor.write(LForward);
rightMotor.write(RForward); vooruit
delay(100);
}
anders //if pad is geblokkeerd
{
leftMotor.write(LNeutral);
rightMotor.write(RNeutral);
panMotor.write(0);
delay(500);
rightDistance = ping(); scannen naar rechts
delay(500);
panMotor.write(180);
delay(700);
leftDistance = ping(); scannen naar links
delay(500);
panMotor.write(90); terug naar centrum
delay(100);
compareDistance();
}
}

VOID compareDistance()
{
Als (leftDistance > rightDistance) //if links minder wordt belemmerd
{
leftMotor.write(LBackward);
rightMotor.write(RForward); Sla linksaf
delay(500);
}
anders als (rightDistance > leftDistance) //if recht minder wordt belemmerd
{
leftMotor.write(LForward);
rightMotor.write(RBackward); Sla rechtsaf
delay(500);
}
anders //if die ze even zijn geblokkeerd
{
leftMotor.write(LForward);
rightMotor.write(RBackward); 180 graden draaien
delay(1000);
}
}

lange ping()
{
Sturen PING))) signaal pulse
pinMode (pingPin, OUTPUT);
digitalWrite (pingPin, laag);
delayMicroseconds(2);
digitalWrite (pingPin, hoge);
delayMicroseconds(5);
digitalWrite (pingPin, laag);

Krijgen van de duur die nodig is om te ontvangen van echo
pinMode (pingPin, INPUT);
duur = pulseIn (pingPin, hoge);

Duur converteren naar afstand
retourneren van duur / 29 / 2;
}

Voort voor extra foto's en de sluiting.