HC-SR04 Robot Control Problems

prokiller1800 · March 29, 2016, 2:08pm

Hi!
Im making a robot that is controlled by 4 HC-SRO4 modules. The problem is that my code is not running how it should, because he only detects close objects and starts going back with the uS1 declared sensor and if all are above 70 cm, he justs goes front to the right.

int uS1 = SonarFrontLeft.ping();
int uS2 = SonarFrontRight.ping();
int uS3 = SonarLeft.ping();
int uS4 = SonarRight.ping();

   if (uS1 / US_ROUNDTRIP_CM < 35 || uS2 / US_ROUNDTRIP_CM < 35 || uS3 / US_ROUNDTRIP_CM < 35 || uS4 / US_ROUNDTRIP_CM < 35){
   digitalWrite(IN1,HIGH);
   digitalWrite(IN2,HIGH);
   digitalWrite(IN3,LOW);
   digitalWrite(IN4,HIGH);
  }
    if (uS1 / US_ROUNDTRIP_CM > 70 && uS2 / US_ROUNDTRIP_CM > 70 && uS3 / US_ROUNDTRIP_CM > 70 && uS4 / US_ROUNDTRIP_CM > 70){ 
    digitalWrite(IN1,LOW);
    digitalWrite(IN2,HIGH);
    digitalWrite(IN3,HIGH);
    digitalWrite(IN4,HIGH);
    }
    if ((uS1 / US_ROUNDTRIP_CM <70 && uS1 / US_ROUNDTRIP_CM > 35) || (uS3 / US_ROUNDTRIP_CM >70 && uS3 / US_ROUNDTRIP_CM >35)){
    digitalWrite(IN1,LOW);
    digitalWrite(IN2,LOW);
    digitalWrite(IN3,HIGH);
    digitalWrite(IN4,HIGH);
}
    if ((uS2 / US_ROUNDTRIP_CM <70 && uS2 / US_ROUNDTRIP_CM >35) || (uS4 / US_ROUNDTRIP_CM >70 && uS4 / US_ROUNDTRIP_CM >35)){
    digitalWrite(IN1,LOW);
    digitalWrite(IN2,HIGH);
    digitalWrite(IN3,HIGH);
    digitalWrite(IN4,LOW);  
}
if ((uS1 / US_ROUNDTRIP_CM <70 && uS1 / US_ROUNDTRIP_CM >35) && (uS2 / US_ROUNDTRIP_CM <70 && uS2 / US_ROUNDTRIP_CM >35) && (uS3 / US_ROUNDTRIP_CM >70 && uS3 / US_ROUNDTRIP_CM >35) && (uS4 / US_ROUNDTRIP_CM >70 && uS4 / US_ROUNDTRIP_CM >35)){
    
    if ((uS2 / US_ROUNDTRIP_CM > uS1 / US_ROUNDTRIP_CM) || (uS4 / US_ROUNDTRIP_CM > uS3 / US_ROUNDTRIP_CM )){
    digitalWrite(IN1,LOW);
    digitalWrite(IN2,HIGH);
    digitalWrite(IN3,HIGH);
    digitalWrite(IN4,LOW);  
}
    if ((uS2 / US_ROUNDTRIP_CM < uS1 / US_ROUNDTRIP_CM) || (uS4 / US_ROUNDTRIP_CM < uS3 / US_ROUNDTRIP_CM )){
    digitalWrite(IN1,LOW);
    digitalWrite(IN2,LOW);
    digitalWrite(IN3,HIGH);
    digitalWrite(IN4,HIGH);
}
    if ((uS2 / US_ROUNDTRIP_CM == uS1 / US_ROUNDTRIP_CM) && (uS4 / US_ROUNDTRIP_CM == uS3 / US_ROUNDTRIP_CM )){
    digitalWrite(IN1,LOW);
    digitalWrite(IN2,LOW);
    digitalWrite(IN3,HIGH);
    digitalWrite(IN4,HIGH);
}

johnwasser · March 29, 2016, 2:21pm

Because you just have a series of 'if' statements, the last one to evaluate as 'true' will be the one that controls your robot. If you want the FIRST one that evaluates as 'true' to control your robot you should put 'else' between each 'if'.

You can make your code more readable by saving the distances in cm rather than microseconds:

int cm1 = SonarFrontLeft.ping() / US_ROUNDTRIP_CM;
int cm2 = SonarFrontRight.ping() / US_ROUNDTRIP_CM;
int cm3 = SonarLeft.ping() / US_ROUNDTRIP_CM;
int cm4 = SonarRight.ping() / US_ROUNDTRIP_CM;

That way you don't have to do the conversion every time you want to compare.

What does .ping() return when there is no object in range? If it returns 0 and you treat that as "OBJECT VERY CLOSE" that might be causing your problem.

thelittlesthobo · March 29, 2016, 4:36pm

Hello

I'm using the same for a robot project I am currently doing, however I am using a different header file than ping() called newping() which also declares max distances etc and I have had no problems with it (attached).
The following short code I used when testing all the sensors after connecting them to the board.

#include <NewPing.h>

#define FUS  TKD3  // Arduino pin tied to both trigger and echo pins on the ultrasonic sensor.
#define LHUS  TKD1  // Arduino pin tied to both trigger and echo pins on the ultrasonic sensor.
#define RHUS  TKD4  // Arduino pin tied to both trigger and echo pins on the ultrasonic sensor.
#define LRUS  TKD2  // Arduino pin tied to both trigger and echo pins on the ultrasonic sensor.
#define RRUS  TKD0  // Arduino pin tied to both trigger and echo pins on the ultrasonic sensor.
#define MAX_DISTANCE 200

NewPing sonar1(FUS, FUS, MAX_DISTANCE); // NewPing setup of pin and maximum distance.
NewPing sonar2(LHUS, LHUS, MAX_DISTANCE); // NewPing setup of pin and maximum distance.
NewPing sonar3(RHUS, RHUS, MAX_DISTANCE); // NewPing setup of pin and maximum distance.
NewPing sonar4(LRUS, LRUS, MAX_DISTANCE); // NewPing setup of pin and maximum distance.
NewPing sonar5(RRUS, RRUS, MAX_DISTANCE); // NewPing setup of pin and maximum distance.

void setup() {
  Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.
}

void loop() {
  //#####################################################################################################################################
  //Assigns the calculated distance from each sensor ping to the corresponding variable
  //#####################################################################################################################################
  delay(50);                          // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  unsigned int FRONT = sonar1.ping() / US_ROUNDTRIP_CM;       // Send ping, get ping time in microseconds (uS).
  unsigned int LEFT  = sonar2.ping() / US_ROUNDTRIP_CM;       // Send ping, get ping time in microseconds (uS).
  unsigned int RIGHT  = sonar3.ping() / US_ROUNDTRIP_CM;      // Send ping, get ping time in microseconds (uS).
  unsigned int LEFT_REAR  = sonar4.ping() / US_ROUNDTRIP_CM;  // Send ping, get ping time in microseconds (uS).
  unsigned int RIGHT_REAR  = sonar5.ping() / US_ROUNDTRIP_CM; // Send ping, get ping time in microseconds (uS).
  //######################################################################################################################################
  //Following section is for the serial monitor
  //######################################################################################################################################
  Serial.print("FRONT: ");
  Serial.print(FRONT); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  Serial.println("cm");
  Serial.print("LEFT: ");
  Serial.print(LEFT); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  Serial.println("cm");
  Serial.print("RIGHT: ");
  Serial.print(RIGHT); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  Serial.println("cm");
  Serial.print("LEFT_REAR: ");
  Serial.print(LEFT_REAR); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  Serial.println("cm");
  Serial.print("RIGHT_REAR: ");
  Serial.print(RIGHT_REAR); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  Serial.println("cm");
}

The ping is returned in cm as well making it easier to work with later on.

NewPing.h (12.1 KB)

prokiller1800 · March 29, 2016, 5:52pm

If i do this, the code just do what the statement "else" defines

int cm1 = SonarFrontLeft.ping() / US_ROUNDTRIP_CM;
int cm2 = SonarFrontRight.ping() / US_ROUNDTRIP_CM;
int cm3 = SonarLeft.ping() / US_ROUNDTRIP_CM;
int cm4 = SonarRight.ping() / US_ROUNDTRIP_CM;

if (cm1 < 35 || cm2 < 35 || cm3 < 35 || cm4 < 35){
digitalWrite(IN1,HIGH);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
}
if (cm1 > 70 && cm2 > 70 && cm3 > 70 && cm4 > 70){
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,HIGH);
}
else{
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
}

