Multiple if statements - stuck

Question

I have a robot that uses two motors to skid steer. The robot gets input from three ultrasonic sensors (left, middle, and right) and a passive infrared sensor. If the robot detects a person in front of it, then it follows.

The sensors seem to work as they are intended. The infrared sensor sees a person and the ultrasonic sensors see that the person is between 65cm and 150cm and it follows. If the left ultrasonic sensor has the person in range, it turns left, and if the right ultrasonic sensor has the person in range, it turns right.

The problem is that the robot gets stuck turning left or turning right depending on which sensor it checks first in the code. It's like it's stuck in a while loop even though I'm not using a while loop.

I have adapted NewPing and library. The code in question is at the bottom in the last function. The rest is part of NewPing to read the ultrasonic sensors.

If anybody has any suggestions, it would be greatly appreciated.

#include <NewPing.h>

#define SONAR_NUM     3 // Number of sensors.
#define MAX_DISTANCE 170 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 35 // 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.

NewPing sonar[SONAR_NUM] = {     // Sensor object array.
  NewPing(11, 4, MAX_DISTANCE), //Left -- Each sensor's trigger pin, echo pin, and max distance to ping.
  NewPing(8, 3, MAX_DISTANCE),  //Center
  NewPing(7, 9, MAX_DISTANCE),  //Right
};

int ledR = 13;
int ledG = 2;
int ledB = A0;
int motorPinL = 5;
int motorPinR = 6;
int ePIR = 10;
boolean epirVal;

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;
  pinMode (ledR, OUTPUT);
  pinMode (ledG, OUTPUT);
  pinMode (ledB, OUTPUT);
  pinMode (motorPinL, OUTPUT);
  pinMode (motorPinR, OUTPUT);
  pinMode (ePIR, INPUT);
}

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] = 155;                      // Make distance 155 in case there's no ping echo for this sensor. changed from zero
      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 allStop () {
  analogWrite (motorPinL, 0);
  analogWrite (motorPinR, 0);
  digitalWrite (ledG, LOW);    //LED corresponding to right ultrasonic sensor
  digitalWrite (ledR, LOW);    //LED corresponding to left ultrasonic sensor
  digitalWrite (ledB, LOW);    //LED corresponding to center ultrasonic sensor
}

Answer 1

Your logic is flawed. Translating your loop in pseudocode gives something like this:

if (PIR does not detect OR ANY sonar is out of range) {  // cond. 1
    stop
} else if (PIR does detect AND right sonar is in range) {  // cond. 2
    turn right
} else if (PIR does detect AND left sonar is in range) {  // cond. 3
    turn left
} else if (PIR does detect AND center sonar is in range) {  // cond. 4
    go straight
}

Notice that, if condition 1 is false, then that means that the PIR does detect AND ALL of the sonars are in range. Which implies that condition 2 is necessarily true. Which implies that the following elses have no effect. Your code can then be optimized (and the compiler probably did optimize it) into

if (ANY sonar is out of range OR PIR does not detect) {  // cond. 1
    stop
} else {  // cond. 2 is true if cond 1. is false
    turn right
}

You should rethink your problem asking yourself the following questions:

• under what conditions should the robot turn right?
• under what conditions should the robot turn left?
• under what conditions should the robot go straight?
• under what conditions should the robot stop?

Make sure that in any circumstance EXACTLY one of the above conditions holds true. In other words, the conditions should be mutually exclusive and cover every possibility. One you have the answers, the coding should be straightforward.

---

Edit: Your updated code still contains logic flaws. For example, if the PIR senses the person and BOTH the left and right sonars are in range, then you are asking the robot to BOTH turn left and turn right. Unless it's a quantum robot evolving in an Everett's multiverse, it won't be able to comply. To the risk of repeating myself, you must decide on one and only one action for every possible situation.

If you are struggling with that concept, I suggest you start by filling the truth table below. On each of the 16 lines of the table you should fill the last column with one (and only one) action among: allStop, turnRight, turnLeft and straight. Yes, I know it's tedious to consider all the possible situations one by one, but you must do that before starting to write the code. Otherwise you end up with completely broken logic.

+--------+----------+----------+----------+-----------+
| PIR    | sonar 0  | sonar 1  | sonar 2  |  robot    |
| senses | in range | in range | in range |  action   |
+--------+----------+----------+----------+-----------+
|  no    |   no     |   no     |   no     |           |
|  no    |   no     |   no     |   yes    |           |
|  no    |   no     |   yes    |   no     |           |
|  no    |   no     |   yes    |   yes    |           |
|  no    |   yes    |   no     |   no     |           |
|  no    |   yes    |   no     |   yes    |           |
|  no    |   yes    |   yes    |   no     |           |
|  no    |   yes    |   yes    |   yes    |           |
|  yes   |   no     |   no     |   no     |           |
|  yes   |   no     |   no     |   yes    |           |
|  yes   |   no     |   yes    |   no     |           |
|  yes   |   no     |   yes    |   yes    |           |
|  yes   |   yes    |   no     |   no     |           |
|  yes   |   yes    |   no     |   yes    |           |
|  yes   |   yes    |   yes    |   no     |           |
|  yes   |   yes    |   yes    |   yes    |           |
+--------+----------+----------+----------+-----------+