Help with code for telling direction

I have set up two ultrasonic sensor right next eachother and want to be able to detect the direction of passing objects (left, right).

It probably is really easy but I cant figura out how to make it happen.

This is the current sketch that I tried but does not work.

#include <NewPing.h>

#define SONAR_NUM     2 // Number of sensors.
#define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 50 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).

unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
unsigned int cm[SONAR_NUM];         // Where the ping distances are stored.
uint8_t currentSensor = 0;          // Keeps track of which sensor is active.
int dirA = 0;
int dirB = 0;

int doorDistance = 65;

boolean bigChange_0;  //SensorA
boolean bigChange_1;  //SensorB

NewPing sonar[SONAR_NUM] = {     // Sensor object array.
  NewPing(8, 7, MAX_DISTANCE), // Each sensor's trigger pin, echo pin, and max distance to ping.
  NewPing(6, 5, MAX_DISTANCE)
  
};

void setup() {
  Serial.begin(115200);
  pingTimer[0] = millis() + 75;           // First ping starts at 75ms, gives time for the Arduino to chill before starting.
  for (uint8_t i = 1; i < SONAR_NUM; i++) // Set the starting time for each sensor.
    pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
}

void loop() {
  for (uint8_t i = 0; i < SONAR_NUM; i++) { // Loop through all the sensors.
    if (millis() >= pingTimer[i]) {         // Is it this sensor's time to ping?
      pingTimer[i] += PING_INTERVAL * SONAR_NUM;  // Set next time this sensor will be pinged.
      if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle(); // Sensor ping cycle complete, do something with the results.
      sonar[currentSensor].timer_stop();          // Make sure previous timer is canceled before starting a new ping (insurance).
      currentSensor = i;                          // Sensor being accessed.
      cm[currentSensor] = 0;                      // Make distance zero in case there's no ping echo for this sensor.
      sonar[currentSensor].ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
    }
  }
  // Other code that *DOESN'T* analyze ping results can go here.
}

void echoCheck() { // If ping received, set the sensor distance to array.
  if (sonar[currentSensor].check_timer())
    cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}

void oneSensorCycle() { // Sensor ping cycle complete, do something with the results.
  // The following code would be replaced with your code that does something with the ping results.
  //for (uint8_t i = 0; i < SONAR_NUM; i++) {
    //Serial.print(i);
    //Serial.print("=");
    //Serial.print(cm[i]);
    //Serial.print("cm ");
  //}
  //Serial.println();
  bigChange_0 = ((doorDistance-(int)cm[0]) > 2)?true:false;
  bigChange_1 = ((doorDistance-(int)cm[1]) > 2)?true:false;

  if(bigChange_0 && bigChange_1){
 
      if(dirA > dirB && dirA+dirB > 2){
          Serial.print("dirA>dirB");
          Serial.println();
          
          Serial.print("Entering Room");
          Serial.println();
          dirA = 0;
          dirB = 0;
        }
        else if(dirA < dirB && dirA+dirB > 2){
          Serial.print("dirB<dirA");
          Serial.println();
          
          Serial.print("Leaving Room ");
          Serial.print(dirA);
          Serial.print("   ");
          Serial.print(dirB);
          Serial.println();
          dirA = 0;
          dirB = 0;
          }
    }
  else if(bigChange_0){
      Serial.print("bigChange_0");
      Serial.println();
    dirA++;
  }
  else if(bigChange_1){
       Serial.print("bigChange_0 && bigChange_1");
      Serial.println();
    dirB++;
    }
    
  }

Well done for using code tags.

But your description of your project requirements, and your description of the fault with your current code are completely inadequate. Describe the project in real world terms. Some diagrams might help!

It's going to be hard to tell which way a person is moving if all you have is their distance from the sensors last time you checked. Especially if the sensors are "right next to each other".

My best guess at what MIGHT work is to record the time when the object (person) was closest to each sensor and use the relative time to tell which direction the person came from.

When nothing is passing, set the two distances to some large number. Each time a sensor reports a smaller number, record that as the current shortest distance for that sensor and record the millis() value for that sensor. When the object has passed both sensors, compare the times to see which sensor saw the object first.

You will get a more positive detection if you separate the sensors, or at least aim them apart a bit.

Sorry for the lack of description. I have both ultrasonic sensors set up next to eachother and they are both measuring the distance to the opposite wall which is around 65cm away. when any of the sensors detect a new measurement which differs more than 2cm from the 65cm default value the boolean value for that sensor changes to true.

"bigChange_0 = ((doorDistance-(int)cm[0]) > 2)?true:false;" /Sensor 1 "bigChange_1 = ((doorDistance-(int)cm[0]) > 2)?true:false;" /Sensor 2

