Ok, here is the latest code. I added the distance function and added code to output the Ping readings to the Serial Monitor so I can verify operation. The Ping sensor is providing correct distances but when the 10 cm threshold is crossed, the drive servos do not change direction but keep on driving in the same direction. It would be nice to be able to step through the code as it runs to help see the problems, but hopefully your experience can see why the Ping return values are not affecting the drive servos. Thanks!!
const int TxPin = 7;
#include <SoftwareSerial.h>
SoftwareSerial mySerial = SoftwareSerial(255, TxPin);
#include <LiquidCrystal.h>
LiquidCrystal lcd(12, 11, 10, 5, 4, 3, 2);
#include <VarSpeedServo.h> //include Servo library
long duration,distanceInches,distanceCm; //time it takes to recieve PING))) signal
long microsecondsToInches(long microseconds)
{
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}
const int RForward = 65; //speed value of drive servo, value 0 for full speed rotation
const int RBackward = 130; //speed value of drive servo, value 180 for full speed rotation
const int LForward = RBackward; //the servos must rotate in opposite directions in order to drive in the same direction
const int LBackward = RForward; //the servos must rotate in opposite directions in order to drive in the same direction
const int RNeutral = 90; //value 90 for center, my servo is a little off
const int LNeutral = 90; //value 90 for center, my servo is a little off
const int pingPin = 8; //signal pin on parallax ping sensor connected to this arduino pin "remember the ping sensor is 5V, not source"
//const int irPin = 0; //Sharp infrared sensor connected to this arduino pin "not currently used"
const int dangerThresh = 10; //threshold for obstacles (in cm), this was 10 by default
int leftDistance, rightDistance; //distances on either side
int delayTime = 2;
VarSpeedServo pingServo; //pings the servo when called
VarSpeedServo panMotor; //servo that pans the ping sensor
VarSpeedServo leftMotor; //continuous rotation servo that drives the left wheel
VarSpeedServo rightMotor; //continuous rotation servo that drives the right wheel
int pingScan() //sets the ping for static ping servo directions
{
pingServo.write(90);
delay(delayTime);
pingServo.write(180);
delay(delayTime);
pingServo.write(0);
delay(delayTime);
pingServo.write(90);
delay(delayTime);
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
distanceInches = microsecondsToInches(duration); //displays returned conversion to inches
distanceCm = microsecondsToCentimeters(duration); //displays returned conversion to centimeters
Serial.print(distanceInches);
Serial.print("in, ");
Serial.print(distanceCm);
Serial.print("cm");
Serial.println();
delay(100);
}
void setup()
{
rightMotor.attach(6); //signal wire on right countinuous rotation "drive" servo connected to this arduino pin
leftMotor.attach(5); //signal wire on left continuous rotation "drive servo connected to this arduino pin
panMotor.attach(4); //signal wire on pan servo for ping sensor connected to this arduino pin
panMotor.write(90); //set PING))) pan to center "90 by default"
lcd.begin(2, 12); // This sets the lcd to a two line, 12 char display
// initialize serial communication:
pinMode(TxPin, OUTPUT);
digitalWrite(TxPin, HIGH);
mySerial.begin(9600);
Serial.begin(9600);
}
void loop()
{
// establish variables for duration of the ping,
// and the distance result in inches and centimeters:
long duration, inches, cm;
// convert the time into a distance
inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
Serial.print(inches); //Prints to Serial Monitor
Serial.print("in, "); //Prints to Serial Monitor
Serial.print(cm); //Prints to Serial Monitor
Serial.print("cm"); //Prints to Serial Monitor
Serial.println(); //Prints to Serial Monitor
mySerial.print(" Inches "); // First line
mySerial.print(inches); // Second line
delay(100); // Wait 3 seconds is 3000
lcd.clear(); //Prints to LCD
lcd.print("Inches "); //Prints to LCD
lcd.print(inches); //Prints to LCD
lcd.setCursor(0, 1); //Prints to LCD
lcd.print("Centmtrs "); //Prints to LCD
lcd.print(cm); //Prints to LCD
delay(100);
int distanceFwd = pingScan(); //ping stright ahead
if (distanceFwd>dangerThresh) //if the path is clear
{
leftMotor.write(LForward); //move forward
rightMotor.write(RForward); //move forward
}
else //if path is blocked
{
leftMotor.write(LNeutral); //stop left drive motor
rightMotor.write(RNeutral); //stop right drive motor
panMotor.write(0); //turn ping sensor right, 0 for full right
delay(500); //wait this many milliseconds
rightDistance = pingScan(); //ping
delay(500); //wait this many milliseconds
panMotor.write(170); //turn ping sensor left, 180 for full right "my servo hits stop limit and binds at 180"
delay(500); //wait this many milliseconds
leftDistance = pingScan(); //ping
delay(500); //wait this many milliseconds
panMotor.write(83); //return to center, "90 by default" my servo is off center
delay(100); //wait this many milliseconds
compareDistance(); //compare the two distances measured
}
}
void compareDistance()
{
if (leftDistance>rightDistance) //if left is less obstructed
{
leftMotor.write(LBackward); //rotate left drive servo bacward "skid steer"
rightMotor.write(RForward); //rotate right drive servo forward "skid steer"
delay(1000); //run motors this many milliseconds
}
else if (rightDistance>leftDistance) //if right is less obstructed
{
leftMotor.write(LForward); //rotate left drive servo forward "skid steer"
rightMotor.write(RBackward); //rotate right drive servo backward "skid steer"
delay(1000); //run motors this many milliseconds
}
else //if they are equally obstructed
{
leftMotor.write(LForward); //rotate left drive servo forward
rightMotor.write(RBackward); //rotate right drive servo backward
delay(2000); //run motors this many milliseconds
}
}