Single Method for 2 Ultrasonic Sensor is not working as expected

Question

So I have created a method to read and return Ultrasonic Sensor Data. I have 2 Ultrasonic Sensors.

Below is my code.

  // Ultrasonic Pins
  #define T1 2
  #define E2 4
  #define E1 3
  #define T2 5
  // Motor Pins
  #define M1 7
  #define M2 8
  #define M3 9
  #define M4 10
  // Motor Movements Definition
  enum MOTOR
  {
    FORWARD,
    BACKWARD,
    LEFT,
    RIGHT,
    STOP
  };
  long duration1, distance1, duration2, distance2;
  void setup()
  {
    Serial.begin(9600);
    pinMode(E1, INPUT);
    pinMode(E2, INPUT);
    pinMode(T1, OUTPUT);
    pinMode(T2, OUTPUT);
    pinMode(M1, OUTPUT);
    pinMode(M2, OUTPUT);
    pinMode(M3, OUTPUT);
    pinMode(M4, OUTPUT);
    Motor(STOP);
  }
  void loop()
  {
    // If Both Sensors detect object then go in backward Direction
    while (DetectObject(T1,E1,1) < 10 && DetectObject(T2,E2,2) < 10)
    {
      Serial.println("Backward");
      Motor(BACKWARD);
      delay(200);
      Motor(STOP);
      delay(50);
    }
    // If Left Sensor detect object then go in Right Direction
    while (DetectObject(T1,E1,1) < 10 && DetectObject(T2,E2,2) > 10)
    {
      Serial.println("Right");
      Motor(RIGHT);
      delay(200);
      Motor(STOP);
      delay(50);
    }
    // If Right Sensor detect object then go in Left Direction
    while (DetectObject(T1,E1,1) > 10 && DetectObject(T2,E2,2) < 10)
    {
      Serial.println("Left");
      Motor(LEFT);
      delay(200);
      Motor(STOP);
      delay(50);
    }
    // If Both Sensors detect no object then go in Forward Direction
    while (DetectObject(T1,E1,1) > 10 && DetectObject(T2,E2,2) > 10)
    {
      Serial.println("Forward");
      Motor(FORWARD);
      delay(200);
      Motor(STOP);
      delay(50);
    }
  }
  void Motor(MOTOR motor)
  {
    switch (motor)
    {
    case STOP:
      digitalWrite(M1, LOW);
      digitalWrite(M2, LOW);
      digitalWrite(M3, LOW);
      digitalWrite(M4, LOW);
      break;
    case FORWARD:
      digitalWrite(M1, HIGH);
      digitalWrite(M2, LOW);
      digitalWrite(M3, HIGH);
      digitalWrite(M4, LOW);
      break;
    case BACKWARD:
      digitalWrite(M1, LOW);
      digitalWrite(M2, HIGH);
      digitalWrite(M3, LOW);
      digitalWrite(M4, HIGH);
      break;
    case LEFT:
      digitalWrite(M1, HIGH);
      digitalWrite(M2, LOW);
      digitalWrite(M3, LOW);
      digitalWrite(M4, HIGH);
      break;
    case RIGHT:
      digitalWrite(M1, LOW);
      digitalWrite(M2, HIGH);
      digitalWrite(M3, HIGH);
      digitalWrite(M4, LOW);
      break;
    default:
      break;
    }
    delay(100);
  }
  long DetectObject(uint8_t trigPin, uint8_t echoPin, int sensorNo)
  {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    Serial.print("Distance Sensor");
    Serial.print(sensorNo);
    Serial.print(": ");
    Serial.print(pulseIn(echoPin, HIGH) * 0.034 / 2);
    Serial.println(" cm");
    return pulseIn(echoPin, HIGH) * 0.034 / 2;
  }

I am getting continuously getting Backward in the Serial monitor.

Output of Serial Monitor -

Distance Sensor1: 20 cm
Distance Sensor2: 15 cm
Backward
Distance Sensor1: 17 cm
Distance Sensor2: 21 cm
Backward
Distance Sensor1: 24 cm
Distance Sensor2: 9 cm
Backward

But when I created 2 different methods for each Ultrasonic Sensor. It is working fine.

I want to have a single method for N number of Ultrasonic Sensors.

Thanks in Advance!

• Ansari Aquib: @jsotola I have added the output of Serial Monitor.
• 6v6gt: I think you should try restructuring your loop(). At the start of the loop(), get a value for DetectObject(T1,E1,1) and DetectObject(T2,E2,2) called say Det1 and Det2 respectively. Then change your while statements to if statements, say if (Det1 < 10 && Det2 < 10 ) . . .
• Ansari Aquib: @6v6gt Already tried it is not working.

Answer 1

The problem lies in the function, that does the ultrasonic detection. First I would suggest to not do Serial printing between the trigger pulse and pulseIn(), since the Serial transmission might take longer (for example when the buffer is full) and then your reading is garbage. Execute pulseIn() directly after the trigger pulse part and save the result in a local variable, which you can later print and return. If you do that, your problem is also solved.

For understanding what actually happens with your code, I annotated the function with some comments:

long DetectObject(uint8_t trigPin, uint8_t echoPin, int sensorNo)
 {
 // Create Trigger pulse
 digitalWrite(trigPin, LOW);
 delayMicroseconds(2);
 digitalWrite(trigPin, HIGH);
 delayMicroseconds(10);
 digitalWrite(trigPin, LOW);
 // Do some printing
 Serial.print("Distance Sensor");
 Serial.print(sensorNo);
 Serial.print(": ");
 // Read a pulse and print it directly to Serial
 Serial.print(pulseIn(echoPin, HIGH) * 0.034 / 2);
 Serial.println(" cm");
 // Read ANOTHER pulse and return it
 return pulseIn(echoPin, HIGH) * 0.034 / 2;
 }

As you can see, you are trying to read 2 pulses. But there is only one echo pulse. That one pulse is read by the first execution of pulseIn() and you print the result to Serial, so in the Serial Monitor you are seeing the correct value.

After that you are executing pulseIn() again and returning the result. pulseIn() will wait for a pulse to begin (so it can start counting the time). But there is no second pulse on the echo line, so pulseIn() will exit after a timeout (by default 1s). On timeout this function returns zero. So every time you execute this function, it will return zero.

If you then look at the logic in loop(), you can easily see why you are only getting backwards. The code is stuck in the first while loop, because the value returned from the function is always zero, which is smaller than 10.

Here is a pseudo code version of how your function really should work:

execute trigger pulse
execute pulseIn function and save result in local variable
print whatever you want to Serial
return the value of the local variable

• Ansari Aquib: Thanks alot. It's working now. Learned something new.