I wanted my robot to have a ping sensor that moves from left to right.
The pseudo code would be:
check for obtacles in front
if there is nothing near in front move forward
if there is something near in front, then stop.
after stopping turn the ping left and listen for obstacles then turn the ping right and listen
return the ping to center
If the left value was larger than right, turn right
if the right value was larger than left, turn left
if the right value equals the left value, reverse
repeat this over and over again
well this is what i want it to do.
the code i have now (works great for turning one direction) is:
#include <Servo.h>
Servo ServoLeft;
Servo ServoRight;
int ledPin1 = 1;
int ledPin3 = 3;
unsigned long echo = 0;
int ultraSoundSignal = 7; // Ultrasound signal pin
unsigned long ultrasoundValue = 0;
void setup()
{
ServoLeft.attach(8);
ServoRight.attach(9);
pinMode(ledPin1, OUTPUT);
pinMode(ledPin3, OUTPUT);
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 straight() {
ServoLeft.write(0); //0 is bak
ServoRight.write(180); //180 is bak
}
void turnRight() // turn right function
{ServoLeft.write(0); //0 is bak
ServoRight.write(0); //180 is bak
}
void avoideWalls()
{
if(ultrasoundValue < 18) // if the distance is less than ___
{
digitalWrite(ledPin3, HIGH);
digitalWrite(ledPin1,LOW);
turnRight(); // turns Walbot right using turnRight function
delay(100);
}
else // otherwise
{
digitalWrite(ledPin3, LOW);
digitalWrite(ledPin1, HIGH);
straight(); // go straight using the straight function
}
}
void loop() {
ping();
avoideWalls();
delay(20);
}
So can someone help me with making that happen? 