(Please Help) Ultrasonic Sensors are not working in the if else statement

I am currently modifying a RC car with two ultrasonic sensors HC-SR04 and I am using a Arduino Mega 2560. Each function is working perfectly. However, if I combine them all together in the void loop(), it work on the third statement directly (“else if (((distance_two <= 5)) && ( (distance_one >=6)))”) but nothing in front of the sensor and the rest of the statement are not working.

The following describe what the void loop do:
if obstacle away from both sensor, then the car moving forward
if obstacle is detected by the ultra_sensor_one within 5 cm, then the car turn right
if obstacle is detected by the ultra_sensor_two within 5 cm, then the car turn left
if obstacle is detected by both sensor within 5 cm, then the car reverse and turn right

The following is the code that I did:

int forward = 26; // forward pin
int reverse = 27; // reverse pin
int left = 28; // left pin
int right = 29; // right pin
long  duration_one, distance_one, duration_two, distance_two;

#define trigPin_one 23 // trigger pin in the ultrasonic sensor 1
#define echoPin_one 22 // echo pin in the iltrasonic sensor 1
#define trigPin_two 25 // trigger pin in the ultrasonic sensor 2
#define echoPin_two 24 // echo pin in the iltrasonic sensor 2

void setup() {
// initialize the digital pins as an outputs:
pinMode(forward, OUTPUT);
pinMode(reverse, OUTPUT);
pinMode(left, OUTPUT);
pinMode(right, OUTPUT);

Serial.begin (9600);
pinMode(trigPin_one, OUTPUT);
pinMode(echoPin_one, INPUT); 
Serial.begin (9600);
pinMode(trigPin_two, OUTPUT);
pinMode(echoPin_two, INPUT);

}

void go_forward()
{
digitalWrite(forward,HIGH); // turn forward motor on
digitalWrite(reverse,LOW); // turn revers motor off
digitalWrite(left,LOW);
digitalWrite(right,LOW);
}
void go_reverse()
{
digitalWrite(reverse,HIGH); // turn reverse motor on
digitalWrite(forward,LOW); // turn forward notor off
digitalWrite(left,LOW);
digitalWrite(right,LOW);
}
void stop_car()
{
digitalWrite(reverse,LOW); // turn revers motor off
digitalWrite(forward,LOW); // turn forward motor off
digitalWrite(left,LOW);
digitalWrite(right,LOW);
}
void go_left()
{ 
digitalWrite(forward,HIGH); // turn forward motor on
digitalWrite(reverse,LOW); // turn revers motor off
digitalWrite(left,HIGH); // turn left motor on
digitalWrite(right,LOW); // turn right motor off
}
void go_right()
{ 
digitalWrite(forward,HIGH); // turn forward motor on
digitalWrite(reverse,LOW); // turn revers motor off
digitalWrite(right,HIGH); // turn right motor on
digitalWrite(left,LOW); // tune left motor off
}

void ultra_sensor_two() {
int duration_two, distance_two;
digitalWrite(trigPin_two,HIGH);
delayMicroseconds(1000);
digitalWrite(trigPin_two, LOW);
duration_two = pulseIn(echoPin_two, HIGH);
distance_two = (duration_two /2) / 29.1;
Serial.print(distance_two);
Serial.println ("cm_2");
delay(2);
}

void ultra_sensor_one() {
int duration_one, distance_one;
digitalWrite(trigPin_one,HIGH);
delayMicroseconds(1000);
digitalWrite(trigPin_one, LOW);
duration_one = pulseIn(echoPin_one, HIGH);
distance_one = (duration_one / 2) / 29.1;
Serial.print(distance_one);
Serial.println ("cm_1");
delay(2);
}

void ultra_sensor_both() {
ultra_sensor_one();
delay(2);
ultra_sensor_two();
delay(2);
}



void loop() {

ultra_sensor_both();

if (( (distance_one >= 6)) || ( (distance_two >= 6))){
  go_forward();
  delay(800);
}
else if (( (distance_one <= 5)) && ( (distance_two >= 6))){
  stop_car();
  delay (800);
  go_reverse();
  delay(800);
  go_left();
  delay(800);
}
else if (((distance_two <= 5)) && ( (distance_one >=6))){
  stop_car();
  delay(800);
  go_reverse();
  delay(800);
  go_right();
  delay(800);
}
else if (( (distance_one <=5)) && ( (distance_two <=5))){
  stop_car();
  delay(800);
  go_reverse();
  delay(800);
  go_right();
  delay(800); 
}  
}
  1. please post according guidelines - using code tags
  2. CTRL T gives you autoindent to make the code more readable

wrt your problem, recode the application to get rid of delay and use comparison with millis instead.

  • check the blink without delay example for the basics.
    It will make the sketch far more responsive (as delay blocks everything except interrupts)

. . . and DON'T CROSS-POST.

Duplicate deleted.

if (( (distance_one >= 6)) || ( (distance_two >= 6))){
  go_forward();
  delay(800);
}
else if (( (distance_one <= 5)) && ( (distance_two >= 6))){

I think you have your logic confused. Let’s think about just these two here.

In order to get into the second one, the first one has to be false, because the second one is an else if. Can the second one be true if the first one is false? The second requires that distance_two is greater than or equal to 6. But the first one say that if either one or the other is greater than 6 then do the first thing. So this second condition can never be met. If it were ever true, then the one above would also be so it would run instead.

You have the same problem with the next block. So really, only the first block (either sensor reading greater than 6) or the last block (both sensors reading less than 5) can ever be executed.

I think you should spend some time with a pencil and a piece of paper and try to map the logic for this thing out a little better.

You need only initialize Serial once.
When changing motion, I would FIRST "turn-off" previous motions (to achieve a neutral state), THEN "turn-on" next motion.