Stap 2: Schrijven van Code
int echoPin = 9;
int een = 3;
int b = 2;
int c = 5;
int d = 4;
float afstand;
float pingTime;
zweven 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);
afstand =(speedOfSound*pingTime)/20000; //in cm
retourneren van afstand;
}
ongeldig vooruit() {}
digitalWrite(a,HIGH);
digitalWrite(b,LOW);
digitalWrite(c,HIGH);
digitalWrite(d,LOW);
delay(500);
Stopp();
}
ongeldig backward() {}
digitalWrite(b,HIGH);
digitalWrite(a,LOW);
digitalWrite(d,HIGH);
digitalWrite(c,LOW);
delay(1000);
Stopp();
}
VOID datumwaarde {}
digitalWrite(a,HIGH);
digitalWrite(b,LOW);
digitalWrite(d,HIGH);
digitalWrite(c,LOW);
delay(570);
Stopp();
}
VOID rechts() {}
digitalWrite(b,HIGH);
digitalWrite(a,LOW);
digitalWrite(c,HIGH);
digitalWrite(d,LOW);
delay(1000);
Stopp();
}
ongeldig stopp() {}
digitalWrite(a,LOW);
digitalWrite(b,LOW);
digitalWrite(c,LOW);
digitalWrite(d,LOW);
Serial.println("Stopp");
}
void loop {}
afstand van de maatregel
digitalWrite(trigPin,LOW);
delayMicroseconds(2000);
digitalWrite(trigPin,HIGH);
delayMicroseconds(10);
digitalWrite(trigPin,LOW);
pingTime=pulseIn(echoPin,HIGH);
afstand =(speedOfSound*pingTime)/20000; //in cm
if(Distance>20) {}
vooruit();
Serial.println ("geen obstackle");
}
Als (afstand < = 20) {}
Stopp();
Serial.println ("controleren links");
datumwaarde;
delay(250);
lDistance = meaSure();
Serial.println ("controleren recht");
rechts();
delay(250);
rDistance=meaSure();
Als (rDistance > lDistance & & rDistance > 20) {}
vooruit();
Serial.println ("vooruit in rechte pad");
}
Als (rDistance < = lDistance & & lDistance > 20) {}
rechts();
delay(250);
vooruit();
Serial.println ("vooruit in linker pad");
}
else {}
Serial.println ("In grot");
Serial.println ("terug te gaan en opnieuw te controleren");
backward();
Stopp();
datumwaarde;
delay(250);
lDistance=meaSure();
rechts();
delay(250);
rDistance=meaSure();
Als (rDistance > lDistance & & rDistance > 20) {}
vooruit();
}
Als (rDistance < = lDistance & & lDistance > 20) {}
rechts();
delay(250);
vooruit();
}
}
}
}
