Hi, I am building a robot with two continuous rotation servos, a ping))) ultrasonic sensor, and a servo for the ping))). I am having troubles with my code, namedly, stopping the servos, and for some reason, when it initially sees an obstacle, the servo (sensor) does not turn, as it feels it is constricted to time. I need both commands (stopping cont. servos, and turning sensor servo to apply to anything within the if statement.
Here is my code:
#include <Servo.h>
Servo ServoLeft;
Servo ServoRight;
Servo ServoPing;
unsigned long echo = 0;
int ultraSoundSignal = 7; // Ultrasound signal pin
unsigned long ultrasoundValue = 0;
void setup()
{
ServoLeft.attach(8);
ServoRight.attach(9);
ServoPing.attach(10);
Serial.begin(9600);
pinMode(ultraSoundSignal,OUTPUT);
}
unsigned long ping(){
pinMode(ultraSoundSignal, OUTPUT); // Switch signalpin to output
digitalWrite(ultraSoundSignal, LOW); // Send low pulse
delayMicroseconds(2); // Wait for 2 microseconds
digitalWrite(ultraSoundSignal, HIGH); // Send high pulse
delayMicroseconds(5); // Wait for 5 microseconds
digitalWrite(ultraSoundSignal, LOW); // Holdoff
pinMode(ultraSoundSignal, INPUT); // Switch signalpin to input
digitalWrite(ultraSoundSignal, HIGH); // Turn on pullup resistor
echo = pulseIn(ultraSoundSignal, HIGH); //Listen for echo
ultrasoundValue = (echo / 58.138) * .39; //convert to CM then to inches
return ultrasoundValue;
}
void loop()
{
int x = 0;
x = ping();
Serial.println(x);
delay(250); //delay 1/4 seconds.
if(ultrasoundValue < 12){ //If sees an obstacle:
ServoLeft.write(0); //Driving Servos Stop
ServoRight.write(0);
ServoPing.write(0); //Sensor turns left
if(ultrasoundValue > 12){ //If there is nothing there:
ServoPing.write(90); //Sensor Centers
ServoLeft.write(2000); //Driving Servos turn 90º left
ServoRight.write(2000);
delay(500);
}
else{ //If there is something there:
ServoPing.write(180); //Sensor turns right
if(ultrasoundValue > 12){ //If there is nothing there:
ServoPing.write(90); //Sensor centers
ServoLeft.write(1000); //Driving Servos turn 90º right
ServoRight.write(1000);
delay(500);
}
else{ //If there is still something there:
ServoPing.write(90); //Sensor centers
ServoLeft.write(1000); //Driving Servos make U turn
ServoRight.write(1000);
delay(1000);
}
}
}
else{
ServoLeft.write(1000);
ServoRight.write(2000);
}
}