Here is my updated code. This code currently pings each sensor every three ms while activating the continuous motion servos until either one or both sensors detects an object within the dangerThresh which is currently set at ten cm. Once the object is greater than dangerThresh, the continuous rotation servos will start up again. so now my problem seems to be that the pan servo does not turn right or left as directed by the if statements for Right and Left respectively. Thanks for your help so far.
#include <Servo.h> //include Servo library
long duration, inchesRight, inchesLeft, cm, cmRight, cmLeft, durationLeft, durationRight; //time it takes to recieve PING))) signal
long microsecondsToInches(long microseconds)
{
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}
//rightMotor stops at 1388
//leftMotor stops at 1586
const int RForward = 400; //speed value of drive servo, value 500 CW, 2500 CCW for full speed rotation
const int RBackward = 2400; //speed value of drive servo, value 500 CW, 2500 CCW for full speed rotation
const int LForward = 1733; //the servos must rotate in opposite directions in order to drive in the same direction
const int pingPinLeft = 6; //signal pin on parallax ping sensor connected to this arduino pin "remember the ping sensor is 5V, not source"
const int pingPinRight = 7;
const int dangerThresh = 10; //about 30 inches threshold for obstacles (in cm), this was 10 by default
const int dangerReverseThresh = 25; //about 10 inches threshold for obstacles (in cm), this was 10 by default
int leftDistance, rightDistance, distanceFwd, chkDistanceFwd; //distances on either side
int delayTime = 2;
int pos = 90; // variable to store the servo position
Servo panMotor; //servo that pans the ping sensor
Servo leftMotor; //continuous rotation servo that drives the left wheel
Servo rightMotor; //continuous rotation servo that drives the right wheel
void pingLeft()
{
pinMode(pingPinLeft, OUTPUT);
digitalWrite(pingPinLeft, LOW);
delayMicroseconds(2);
digitalWrite(pingPinLeft, HIGH);
delayMicroseconds(5);
digitalWrite(pingPinLeft, LOW);
pinMode(pingPinLeft, INPUT);
durationLeft = pulseIn(pingPinLeft, HIGH);
}
void pingRight()
{
pinMode(pingPinRight, OUTPUT);
digitalWrite(pingPinRight, LOW);
delayMicroseconds(2);
digitalWrite(pingPinRight, HIGH);
delayMicroseconds(5);
digitalWrite(pingPinRight, LOW);
pinMode(pingPinRight, INPUT);
durationRight = pulseIn(pingPinRight, HIGH);
}
void forward()
{
rightMotor.writeMicroseconds(RForward); //move forward
leftMotor.writeMicroseconds(LForward); //move forward
}
void bothWheelStop()
{
rightMotor.writeMicroseconds(1388);
leftMotor.writeMicroseconds(1586);
}
void turnLeft() //if left is less obstructed
{
panMotor.write(180);
delay(100); //run motors this many milliseconds
bothWheelStop();
}
void turnRight() //if right is less obstructed
{
panMotor.write(0);
delay(100); //run motors this many milliseconds
bothWheelStop();
}
void setup()
{
rightMotor.writeMicroseconds(1388);
leftMotor.writeMicroseconds(1586);
rightMotor.attach(10); //signal wire on right countinuous rotation "drive" servo connected to this arduino pin
leftMotor.attach(8); //signal wire on left continuous rotation "drive servo connected to this arduino pin
panMotor.attach(11); //signal wire on pan servo for ping sensor connected to this arduino pin
panMotor.write(pos); //set PING))) pan to center "90 by default"
Serial.begin(9600);
}
void loop()
{
pingRight();
rightDistance = microsecondsToCentimeters(durationRight);
delay(300);
pingLeft();
leftDistance = microsecondsToCentimeters(durationLeft);
delay(300);
// convert the time into a distance
inchesLeft = microsecondsToInches(durationLeft);
cmLeft = microsecondsToCentimeters(durationLeft);
inchesRight = microsecondsToInches(durationRight);
cmRight = microsecondsToCentimeters(durationRight);
Serial.print(inchesRight); //Prints to Serial Monitor
Serial.print("inRight, "); //Prints to Serial Monitor
Serial.print(cmRight); //Prints to Serial Monitor
Serial.print("cmRight"); //Prints to Serial Monitor
Serial.println(); //Prints to Serial Monitor
Serial.print(inchesLeft); //Prints to Serial Monitor
Serial.print("inLeft, "); //Prints to Serial Monitor
Serial.print(cmLeft); //Prints to Serial Monitor
Serial.print("cmLeft"); //Prints to Serial Monitor
Serial.println(); //Prints to Serial Monitor
Serial.print(" dangerThresh="); //displays value of dangerThresh to validate operation
Serial.print(dangerThresh);
Serial.print(" rightDistance=");
Serial.print (rightDistance);
Serial.print(" leftDistance=");
Serial.print (leftDistance);
if ((rightDistance > dangerThresh) && (leftDistance > dangerThresh)) //if target distance is greater than programmed default, go straight
{
forward();
}
else if ((leftDistance > rightDistance) && (rightDistance > dangerThresh)) //if target distance is greater than programmed default AND if target is closer on the RIGHT side...turn RIGHT to face target
{
turnRight();
}
else if ((rightDistance > leftDistance) && (leftDistance > dangerThresh)) //if target distance is greater than programmed default AND if target is closer on the LEFT side...turn LEFT to face target
{
turnLeft();
}
else if ((rightDistance <= dangerThresh) || (leftDistance <= dangerThresh)) //if target is <= minimum programmed distance. STOP
{
bothWheelStop();
}
}