#include <Servo.h>
Servo myservo;
int triggerPin = 11;
int echoPin = 7;
int revleft4 = 2;
int fwdleft5 = 4;
int revright6 = 5;
int fwdright7 = 6;
float distance;
float duration;
float distancelt;
float distancert;
void setup() {
pinMode(revleft4, OUTPUT);
pinMode(fwdleft5, OUTPUT);
pinMode(revright6, OUTPUT);
pinMode(fwdright7, OUTPUT);
pinMode(triggerPin, OUTPUT);
pinMode(echoPin, INPUT);
Serial.begin(9600);
myservo.write(90);
myservo.attach(3);
}
void goesforward() {
Serial.println("Going Forward");
digitalWrite(fwdleft5, HIGH);
digitalWrite(fwdright7, HIGH);
}
void goesbackward() {
Serial.println("Going Backward");
digitalWrite(revleft4, HIGH);
digitalWrite(revright6, HIGH);
delay(500);
digitalWrite(revleft4, LOW);
digitalWrite(revright6, LOW);
Serial.println("Going Backward Stopped");
}
void turnright() {
Serial.println("Turning Right");
digitalWrite(fwdleft5, HIGH);
digitalWrite(revright6, HIGH);
delay(500);
Serial.println("Turning Right Stopped");
digitalWrite(fwdleft5, LOW);
digitalWrite(revright6, LOW);
}
void turnleft() {
Serial.println("Turning Left");
digitalWrite(fwdright7, HIGH);
digitalWrite(revright6, HIGH);
delay(500);
Serial.println("Turning Left Stopped");
digitalWrite(fwdright7, LOW);
digitalWrite(revright6, LOW);
}
void checkdistance() {
digitalWrite(triggerPin, 0);
delayMicroseconds(2);
digitalWrite(triggerPin, 1);
delayMicroseconds(10);
duration = pulseIn(echoPin, 1);
}
void loop() {
checkdistance();
distance = duration / 58.2;
if (distance >=15.00) {
goesforward();
}
else if (distance = 0) {
Serial.println("Please input the Ultrasonic Sound Sensor");
}
else {
myservo.write(180);
delay(400);
checkdistance();
distancelt = duration / 58.2;
delay(400);
myservo.write(90);
delay(400);
myservo.write(0);
checkdistance();
distancert = duration / 58.2;
delay(400);
myservo.write(90);
delay(250);
if (distancert >= distancelt) {
goesbackward();
turnright();
}
else {
goesbackward();
turnleft();
}
}
}
Comments
Please log in or sign up to comment.