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!

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