I could probably give the sensors a better variable-name and also increase the minimum change from 2cm to 10-20cm.

So when I walk in front of the sensors the first sensor will be set to true as long as I am between it and the wall and then the next sensor will report true. So if the first sensor is set to true before the second that would be "direction 1" and the opposite when walking from the other sida ande the second sensor triggers before the first.

I tried a little more and got some progress but the sketch locks upp after a few passings and I guess there is alot of more scenarios to add to the if/else like if I stop after the first sensor has been triggered and walk back and probably a few more similar scenarios.

This is what I have now:

#include <NewPing.h>

#define SONAR_NUM     2 // Number of sensors.
#define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 50 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).

unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
unsigned int cm[SONAR_NUM];         // Where the ping distances are stored.
uint8_t currentSensor = 0;          // Keeps track of which sensor is active.
int dirA = 0;
int dirB = 0;

int doorDistance = 65;

boolean bigChange_0;  //SensorA
boolean bigChange_1;  //SensorB

NewPing sonar[SONAR_NUM] = {     // Sensor object array.
  NewPing(8, 7, MAX_DISTANCE), // Each sensor's trigger pin, echo pin, and max distance to ping.
  NewPing(6, 5, MAX_DISTANCE)
  
};

void setup() {
  Serial.begin(115200);
  pingTimer[0] = millis() + 75;           // First ping starts at 75ms, gives time for the Arduino to chill before starting.
  for (uint8_t i = 1; i < SONAR_NUM; i++) // Set the starting time for each sensor.
    pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
}

void loop() {
  for (uint8_t i = 0; i < SONAR_NUM; i++) { // Loop through all the sensors.
    if (millis() >= pingTimer[i]) {         // Is it this sensor's time to ping?
      pingTimer[i] += PING_INTERVAL * SONAR_NUM;  // Set next time this sensor will be pinged.
      if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle(); // Sensor ping cycle complete, do something with the results.
      sonar[currentSensor].timer_stop();          // Make sure previous timer is canceled before starting a new ping (insurance).
      currentSensor = i;                          // Sensor being accessed.
      cm[currentSensor] = 0;                      // Make distance zero in case there's no ping echo for this sensor.
      sonar[currentSensor].ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
    }
  }
  // Other code that *DOESN'T* analyze ping results can go here.
}

void echoCheck() { // If ping received, set the sensor distance to array.
  if (sonar[currentSensor].check_timer())
    cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}

void oneSensorCycle() { // Sensor ping cycle complete, do something with the results.
  
  bigChange_0 = ((doorDistance-(int)cm[0]) > 10)?true:false;
  bigChange_1 = ((doorDistance-(int)cm[1]) > 10)?true:false;

  if(bigChange_0 && bigChange_1){
 
      if(dirA > dirB){
          Serial.print("Entering Room");
          Serial.println();
          dirA = 0;
          dirB = 0;
        }
        else if(dirA < dirB){
          Serial.print("Leaving Room");
          Serial.println();
          dirA = 0;
          dirB = 0;
          }
    }
  else if(bigChange_0){
      dirA=1;
  }
  else if(bigChange_1){
     dirB=1;
    }
    
  }

OK... I think I see what you are doing. If only one sensor detects an object, set a flag for that object. If both sensors detect an object it came from the side that already has the flag set.

Perhaps if neither is detecting an object you can reset the flags. That would fix the problem of someone being detected by one sensor and not the other.

  else if(bigChange_0) {
      dirA=1;
  }
  else if(bigChange_1) {
     dirB=1;
    } else { // Neither sensor is detecting a big change
      dirA = 0;
      dirB = 0;
    }

Thanks for the help. The code is working as expected most of the time. I added a button to calibrate the distance to the opposite wall so that I can move the sensors to a new location and press the button to have it work the same even if the static distance changes.

But I have another question, right now the measurement-part is always running. if I add a boolen variable like “boolean triggered;” I want the “direction-measurement-part” to run for 10s, like this:

if (triggered){
//run measurement for 10s
}

then after 10s it should stop untill triggered again.
I have tried reading the “blink_without_delay” -sketch as it uses millis() and it seams to do almost exactly what I want. But I cant quite figure it out. Do I need a while-loop instead of an “if” -statement?

#include <NewPing.h>
#include <Bounce2.h>
#include <SPI.h>

#define SONAR_NUM     2 // Number of sensors.
#define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 40 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).


int ledPin = 4;
//Bounce debouncer = Bounce(); 
//int oldValue=-1;

unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
unsigned int cm[SONAR_NUM];         // Where the ping distances are stored.
uint8_t currentSensor = 0;          // Keeps track of which sensor is active.
int dirA = 0;
int dirB = 0;

