Sonar shows false readings

Hi,

I’m trying to do something basic with my sonar (HC-SR04) - just testing code at this point.

By default, I want my robot to go forward, unless it comes near an object.

When the sonar reading drops below a preset threshold value, I want to the robot to stop, which indicates that an object is in front of it.

This part I have working. The problem is that when no object is within the defined range (the sonar has to have some max range setting), the sonar register’s a 0.00 value. I cannot seem to exclude this value in the ‘if’ statement. It seems to “studder” when no object is detected at all, as if the robot cannot truly decide what the true sonar reading is.

However, if there is an object in range (e.g. 20 inches away), it will run just fine, and will stop when the sonar picks up the object below the threshold of 5 inches as it moves closer to it.

Again, it’s the problem of when there is no object picked up by the sonar, a “false” reading of 0.00 is given, and I cannot seem to exclude this. I even tried doing:

if (distance > threshold || distance < 1.00)

and that doesn’t work.

Any ideas how to filter for the ‘false object’ issue?

/*****

* Notes about this code:
   - This code is on the Leonard board.  Therefore, use "Serial1."
       when wiring code, as opposed to the UNO which uses "Serial."

* This code allows the robot to move autonomously by:
   >> using sonars to detect obstacles (it moves forward if no obstacle detected)
   >> It turns by turning in place (wheels on opposite sides spin in opposite directions)

* Pins Used by:

   >> Motor Shield - (digital: 3,5,6,11)  // defined in library source code, 'AFMotor.cpp'

*****/

#include <AFMotor.h>  // AF Motor shield library
#include <NewPing.h>  // sonar library

// Sonar setup
#define TRIGGER_PIN  12  // Arduino trigger pin on sonar (non PWM)
#define ECHO_PIN     10  // Arduino echo pin on sonar    (PWM)
#define MAX_DISTANCE 200 /* Maximum distance we want to ping for (in centimeters). 
                            Maximum sensor distance is rated at 400-500cm.
                         */
                         
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

double threshold = 1.00;

// Motor setup
const int speed = 100; // percent of maximum speed

int pwm;

int incomingByte;      // a variable to read incoming serial data into

bool value = false; // set variable once for RC signal received


// define wheels (individual)

  // front wheels
  AF_DCMotor Motor_Left_Front(4, MOTOR34_1KHZ); // motor 4
  AF_DCMotor Motor_Right_Front(3, MOTOR34_1KHZ); // motor 3

  // rear wheels
  AF_DCMotor Motor_Left_Rear(1, MOTOR12_64KHZ); // motor 1
  AF_DCMotor Motor_Right_Rear(2, MOTOR12_64KHZ); // motor 2


//*****MOTOR FUNCTIONS  
      
  // Forward, Backward, Release
  
void Drive_State(int DRIVE_STATE)
{
  
  if (DRIVE_STATE == FORWARD)
    {Serial1.println("FORWARD");}
    
    else if (DRIVE_STATE == BACKWARD)
      {Serial1.println("BACKWARD");}

    else  
      {Serial1.println("RELEASE");}

  
      // front wheels
      Motor_Left_Front.run(DRIVE_STATE);
      Motor_Right_Front.run(DRIVE_STATE);
      
      // rear wheels
      Motor_Left_Rear.run(DRIVE_STATE);
      Motor_Right_Rear.run(DRIVE_STATE);
  
      delay(10);
}

// TURN LEFT
void Left_Turn(void)
{
    Serial1.println("LEFT TURN");
    
    // front wheels
    Motor_Left_Front.run(BACKWARD);
    Motor_Right_Front.run(FORWARD);
    
    // rear wheels
    Motor_Left_Rear.run(BACKWARD);
    Motor_Right_Rear.run(FORWARD);
    
    delay(10);    
}

// TURN RIGHT
void Right_Turn(void)
{
    Serial1.println("RIGHT TURN");
    
    // front wheels
    Motor_Left_Front.run(FORWARD);
    Motor_Right_Front.run(BACKWARD);
    
    // rear wheels
    Motor_Left_Rear.run(FORWARD);
    Motor_Right_Rear.run(BACKWARD);
    
    delay(10);   
}



void setup() 
{
  Serial1.begin(9600);           // set up Serial library at 9600 bps



// scale percent into pwm range
pwm = map(speed, 0, 100, 0, 255);  // takes range of 0-255 and maps it to 0-100 for variable 'speed'
                                   // actual pwm value is define as const at beginning of code
                                   
// front wheels
Motor_Left_Front.setSpeed(pwm);
Motor_Right_Front.setSpeed(pwm);

// rear wheels
Motor_Left_Rear.setSpeed(pwm);
Motor_Right_Rear.setSpeed(pwm);


}

void loop() 
{

  /********** sonar reading - beginning **********/
  
 delay(50);    // Wait 50ms between pings (~ 20 pings/sec). 29ms should be min delay between pings
  
   // initially, these types were 'unsigned int', but seemed to be no difference
   int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS)
   double distance = (uS / US_ROUNDTRIP_CM) / 2.54;  // Convert ping time to distance, 0 = outside set distance range
                                                    // units are converted from cm to inches
 
  Serial.print("Distance: ");
  Serial.print(distance);      // units are in inches
  Serial.println("in");
  
  if (distance > threshold || distance < 1.00)
  {
     Drive_State(FORWARD);    // forward
     
  }
  
  else
  {
       Drive_State(RELEASE);    // stop 
  }

}

I don’t see anything obviously wrong with your if statement – although it is good practice use extra parentheses to make the order of comparison clear, e.g.

if ( (distance > threshold) || (distance < 1.00) )

I do note that “threshold” is equal to 1.00, so in fact the if statement is true unless distance is exactly equal to 1.00; probably not what you intended.

That difficulty aside, your loop() executes about every 50 ms. If the sensor returns an out of range value then the response will be overridden 50 ms later by a return value in range. This could cause the stuttering you mention.

It is widely known that the HC-SR04 sensors return false echos, and you have to plan for it. For the moment, I suggest to ditch all the motor control, figure out how the sensor responds in a variety of situations and then write your program to react appropriately.

As an aside, there is no reason to use floating point variables. Integers will work just fine and execution will be much faster.

Just to get the idea, can you paste the serial output of when there is something, and when there is nothing.

I pasted a libraryless code that manage the sr04 a few days ago, when there is nothing, it output nothing, would that help?

https://github.com/fredericplante/HC-SR04