Ultrasonic navigation (should be simple)

So i finally got my redbot to use an ultrasonic sensor for navigation but i am having an issue… it runs into the wall anyway. what is happening is that the ultrasonic sensor doesn’t pick up the obstruction until after it has hit it. then it will look left and right to find the best scenario.

Not quite sure what i am doing wrong here… not enough samples or delays causing the issue?? Baud rate?

/***********************************************************************

Right and Left motor encoder inputs must be connected to:
=========================================================
3 - Right wheel encoder
A2 - Left wheel encoder

If your connection is to different pins, simply alter the RedBotEncoder
pins to match the pins that your encoder is connected to.

Servo connection:
=================
A0 - white wire
5V - red wire
GND - black wire

HC-SR04 Ultrasonic connection:
=============================
RedBot  HC-SR04
A4      Trig
A5      Echo
5V      VCC
GND     GND

***********************************************************************/
// Include the libraries. We make a provision for using the Xbee header
//  via software serial to report values, but that's not really used in
//  the code anywhere.
#include <RedBot.h>
RedBotMotors motors;
#include <Servo.h> 
#include <Ultrasonic.h>

// Pin connections for the HC-SR04 module
#define trigPin A4
#define echoPin A5

Ultrasonic ultrasonic(A4,A5);

const int RForward = 0;
const int RBackward = 180;
const int LForward = 180;
const int LBackward = 0;
const int RNeutral = 91;
const int LNeutral = 92; //constants for motor speed

const int TrigPin = 12;
const int EchoPin = 13;  //Echo pin

const int dangerThresh = 10; //threshold for obstacles (in cm)
int leftDistance = 0;
int rightDistance = 0; //distances on either side


Servo HeadServo;

long duration; // for HC-SR04

void setup()
{
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);  
  HeadServo.attach(A0); //attach servo to proper pins
  HeadServo.write(90); //centre head servo
  delay(1000);  
}


void loop()
{
int distanceFwd = (ultrasonic.Ranging(CM));

Serial.begin( 9600 );
Serial.print( ultrasonic.Ranging(CM) );
Serial.println( "cm" );
delay(500);
  
  if (distanceFwd>dangerThresh) //if path is clear
      {
       motors.leftMotor(150);  // spin CCW
       motors.rightMotor(150); // spin CCW
      }

   else //if path is blocked
      {
      motors.leftMotor(0);  // spin CCW
      motors.rightMotor(0); // spin CCW
      HeadServo.write(0);
      delay(500);
      rightDistance = ultrasonic.Ranging(CM); //scan to the right
      delay(500);
      HeadServo.write(180);
      delay(700);
      leftDistance = ultrasonic.Ranging(CM); //scan to the left
      delay(500);
      HeadServo.write(90); //return to center
      delay(100);

      compareDistance();
      }

}

void compareDistance()
 {
    if (leftDistance>rightDistance) //if left is less obstructed
      {
      turnLeft();
      delay(2000);
      }
    else if (rightDistance>leftDistance) //if right is less obstructed
      {
      turnRight();
      delay(2000);
      }
    else //if they are equally obstructed
      {
      turn180();
      delay(2000);
      }
  }

long ping()
{
// Send out PING))) signal pulse
pinMode(TrigPin, OUTPUT);
digitalWrite(TrigPin, LOW);
delayMicroseconds(2);
digitalWrite(TrigPin, HIGH);
delayMicroseconds(5);
digitalWrite(TrigPin, LOW);

//Get duration it takes to receive echo
pinMode(TrigPin, INPUT);
duration = pulseIn(TrigPin, HIGH);

//Convert duration into distance
return duration / 29 / 2;
}

// reverse() function -- backs up at full power
void reverse()
{
  motors.drive(-255);
  delay(500);
  motors.brake();
  delay(100);  // short delay to let robot fully stop
}

// turnRight() function -- turns RedBot to the Right
void turnRight()
{
  motors.leftMotor(+150);  // spin CCW
  motors.rightMotor(-150); // spin CCW
  delay(500);
  motors.brake();
  delay(100);  // short delay to let robot fully stop
}

// turnRight() function -- turns RedBot to the Left
void turnLeft()
{
  motors.leftMotor(-150);  // spin CW
  motors.rightMotor(+150); // spin CW
  delay(500);
  motors.brake();
  delay(100);  // short delay to let robot fully stop
}
void turn180()
{
  motors.leftMotor(+150);  // spin CW
  motors.rightMotor(-150); // spin CW
  delay(500);
  motors.brake();
  delay(100);  // short delay to let robot fully stop
}

Take this out of loop() and put it in setup():

Serial.begin( 9600 );

How do you know that the ultrasonic sensor is actually working properly? Put in some more print statements at various places to see what is happening.

const int dangerThresh = 10; //threshold for obstacles (in cm)

10 cm is not a very great distance. How fast is the bot travelling ? Are you applying hard braking by stopping the wheels or soft breaking by letting the wheels coast ? What happens when you increase the value of dangerThresh ?

i added a brake, doubled the threshold and removed the delay after the serial print. it reacts much quicker now, the only issue i am having is sometimes it will just stop in the middle of the room and change direction for no reason. Additionally, sometimes, after driving for awhile and changing directions multiple times, it will just keep going and the ultrasonic wont pickup anything.

The ONLY delay in your program should be the one between triggering the US sensor and checking for the echo, ANY OTHERS WILL SCREW UP YOUR PROGRAM!

Mark

after driving for awhile and changing directions multiple times, it will just keep going and the ultrasonic wont pickup anything.

Ultrasonic sensors are not completely reliable. Welcome to the real world!

As has been noted, ultrasonic sensors are not completely reliable and will sometimes provide spurious readings. You could consider taking further readings to confirm them before taking action.