HINDER att undvika BOT (2 / 2 steg)
Steg 2: Skriva kod
int echoPin = 9;
int en = 3;
int b = 2;
int c = 5;
int d = 4;
float avstånd;
float pingTime;
flyta speedOfSound = 347.22;
float lDistance;
float rDistance;
void setup() {
Serial.BEGIN(9600);
pinMode(trigPin,OUTPUT);
pinMode(a,OUTPUT);
pinMode(b,OUTPUT);
pinMode(c,OUTPUT);
pinMode(d,OUTPUT);
pinMode(echoPin,INPUT);
}
float meaSure()
{
digitalWrite(trigPin,LOW);
delayMicroseconds(2000);
digitalWrite(trigPin,HIGH);
delayMicroseconds(10);
digitalWrite(trigPin,LOW);
pingTime=pulseIn(echoPin,HIGH);
avstånd =(speedOfSound*pingTime)/20000; //in cm
returnera avstånd;
}
void forward() {
digitalWrite(a,HIGH);
digitalWrite(b,LOW);
digitalWrite(c,HIGH);
digitalWrite(d,LOW);
Delay(500);
stopp();
}
void backward() {
digitalWrite(b,HIGH);
digitalWrite(a,LOW);
digitalWrite(d,HIGH);
digitalWrite(c,LOW);
Delay(1000);
stopp();
}
void left() {
digitalWrite(a,HIGH);
digitalWrite(b,LOW);
digitalWrite(d,HIGH);
digitalWrite(c,LOW);
Delay(570);
stopp();
}
void right() {
digitalWrite(b,HIGH);
digitalWrite(a,LOW);
digitalWrite(c,HIGH);
digitalWrite(d,LOW);
Delay(1000);
stopp();
}
void stopp() {
digitalWrite(a,LOW);
digitalWrite(b,LOW);
digitalWrite(c,LOW);
digitalWrite(d,LOW);
Serial.println("stopp");
}
void loop() {
mäta avstånd
digitalWrite(trigPin,LOW);
delayMicroseconds(2000);
digitalWrite(trigPin,HIGH);
delayMicroseconds(10);
digitalWrite(trigPin,LOW);
pingTime=pulseIn(echoPin,HIGH);
avstånd =(speedOfSound*pingTime)/20000; //in cm
IF(Distance>20) {
Forward();
Serial.println ("ingen obstackle");
}
om (avstånd < = 20) {
stopp();
Serial.println ("kontrollera vänster");
Left();
Delay(250);
lDistance = meaSure();
Serial.println ("Kontrollera rätt");
Right();
Delay(250);
rDistance=meaSure();
om (rDistance > lDistance & & rDistance > 20) {
Forward();
Serial.println ("går framåt i rätt väg");
}
om (rDistance < = lDistance & & lDistance > 20) {
Right();
Delay(250);
Forward();
Serial.println ("går framåt i vänster väg");
}
annat {
Serial.println ("i grottan");
Serial.println (gå tillbaka och kontrollera igen");
Backward();
stopp();
Left();
Delay(250);
lDistance=meaSure();
Right();
Delay(250);
rDistance=meaSure();
om (rDistance > lDistance & & rDistance > 20) {
Forward();
}
om (rDistance < = lDistance & & lDistance > 20) {
Right();
Delay(250);
Forward();
}
}
}
}