johnwasser · March 29, 2016, 8:20pm

prokiller1800:
If i do this, the code just do what the statement "else" defines

So they are not all over 70 cm. Perhaps you should print out the four distance values to see if they match what you expect.

prokiller1800 · March 29, 2016, 9:44pm

the problem is that if i put one below 40 cm it wont recognize.

anon57585045 · March 29, 2016, 10:33pm

prokiller1800:
the problem is that if i put one below 40 cm it wont recognize.

That's what you told it to do with the else statement.

OldSteve · March 29, 2016, 10:57pm

On an aside, rather than doing the conversion from round-trip us to cm yourself, you can just use the 'ping_cm()' method, which automatically returns the distance in cm:-
int cm1 = SonarFrontLeft.ping_cm();It's less typing, if nothing else.

johnwasser:
What does .ping() return when there is no object in range? If it returns 0 and you treat that as "OBJECT VERY CLOSE" that might be causing your problem.

Yes John, it does return 0 for out-of-range.

prokiller1800 · March 30, 2016, 12:14am

its impossible from where im testing it that it exceeds 400 cm (defined max distance) and with that code it should set when one of the sensors detects an object closer than 30 cm to do the first "if" statement. But its not.

Here´s the entire code, i checked the connections they re all good:

#include <NewPing.h>
NewPing SonarFrontLeft(39, 41, 400);
NewPing SonarFrontRight(43, 45, 400);
NewPing SonarLeft(47, 49, 400);
NewPing SonarRight(51, 53, 400);

