0

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
}
  • 1
    What exactly do you want it to do? Currently your top priority is to stop if any sensor is outside the range bracket or if epirVal is true. If epirVal is false and a sensor is in range you turn. If epirVal is false and no sensor is within range you keep doing whatever you were doing before. – Andrew Jan 24 '17 at 17:05
  • @Andrew I want the robot to go straight if the middle sensor detects the person between 65cm and 150cm and the PIR sensor sees the person (when it sees a person epirVal == false because it's active low. If the right sensor sees the person in the same range and the epirVal is low, the robot turns right. If the left sensor sees the person in the same range and the epirVal is low, then it turns left. I had too many else if's in the last code. I have rewritten my code and the applicable part is the very last function. There is something very wrong with my logic. looking for help. – Daniel Shields Jan 26 '17 at 21:49
  • @Andrew and btw it doesn't do anything with this current code. It just sits there. – Daniel Shields Jan 26 '17 at 21:51
  • 2
    Do not vandalise your post. This may result in a question ban. see question bans – Nick Gammon Feb 5 '17 at 7:28
2

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    |           |
+--------+----------+----------+----------+-----------+
  • The pseudo-code above is what I want the robot to do. I just can't think how to make it mutually exclusive. I have been looking at your answer and thinking about this most of the day, and I don't know what the next step is. I know you say to rethink the problem and ask myself the questions, but I thought I already had it coded to do just exactly what your questions are. I'm not seeing the logic. Any hints to give me another step in the right direction? – Daniel Shields Jan 25 '17 at 23:02
  • You said, "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." I feel like I'm missing something on the fundamental use of if...else if. Can you help? – Daniel Shields Jan 26 '17 at 19:56
  • @DanielShields: See the De Morgan's laws: NOT(NOT(PIR detects) OR NOT(sonar 0 in range) OR NOT(sonar 1 in range) OR NOT(sonar 2 in range)) = (PIR detects AND sonar 0 in range AND sonar 1 in range AND sonar 2 in range). – Edgar Bonet Jan 26 '17 at 20:52
  • I'm not a philosopher or a professional programmer. Can you be a little more specific? – Daniel Shields Jan 26 '17 at 21:12
  • @ Edgar Bonet changed my code above. I now know that you can't have more than one else if statement so those were changed. I tried to think of every scenario and make the code mutually exclusive and cover every possibility. Per your suggestion. Now it does nothing except for the middle and left led flash really quickly. – Daniel Shields Jan 26 '17 at 21:38

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.