Go Down

### Topic: 3 servo + PING))) Sensor Obstacle Avoiding robot Issue (Read 1 time)previous topic - next topic

#### geryuu11

##### Sep 16, 2014, 06:31 pm
I currently got all the parts to build a obstacle avoiding robot using two parallax continuous servos and a parallax standard servo,all rated at 5volts . But im having issues. First ill tell you  the logic for this robot  and how I have everything wired up and then the code.

LOGIC:
If reading is greater then threshhold(tooclose to wall), turn servos to go forward.
If reading is less then threshold(toclose to wall distance), stop.
Turn ping servo left,take a reading,then turn ping servo right, take a reading.
Compare these readings and make a judgement on whether to go left or right or turn around 180degrees.

whats happening.When I plug in my USB to upload the code the two servo wheels spin in opposite directions. And once the sensor senses something is close and takes a reading to compare distances, the servos spin in opposite directions indefinitely.

Now I know to make the servos turn clockwise its 180 and left is 0 and 90 is stop.
I calibrated the servos by testing to see if they do indeed stop when at 90,seperatly by themselves, while not hooked up to the project. And I know once you screw on the servos as wheels the turning of the servos is reversed. So to make the left wheel go forward it has to spin on 0 and for the right wheel its 180, vice versa.

I have a seperate power supply which is 4 AA batteries which have the GND on one rail and 6V on the other of my breadboard. Ive plugged in all three servos to this power rail and GND rail and have my GND pin on my Arduino hooked up as well,so everything shares the same GND.
I have my PING))) Sensor hooked up to my 5V pin on Arduino and GND hooked up to the same GND as my motors and Arduino.

My Code:

#include <Servo.h>

Servo leftservo;
Servo rightservo;// create servo object to control a servo

int leftFwd = 0;   // Values determined to go forward or backward for both wheel servos
int leftBwd = 180;
int leftStop = 90;

int rightFwd = 180;
int rightBwd = 0;
int rightStop = 90;

Servo pingservo;          //values to turn my PING in right direction for reading,its alittle off due to how i screwed it on.
int pingStraight = 75;
int pingLeft = 135;
int pingRight = 30;

const int pingpin = 7;             //PING))) variables
long duration;
long leftdistance, rightdistance;
long tooClose = 10;

void setup()
{
Serial.begin(9600);
leftservo.attach(9);
rightservo.attach(10);
pingservo.attach(11);
delay(100);
}

void loop()
{
long goingFwd = ping();   //takes reading using the ping function below
if (goingFwd > tooClose)   //makes the robot move foward
{

leftservo.write(leftFwd);
rightservo.write(rightFwd);
}
else   //if threshold is met,stop and take readings form left and right sides
{
leftservo.write(leftStop);
rightservo.write(rightStop);

pingservo.write(pingLeft);
delay(300);
leftdistance = ping();
delay(300);

pingservo.write(pingRight);
delay(300);
rightdistance = ping();
delay(300);
}

comparedistance();    //compare values generated by the ping function and compare function
Serial.print("Distance=");   //show me the numbers in serial monitor

Serial.println(duration);
delay(500);

}

long ping()
{
pinMode(pingpin, OUTPUT);
digitalWrite(pingpin, LOW);
delayMicroseconds(2);
digitalWrite(pingpin,HIGH);
delayMicroseconds(5);
digitalWrite(pingpin,LOW);

pinMode(pingpin, INPUT);
duration = pulseIn(pingpin, HIGH);
duration = duration / 74 / 2;
return duration;
}
void comparedistance()  //determines where to go based on left and right distances
{
if (leftdistance > rightdistance)
{
leftservo.write(leftBwd);
rightservo.write(rightFwd);
delay(500);
}
else if (rightdistance > leftdistance)
{
leftservo.write(leftFwd);
rightservo.write(rightBwd);
delay(500);
}
else{
leftservo.write(leftBwd);
rightservo.write(rightFwd);
delay(1000);
}
}

The readings I get are normal even when its all connected together with the servos.And the wheel servos spin normally when the PING Sensor isnt attached. I disconnected everything and left just the wheel servos and ping servo and did the Sweep Servo Sketch but modified for two continuous servos and a standard, and everything worked fine.The wheel servos turn in a forward motion(leftservo at 0=foward and rightservo at 180= forward,vice versa) and the standard servo turned to straight,left,right as well. Im just lost.Any help or links would be great.

#### StanD

#1
##### Sep 16, 2014, 08:21 pmLast Edit: Sep 16, 2014, 08:24 pm by StanD Reason: 1
Please use the code tags, when you post code. The current formatting makes it difficult to track things.

From what I can tell, the issue maybe related to this line:
comparedistance();    //compare values generated by the ping function and compare function

It seems that you are running this function at the end of each loop, regardless of whether you are close to an obstacle, or not. I think you meant to have it within the "else" clause (to run it only when you have an obstacle ahead, closer than your "tooClose" variable). Hope that helps.
Arduino experiments, resources and hacks http://42bots.com

Go Up