Ok...latest code. I added some statements so I can watch the serial monitor for changes in the variables. Currently, the rightMotor and leftMotor spin continuously even when the ping results show that the chkDistanceFwd<=dangerThresh is crossed (item too close to ping sensor). Here is a snippet of the serial monitor display when moving an item slowly to the Ping sensor:
1 chkDistanceFwd 10 dangerThresh 4in, 11cm
1 chkDistanceFwd 10 dangerThresh 4in, 11cm
1 chkDistanceFwd 10 dangerThresh 4in, 11cm
1 chkDistanceFwd 10 dangerThresh 4in, 11cm
1 chkDistanceFwd 10 dangerThresh 4in, 11cm
1 chkDistanceFwd 10 dangerThresh 4in, 11cm
1 chkDistanceFwd 10 dangerThresh 4in, 11cm
1 chkDistanceFwd 10 dangerThresh 4in, 11cm
1 chkDistanceFwd 10 dangerThresh 4in, 10cm
1 chkDistanceFwd 10 dangerThresh 0in, 1cm
10 chkDistanceFwd 10 dangerThresh 0in, 1cm
1 chkDistanceFwd 10 dangerThresh 0in, 2cm
1 chkDistanceFwd 10 dangerThresh 0in, 1cm
2 chkDistanceFwd 10 dangerThresh 0in, 1cm
1 chkDistanceFwd 10 dangerThresh 0in, 1cm
1 chkDistanceFwd 10 dangerThresh 0in, 1cm
1 chkDistanceFwd 10 dangerThresh 0in, 1cm
1 chkDistanceFwd 10 dangerThresh 0in, 1cm
1 chkDistanceFwd 10 dangerThresh 0in, 1cm
1 chkDistanceFwd 10 dangerThresh 0in, 1cm
1 chkDistanceFwd 10 dangerThresh 0in, 1cm
I think for this to work, chkDistanceFwd needs to cm (which has been coded) after the ping() function has been initiated. This should send s ping, convert the return data to inches and cm, then assign the value of cm to chkDistanceFwd, so they should be the same value in the serial monitor but they are not. Additionally, once the 10cm threshold is crossed (at which time chkDistanceFwd and dangerThresh are equal, which would seem to indicate that the chkDistanceFwd = cm statement is working) there is a drop in the inches and cm data on the serial monitor even though the item being pinged is still 9 or 10 cm away. I tried playing with the delay thinking a longer time would allow for the processing to complete but it did not have the desired effect. Any help is greatly appreciated!!!
//const int TxPin = 2;
//#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, inches, cm; //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, distanceFwd, chkDistanceFwd; //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 ping() //sets up ping when called
{
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
}
void leftMotorStop()
{
digitalWrite(7, LOW);
}
void rightMotorStop()
{
digitalWrite(6, LOW);
}
void setup() //SERVO MOTORS SHOULD NOT TURN ON UNTIL PING RETURNS TELL THEM TO!!!
{
rightMotorStop(); //stops servo
rightMotor.attach(6); //signal wire on right countinuous rotation "drive" servo connected to this arduino pin
rightMotorStop(); //stops servo
leftMotorStop(); //stops servo
leftMotor.attach(7); //signal wire on left continuous rotation "drive servo connected to this arduino pin
leftMotorStop(); //stops servo
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()
{
// 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
Serial.print(chkDistanceFwd);
Serial.print(" chkDistanceFwd "); //displays value of chkDistanceFwd to validate operation
// Serial.print(cm);
// Serial.print(" rightDistance "); //displays value of rightDistance to validate operation
// Serial.print(cm);
// Serial.print(" leftDistance "); //displays value of leftDistance to validate operation
Serial.print(dangerThresh);
Serial.print(" dangerThresh "); //displays value of dangerThresh to validate operation
// 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);
//distanceFwd = ping(); //ping stright ahead
ping(); //ping stright ahead
if (cm>dangerThresh) //if the path is clear
{
leftMotor.write(LForward); //move forward
rightMotor.write(RForward); //move forward
}
else //if path is blocked
{
rightMotorStop(); //stops servo
leftMotorStop(); //stops servo
// leftMotor.write(LNeutral); //stop left drive motor
// rightMotor.write(RNeutral); //stop right drive motor
ping();
chkDistanceFwd = cm; //ping stright ahead
delay(200); //wait this many milliseconds WAS 500
// panMotor.write(0); //turn ping sensor right, 0 for full right
// delay(500); //wait this many milliseconds WAS 500
// rightDistance = ping(); //ping
// delay(500); //wait this many milliseconds WAS 500
// 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 WAS 500
// leftDistance = ping(); //ping
// delay(500); //wait this many milliseconds WAS 500
// 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 (chkDistanceFwd<=dangerThresh) //if center object is <= minimum programmed distance
{
rightMotorStop(); //stops servo NOT WORKING
leftMotorStop(); //stops servo NOT WORKING
// rightMotor.attach(12); //turns off servo by attaching to unused pin
// leftMotor.attach(11); //turns off servo by attaching to unused pin
// delay(1000); //run motors this many milliseconds
}
else 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
}
}