Hello again. After some research and trial and error, I have working code that will move the servos forwards, backwards, and stop depending on the ping distance compared with the default thresholds. I have tried several methods but have been unable to prevent the servos from rotating in reverse when connected to the signal pins. I do not want the servos to move until the command is given through the program. I tried stopping them before every block of code that didn't cause an error and even directly after they were connected but since they have to be connected to work, no matter where i connected them, they would turn on for a couple seconds. Please help me stop them!!! Thanks.
#include <SoftwareServo.h>
#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;
}
#define DEBUG
#ifdef DEBUG
#define DEBUG_PRINT(x) Serial.println(x)
#else
#define DEBUG_PRINT(x)
#endif
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 pingPin = 8; //signal pin on parallax ping sensor connected to this arduino pin "remember the ping sensor is 5V, not source"
const int dangerThresh = 5; //threshold for obstacles (in cm), this was 10 by default
const int dangerReverseThresh = 3; //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
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
void leftMotorStop()
{
leftMotor.detach();
}
void rightMotorStop()
{
rightMotor.detach();
}
void 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 reverse() //if they are equally obstructed
{
rightMotor.attach(6); //signal wire on right countinuous rotation "drive" servo connected to this arduino pin
// rightMotorStop();
leftMotor.attach(7); //signal wire on right countinuous rotation "drive" servo connected to this arduino pin
// leftMotorStop();
leftMotor.write(LBackward); //rotate left drive servo forward
rightMotor.write(RBackward); //rotate right drive servo backward
delay(200); //run motors this many milliseconds
// rightMotorStop();
// leftMotorStop();
}
void setup()
{
rightMotor.attach(6); //signal wire on right countinuous rotation "drive" servo connected to this arduino pin
rightMotorStop(); //stops servo
leftMotor.attach(7); //signal wire on left continuous rotation "drive servo connected to this arduino pin
leftMotorStop(); //stops servo
panMotor.attach(5); //signal wire on pan servo for ping sensor connected to this arduino pin
panMotor.write(90); //set PING))) pan to center "90 by default"
Serial.begin(9600);
}
void loop()
{
{
DEBUG_PRINT("Begin Loop");
}
// convert the time into a distance
inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
{
DEBUG_PRINT("After inches and cm calculations");
}
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(dangerThresh);
Serial.print(" dangerThresh "); //displays value of dangerThresh to validate operation
panMotor.write(pos); //return to center, "90 by default"
delay(200); //wait this many milliseconds
ping(); //ping stright ahead
if (cm>dangerThresh) //if the path is clear
{
leftMotor.attach(7); //signal wire on right countinuous rotation "drive" servo connected to this arduino pin
rightMotor.attach(6); //signal wire on right countinuous rotation "drive" servo connected to this arduino pin
leftMotor.write(LForward); //move forward
rightMotor.write(RForward); //move forward
}
else //if path is blocked
{
panMotor.write(pos); //return to center, "90 by default"
delay(200); //wait this many milliseconds
ping();
chkDistanceFwd = cm; //ping stright ahead
delay(100); //wait this many milliseconds WAS 500
panMotor.write(80); //turn ping sensor right, 0 for full right
delay(300); //wait this many milliseconds WAS 500
ping();
rightDistance = cm;
delay(100); //wait this many milliseconds WAS 500
panMotor.write(100); //turn ping sensor left, 180 for full right "my servo hits stop limit and binds at 180"
delay(400); //wait this many milliseconds WAS 500
ping();
leftDistance = cm;
delay(100); //wait this many milliseconds WAS 500
panMotor.write(pos); //return to center, "90 by default" my servo is off center
delay(300); //wait this many milliseconds
compareDistance(); //compare the two distances measured
}
}
void compareDistance()
{
if (chkDistanceFwd<=dangerReverseThresh) //if they are equally obstructed
{
reverse();
}
else if (chkDistanceFwd<=dangerThresh) //if center object is <= minimum programmed distance
{
rightMotorStop();
leftMotorStop();
}
}