int IN1=3;
int IN2=4;
int IN3=5;
int IN4=6;

void setup() {
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);

}

void loop() {

int cm1 = SonarFrontLeft.ping() / US_ROUNDTRIP_CM;
int cm2 = SonarFrontRight.ping() / US_ROUNDTRIP_CM;
int cm3 = SonarLeft.ping() / US_ROUNDTRIP_CM;
int cm4 = SonarRight.ping() / US_ROUNDTRIP_CM;

if (cm1 < 35 || cm2 < 35 || cm3 < 35 || cm4 < 35){
digitalWrite(IN1,HIGH);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,LOW);
digitalWrite(IN4,HIGH);
}
if (cm1 > 70 && cm2 > 70 && cm3 > 70 && cm4 > 70){
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,HIGH);
}
else{
digitalWrite(IN1,LOW);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,LOW);
}
}

OldSteve · March 30, 2016, 4:19am

First up, how about you edit your post and place the code in code tags. You did it in your opening post, then reverted to the bad practice of posting your code inline.

its impossible from where im testing it that it exceeds 400 cm (defined max distance)

Besides when it's further away than your defined maximum distance, if the target is on an angle, or is made of a soft material and absorbs the 'ping', (or both), the ping sensor won't receive an echo and 'ping()' or 'ping_cm()' will return 0. You really do need to handle the case of a 0 being returned, even if the target will always be within 400cm. This is especially true with targets that are closer to the max distance.

A ping sensor doesn't work below about 2-3cm anyway, so just handle the case of a value lower than 2 or 3 being returned, and you're covered for those times when no echo is received. Easy enough, and your system will be far more reliable.
Until you do that you can't be guaranteed that it will be reliable.

..... and with that code it should set when one of the sensors detects an object closer than 30 cm to do the first "if" statement. But its not.

Your code actually says 35cm, not 30.

if (cm1 < 35 || cm2 < 35 || cm3 < 35 || cm4 < 35)

