#include <Servo.h> //include Servo library
const int RForward = 0;
const int RBackward = 180;
const int LForward = RBackward;
const int LBackward = RForward;
const int RNeutral = 90;
const int LNeutral = 90; //constants for motor speed
const int trigPin = 12;
const int echoPin = 13; //Sharp infrared sensor pin
const int dangerThresh = 10; //threshold for obstacles (in cm)
int leftDistance, rightDistance; //distances on either side
Servo panMotor;
Servo leftMotor;
Servo rightMotor; //declare motors
long duration; //time it takes to recieve PING))) signal
void setup()
{
rightMotor.attach(10);
leftMotor.attach(9);
panMotor.attach(6); //attach motors to proper pins
panMotor.write(90); //SENSOR TO CENTER
pinMode(trigPin, OUTPUT);// set the trig pin to output (Send sound waves)
pinMode(echoPin, INPUT);// set the echo pin to input (recieve sound waves)
}
void loop()
{
int distanceFwd = distance();
if (distanceFwd>dangerThresh) //if path is clear
{
leftMotor.write(LForward);
rightMotor.write(RForward); //move forward
}
else //if path is blocked
{
leftMotor.write(LNeutral);
rightMotor.write(RNeutral);
panMotor.write(0);
delay(500);
rightDistance = distance(); //scan to the right
delay(500);
panMotor.write(180);
delay(500);
leftDistance = distance(); //scan to the left
delay(500);
panMotor.write(90); //return to center
delay(100);
compareDistance();
}
}
void compareDistance()
{
if (leftDistance>rightDistance) //if left is less obstructed
{
leftMotor.write(LBackward);
rightMotor.write(RForward); //turn left
delay(500);
}
else if (rightDistance>leftDistance) //if right is less obstructed
{
leftMotor.write(LForward);
rightMotor.write(RBackward); //turn right
delay(500);
}
else //if they are equally obstructed
{
leftMotor.write(180);
rightMotor.write(180); //turn 180 degrees
delay(1000);
}
}
long distance()
{
// Send out PING))) signal pulse
long duration, distance; // start the scan
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);;
return duration / 29 / 2;
}
It appears that you forgot to type your question.
Your problem is that pulseIn() will return 0 if no echo is received. This will look like an obstruction at distance 0 when it is actually 'no obstruction sensed'.
It is also unlikely that turning for 500 milliseconds will do a 90° turn and turning for 1000 milliseconds will do a 180° turn but those can be tuned.