I would like this code to take the inputs from two stationary ping sonar sensors and steer a car to follow another car in front of it. The car has two continuous rotation servos to move the car and one pan servo to steer the car. When the code runs, the serial monitor shows the sensor values and the continuous rotation servos operate correctly. When an object is placed several feet in front of the sensors the servos stop moving and the sensors stop pinging. After a period of time, the sensors will start pinging and the servos will start turning. can someone help determine why this is happening instead of the sensors and servos operating continuously to steer thee car as desired.
#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()
{
durationLeft = 0;
durationRight = 0;
cmRight = 0;
cmLeft = 0;
inchesRight = 0;
inchesLeft = 0;
chkDistanceFwd = 0;
rightDistance = 0;
leftDistance = 0;
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);
pingLeft();
leftDistance = microsecondsToCentimeters(durationLeft);
// 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) && (leftDistance > 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) && (rightDistance > 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();
}
}