If this isn't working, then something else is wrong besides the code that you've posted.
As was suggested by johnwasser, (and you totally ignored), add some debugging serial prints to your code after each 'ping' to see what's going on. You don't need to guess.

johnwasser · March 30, 2016, 11:30am

prokiller1800:
the problem is that if i put one below 40 cm it wont recognize.

You forgot the 'else' after the first 'if'. Even if one or more sensors return below 35 you will set the outputs and immediately drop through to the second 'if' statement which will set the outputs differently. They should be as you expected, for a few microseconds.

OldSteve · March 30, 2016, 11:43am

johnwasser:
You forgot the 'else' after the first 'if'. Even if one or more sensors return below 35 you will set the outputs and immediately drop through to the second 'if' statement which will set the outputs differently. They should be as you expected, for a few microseconds.

Ah, of course. I somehow missed the lack of that first 'else'.
(Another good argument for those serial prints. The problem would have showed up immediately.)

prokiller1800 · March 30, 2016, 3:54pm

This time i tested the sensors with an example from the newping library. The problem i encountered is that every single sensor is working, but the only one that counts for the "if" statements is the first one, connected in the 39,41 pins (Trigger, Echho).

#include <NewPing.h>

#define SONAR_NUM     4 
#define MAX_DISTANCE 400 
#define PING_INTERVAL 30 

unsigned long pingTimer[SONAR_NUM]; 
unsigned int cm[SONAR_NUM];         
uint8_t currentSensor = 0;       

NewPing sonar[SONAR_NUM] = {    
  NewPing(39, 41, MAX_DISTANCE),
  NewPing(43, 45, MAX_DISTANCE),
  NewPing(47, 49, MAX_DISTANCE),
  NewPing(51, 53, MAX_DISTANCE),
};

void setup() {
  Serial.begin(115200);
  pingTimer[0] = millis() + 75;         
  for (uint8_t i = 1; i < SONAR_NUM; i++)
    pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
}

void loop() {
  for (uint8_t i = 0; i < SONAR_NUM; i++) { 
    if (millis() >= pingTimer[i]) {         
      pingTimer[i] += PING_INTERVAL * SONAR_NUM;  
      if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle();
      sonar[currentSensor].timer_stop();          
      currentSensor = i;                          
      cm[currentSensor] = 0;                      
      sonar[currentSensor].ping_timer(echoCheck); 
    }
  }

}

void echoCheck() { 
  if (sonar[currentSensor].check_timer())
    cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}

void oneSensorCycle() { 
  for (uint8_t i = 0; i < SONAR_NUM; i++) {
    Serial.print(i);
    Serial.print("=");
    Serial.print(cm[i]);
    Serial.print("cm ");
  }
  Serial.println();

  if(cm[currentSensor] <35){

  Serial.print("TRAS");
  }
  if(cm[currentSensor] >75){
  Serial.print("Frente");
  }
  if(cm[currentSensor] >35 && cm[currentSensor]<75){
  Serial.print("LADO");
  }
}

system · March 30, 2016, 7:02pm

but the only one that counts for the "if" statements is the first one, connected in the 39,41 pins (Trigger, Echho).

What do you mean by "the only one that counts"? In which if statement? There are 3 of them.

prokiller1800 · March 30, 2016, 7:23pm

I mean every single one is working but only one of them is commanding the "if" statements.

system · March 30, 2016, 7:27pm

I mean every single one is working but only one of them is commanding the "if" statements.

I still don't understand. You can't "command an if statement".

johnwasser · April 3, 2016, 1:12pm

i think it is time for you to post your full sketch again.

Topic		Replies	Views
HCSRO4 problems Sensors	48	559	April 2, 2024
Obstacle avoiding car - Ultrasonic sensor only detects 1/4 of obstacles Programming	64	608	June 14, 2025
Arduino Nano & HC-SR04 Ultrasonic sensor Not working General Guidance	33	363	September 30, 2025
Ultrasonic Sensor HCSR-04 Sensors	25	368	August 24, 2024
Two sensors HC-SR04 with different behaviors Sensors	10	215	April 15, 2024

HC-SR04 Robot Control Problems

Related topics