Klimplant Rover Robot (4 / 7 stap)

Stap 4: Arduino Sketch

Klimplant Rover Bot
#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 irPin = 0;  Scherpe infraroodsensor pin
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
}
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(2000);
}
anders als (rightDistance > leftDistance) //if recht minder wordt belemmerd
{
leftMotor.write(LForward);
rightMotor.write(RBackward); Sla rechtsaf
delay(2000);
}
anders //if die ze even zijn geblokkeerd
{
leftMotor.write(LForward);
rightMotor.write(RBackward); 180 graden draaien
delay(2000);
}
}

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;
}