int val = 0;
#define BUTTON_PIN  3  // Arduino Digital I/O pin for button/reed switch
int doorDistance = 65;

boolean bigChange_0;  //SensorA
boolean bigChange_1;  //SensorB

NewPing sonar[SONAR_NUM] = {     // Sensor object array.
  NewPing(8, 7, MAX_DISTANCE), // Each sensor's trigger pin, echo pin, and max distance to ping.
  NewPing(6, 5, MAX_DISTANCE)
  
};

void setup() {
  Serial.begin(115200);
  pingTimer[0] = millis() + 75;           // First ping starts at 75ms, gives time for the Arduino to chill before starting.
  for (uint8_t i = 1; i < SONAR_NUM; i++) // Set the starting time for each sensor.
    pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;

     // Setup the button
  pinMode(BUTTON_PIN,INPUT);
  // Activate internal pull-up
  digitalWrite(BUTTON_PIN,HIGH);



   pinMode(ledPin, OUTPUT);  // declare LED as output
}

void loop() {
  for (uint8_t i = 0; i < SONAR_NUM; i++) { // Loop through all the sensors.
    if (millis() >= pingTimer[i]) {         // Is it this sensor's time to ping?
      pingTimer[i] += PING_INTERVAL * SONAR_NUM;  // Set next time this sensor will be pinged.
      if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle(); // Sensor ping cycle complete, do something with the results.
      sonar[currentSensor].timer_stop();          // Make sure previous timer is canceled before starting a new ping (insurance).
      currentSensor = i;                          // Sensor being accessed.
      cm[currentSensor] = 0;                      // Make distance zero in case there's no ping echo for this sensor.
      sonar[currentSensor].ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
    }
  }
  // Other code that *DOESN'T* analyze ping results can go here.
  val = digitalRead(BUTTON_PIN);  // read input value
  if (val == HIGH) {         // check if the input is HIGH (button released)
    digitalWrite(ledPin, LOW);  // turn LED OFF
  } else {
    digitalWrite(ledPin, HIGH);  // turn LED ON
    
    int measure = (int)cm[0];
    if(measure != doorDistance && measure != 0){
       Serial.print(measure);
       Serial.println();
       doorDistance = measure;
       
      }
   
  }
     
     // Send in the new value
    // send(msg.set(value==HIGH ? 1 : 0));
}

void echoCheck() { // If ping received, set the sensor distance to array.
  if (sonar[currentSensor].check_timer())
    cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}

void oneSensorCycle() { // Sensor ping cycle complete, do something with the results.
  
  bigChange_0 = ((doorDistance-(int)cm[0]) > 10)?true:false;
  bigChange_1 = ((doorDistance-(int)cm[1]) > 10)?true:false;

  if(bigChange_0 && bigChange_1){
 
      if(dirA > dirB){
          Serial.print("Entering Room");
          Serial.println();
          dirA = 0;
          dirB = 0;
        }
        else if(dirA < dirB){
          Serial.print("Leaving Room");
          Serial.println();
          dirA = 0;
          dirB = 0;
          }
        else{
          dirA = 0;
          dirB = 0;
          }  
    }
  else if(bigChange_0){
      dirA=1;
  }
  else if(bigChange_1){
     dirB=1;
    }
  else {
    dirA = 0;
    dirB = 0;
    }    
  }

Blink_without_delay:

// constants won't change. Used here to set a pin number :
const int ledPin =  LED_BUILTIN;// the number of the LED pin

// Variables will change :
int ledState = LOW;             // ledState used to set the LED

// Generally, you should use "unsigned long" for variables that hold time
// The value will quickly become too large for an int to store
unsigned long previousMillis = 0;        // will store last time LED was updated

// constants won't change :
const long interval = 1000;           // interval at which to blink (milliseconds)

void setup() {
  // set the digital pin as output:
  pinMode(ledPin, OUTPUT);
}

void loop() {
  // here is where you'd put code that needs to be running all the time.

  // check to see if it's time to blink the LED; that is, if the
  // difference between the current time and last time you blinked
  // the LED is bigger than the interval at which you want to
  // blink the LED.
  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval) {
    // save the last time you blinked the LED
    previousMillis = currentMillis;

    // if the LED is off turn it on and vice-versa:
    if (ledState == LOW) {
      ledState = HIGH;
    } else {
      ledState = LOW;
    }

    // set the LED with the ledState of the variable:
    digitalWrite(ledPin, ledState);
  }
}

Using two adjacent ultrasonic modules could give problems - what happens if one receives a pulse sent by the other?

Allan

I am using the newping library and according to the examples you can have atleast 15 sensors. The delay have to be 29 ms or more between the sensors.