Arduino Robot with Ping Ultrasonic Programming

[vgtsec]

I am fairly new to Arduino, but I know my way around. The sad thing is I have an EE degree, but haven't used it in 13 years, so it's basically good as gone. I also know a little C, C++, but I am way more proficient in scripting languages like BASH, or Perl. Here is my issue:
I am building a self navigating robot to avoid objects, walls, etc. I am using 2 HS-325HB servos for the wheels, and Parallax (Futaba) Standard Servo to rotate the Ping Ultrasonic. Everything works like it should except the robot runs into every freaking wall it comes into contact with. The Ping Ultrasonic seems to be working; I can serial monitor from Arduino's IDE and accurate measurements. Obviously my problem is in my code. My idea behind the programming was to: 1) Measure distance. 2) If obstacle is less than or equal to 10 inches, move back, delay, turn right, delay, and scan from right to left taking more measurements. 3) If not within 10 inches just move forward.

I'm missing something here, any help would be greatly appreciated, thanks

Code: Select all

//Top head servo = PIN 7 -Yellow sig wire
//Right (looking at back) servo - pin 8
//Left (looking at back) servo - pin 9
//Ping Ultrasonic - pin 1

#include <Servo.h>

Servo pingServo; 

int servoRightPin = 8;
int servoLeftPin = 9;
const int pingPin = 10;
long duration, distanceInches, distanceCm;

Servo servoRight;
Servo servoLeft;
int delayTime = 1000;

// ################# MOVE FUNCTIONS #################

void moveForward()
{
  servoLeft.write(180);
  servoRight.write(0);
}

void moveBack()
{
  servoLeft.write(0);
  servoRight.write(180);
}

void turnRight()
{
  servoLeft.write(180);
  servoRight.write(180);
}

void turnLeft()
{  
  servoLeft.write(0);
  servoRight.write(0);
}

void stopMoving()
{
  servoLeft.write(90);
  servoRight.write(90);
}

void pingScan()
{
  
    pingServo.write(90);
  delay(delayTime);
  
  pingServo.write(180);
  delay(delayTime);
  
  pingServo.write(0);
  delay(delayTime);
   
  pingServo.write(90);
  delay(delayTime);
  
  
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  
  distanceInches = microsecondsToInches(duration);
  distanceCm = microsecondsToCentimeters(duration);
  Serial.print(distanceInches);
  Serial.print("in, ");
  Serial.print(distanceCm);
  Serial.print("cm");
  Serial.println();
  delay(100); 
  
  
  
}

// ################# SETUP #################

void setup()
{
  pingServo.attach(7);
  servoRight.attach(servoRightPin);
  servoLeft.attach(servoLeftPin);
  Serial.begin(9600);
}



// ################# LOOP CODE #################
void loop()
{
 
   pingServo.write(90);
  delay(delayTime);
  
  pingServo.write(180);
  delay(delayTime);
  
  pingServo.write(0);
  delay(delayTime);
   
  pingServo.write(90);
  delay(delayTime);
  
  
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  
  distanceInches = microsecondsToInches(duration);
  distanceCm = microsecondsToCentimeters(duration);
  Serial.print(distanceInches);
  Serial.print("in, ");
  Serial.print(distanceCm);
  Serial.print("cm");
  Serial.println();
  delay(100);

  if (distanceInches <= 10)  
{
   moveBack();
  delay(1000);
  stopMoving();
  delay(1000); 
  turnRight();
  delay(1000);
  pingScan();
}

  else
{
  moveForward();
 delay(3000); 
}
 delay(300);

 stopMoving();
 delay(delayTime);
 pingScan();
 delay(300);
 if (distanceInches <= 10)  
{
   moveBack();
  delay(1000);
  stopMoving();
  delay(1000); 
  turnRight();
  delay(1000);
  pingScan();
}

  else
{
  moveForward();
 delay(3000); 
}
 delay(300);
 
}

// ################# ULTRASONICE MATH CONVERSIONS #################

long microsecondsToInches(long microseconds)
{
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}

[adafruit_support_bill]

Way too many delays!

I haven't tried to analyze all the code in detail, but you end the loop with:

Code: Select all

  moveForward();
delay(3000);
}
delay(300);

}

That means you move forward for more than three seconds before starting the loop again. Then you start the loop with:

Code: Select all

   pingServo.write(90);
  delay(delayTime);
 
  pingServo.write(180);
  delay(delayTime);
 
  pingServo.write(0);
  delay(delayTime);
   
  pingServo.write(90);
  delay(delayTime);
  

That gives you at least another 4 seconds (remember you are still moving forward!) before you even check the ping sensor. That is seven seconds of travel without checking the sensor. I'd get rid of as many delays as you can. The basic idea should be to test your sensor as frequently as possible and adjust course as soon as you detect something.

[philba]

I can't help thinking that you need to learn about interrupts. You've got 2 tasks going on that need to be "concurrent". obstacle sensor management and motor control. When I built sumobots, my sensor code was all driven off of a timer tick and it dropped the data into shared variables. The motor control was in the main loop and used this information to direct the robot direction (which was to aim at the other guy while avoiding the white line on the edge). You've got your control loop doing both and it's hard to keep the priorities straight. The simplicity of just doing motor control in the main loop will become apparent once you do it.

[vgtsec]

Great, thanks a lot for the replies. I will work on it tonight. Thanks again.

[vgtsec]

Thanks for the help, I did get that problem fixed by using nested if statements ... but I've ran into another problem that I've been stuck on for 2 days now. For some reason, one of my servos ( Hitec HS-325HB (modded for continuous rotation)) does nothing but full speed forward. I originally assumed I didn't center it correctly when I modded it for the 360 rotation; I ran the sketch below to find the stopping point but as I watch it via serial monitor the wheel continues one speed, one direction. Any ideas on what could be causing this? Any help is appreciated, thanks in advance.

Code: Select all

#include <Servo.h>

Servo myServo;
int servoPin = 9;

void setup()
{
  myServo.attach(servoPin);
  Serial.begin(115200);
}


void loop()
{
  for (int i = 0; i <= 180; i = i + 1)
  {
    myServo.write(i);
    Serial.println(i);
    delay(150);
  }
  
}

[adafruit_support_bill]

Nothing wrong with the code. It is most likely a problem with your servo modification. Do you have another servo to try?

[vgtsec]

Nothing wrong with the code. It is most likely a problem with your servo modification. Do you have another servo to try?

Well I bought two new fresh servos yesterday, was very careful when I did the mod, and when I tested them they seemed to work. Tonight I put them on the robot and they are doing the same freaking thing, just one way continuous rotation when I run the "FOR code" I listed above.

I am stumped on this, I guess it has to be my arduino, but I just bought this one about a week ago. This is pretty frustrating so if anyone has ever encountered this please let me know.

Thanks

*** Update ***

Never mind on that, I got it - didn't have my 2 grounds connected from each of my power supplies. Thanks for the help.