Arduino Forum

Topics => Robotics => Topic started by: geryuu11 on Sep 16, 2014, 06:31 pm

Title: 3 servo + PING))) Sensor Obstacle Avoiding robot Issue
Post by: geryuu11 on 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:
Take reading when ping servo is looking straight ahead.
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);
pingservo.write(pingStraight);  //sets PING))) looking forward,and ready for reading
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.
   
Title: Re: 3 servo + PING))) Sensor Obstacle Avoiding robot Issue
Post by: StanD on Sep 16, 2014, 08:21 pm
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.