byte la=2,lb=3,ra=5,rb=4,trig=6,echo=7,ls=8,rs=9;
float duration=1,distance;
float d;
void setup() {
pinMode(la,OUTPUT);
pinMode(lb,OUTPUT);
pinMode(ra,OUTPUT);
pinMode(rb,OUTPUT);
pinMode(trig,OUTPUT);
pinMode(echo,INPUT);
pinMode(ls,INPUT);
pinMode(rs,INPUT);
Serial.begin(9600);
}
float dis(){
digitalWrite(trig,LOW);
delayMicroseconds(2);
digitalWrite(trig,HIGH);
delayMicroseconds(10);
digitalWrite(trig,LOW);
duration=pulseIn(echo,HIGH);
distance=duration*(0.034/2);
Serial.print("distance =");
Serial.println(distance);
return distance;
delay(1000);
}
void drive(){
digitalWrite(ra,1);
digitalWrite(la,1);
digitalWrite(lb,0);
digitalWrite(rb,0);
}
void stoper(){
digitalWrite(ra,0);
digitalWrite(la,0);
digitalWrite(lb,0);
digitalWrite(rb,0);
}
void reverse(){
digitalWrite(ra,0);
digitalWrite(la,0);
digitalWrite(lb,1);
digitalWrite(rb,1);
}
void right(){
digitalWrite(ra,0);
digitalWrite(la,1);
digitalWrite(lb,0);
digitalWrite(rb,1);
}
void left(){
digitalWrite(ra,1);
digitalWrite(la,0);
digitalWrite(lb,1);
digitalWrite(rb,0);
}
void loop() {
d=dis();
drive();
if(d<=15)
{
if(digitalRead(ls)==1 && digitalRead(rs)==0)
{
stoper();
delay(1000);
reverse();
delay(1000);
right();
delay(500);
stoper();
}
else
if(digitalRead(rs)==1 && digitalRead(ls)==0)
{
stoper();
delay(1000);
reverse();
delay(1000);
left();
delay(500);
stoper();
}
else
{
stoper();
delay(1000);
reverse();
delay(1000);
right();
delay(2000);
}
}
else
if(digitalRead(ls)==1 || digitalRead(rs) ==1)
{
if(digitalRead(ls)==1)
{
stoper();
delay(1000);
reverse();
delay(1000);
right();
delay(500);
}
else
{
stoper();
delay(1000);
reverse();
delay(1000);
left();
delay(500);
}
}
}
Comments
Please log in or sign up to comment.