Understanding obstacle avoiding sketch

Hi

Im tring to understand this method in an obstacle avoiding robot sketch.

void checkPath() {
  Serial.println("Checking path...");
  int curLeft = 0;
  int curFront = 0;
  int curRight = 0;
  int curDist = 0;
  myservo.write(144); // set servo to face left 54-degrees from forward
  delay(120); // wait 120milliseconds for servo to reach position
  for(pos = 144; pos >= 36; pos-=18)     // loop to sweep the servo (& sensor) from 144-degrees left to 36-degrees right at 18-degree intervals. 
  {
    myservo.write(pos);  // tell servo to go to position in variable 'pos' 
    delay(90); // wait 90ms for servo to get to position   
    checkForward(); // check the robot is still moving forward
    curDist = readPing(); // get the current distance to any object in front of sensor
    
    // If distances are too close///objects are nearby!!!
    if (curDist < COLL_DIST) { // if the current distance to object is less than the collision distance
      Serial.println("call checkCourse()");
      checkCourse(); // run the checkCourse function
      break; // jump out of this loop
    }
    if (curDist < TURN_DIST) { // if current distance is less than the turn distance
      Serial.println("call changePath()");
      changePath(); // run the changePath function
    }
    
    //ELSE set values
    if (curDist > curDist) {maxAngle = pos; Serial.println("curDist");}
    if (pos > 90 && curDist > curLeft) { curLeft = curDist; Serial.println("curLeft");}
    if (pos == 90 && curDist > curFront) {curFront = curDist; Serial.println("curFront");}
    if (pos < 90 && curDist > curRight) {curRight = curDist; Serial.println("curRight");}
  }
  maxLeft = curLeft;
  maxRight = curRight;
  maxFront = curFront;
  Serial.println("Done checking path...");
}
void setCourse() { // set direction for travel based on a very basic distance map, simply which direction has the greatest distance to and object - turning right or left? 
    Serial.println("Setting course...");
    if (maxAngle < 90) {turnRight();}
    if (maxAngle > 90) {turnLeft();}
    maxLeft = 0;
    maxRight = 0;
    maxFront = 0;
}
//-------------------------------------------------------------------------------------------------------------------------------------
void checkCourse() { // we're about to hit something so move backwards, stop, find where the empty path is.
  Serial.println("Checking course...");
  moveBackward();
  delay(500);
  moveStop();
  setCourse();
}
//-------------------------------------------------------------------------------------------------------------------------------------
void changePath() {
  Serial.println("Changing path...");
  if (pos < 90) {veerLeft();} // if current pos of sensor is less than 90-degrees, it means the object is on the right hand side so veer left
  if (pos > 90) {veerRight();} // if current pos of sensor is greater than 90-degrees, it means the object is on the left hand side so veer right
}

All values are initialized to 0 and this method, checkPath() is called once from setup() and then from loop().

As I read it:

  1. It first moves the servo-ultrasonic sensor to the left
  2. The for loop runs about 7 times from 144 to 36 in 18 deg increments.
  3. Every loop moves the servo head, keeps the robot moving forward, reads a distance and…
  4. Compares distance to a collision distance (10cm) which if it is < then it calls checkCourse()
  5. Compares distance to a turning distance (20cm) which if it is < then it calls changePath()
  6. Finally it sets some values and here are a couple of questions:
    -----A. Why would it test if curDist > curDist? That would never happen, would it?
    -----B. It sets that angle, 144, to pos and then if pos > 90 && curDist > curLeft (0), then curLeft = curDist? So if 144 > 90 (which it is, meaning its facing left) && curDist (30cm) > curLeft (0), then set curLeft = 30cm. Im not sure why?
  7. Then in setCourse( which is called when we are going to hit something at 10cm), it checks that if maxAngle, which was 144 (left), is > 90, which it is so its facing left, it turns left. Why would it turn left if we just found an object to the left side?

After 300+ posts, you would think that the concept of POST ALL OF YOUR CODE would have sunk in...

    if (curDist > curDist) {maxAngle = pos; Serial.println("curDist");}

What are the odds of one value being greater than itself?

Well I try not to post the whole thing if its not relevant, but ok.

Im asking about those lines because maybe there is something that I missed about them which makes sense after all. Maybe not. I was wondering if anyone had come across it and could tell me what the developer was thinking? :slight_smile:

#include <AFMotor.h>
#include <Servo.h> 
#include <NewPing.h>

#define TRIG_PIN A4 // Pin A4 on the Motor Drive Shield soldered to the ultrasonic sensor
#define ECHO_PIN A5 // Pin A5 on the Motor Drive Shield soldered to the ultrasonic sensor
#define MAX_DISTANCE 200 // sets maximum useable sensor measuring distance to 200cm
#define MAX_SPEED 180 // sets speed of DC traction motors to 180/256 or about 70% of full speed - to get power drain down.
#define MAX_SPEED_OFFSET 10 // this sets offset to allow for differences between the two DC traction motors
#define COLL_DIST 5 // sets distance at which robot stops and reverses to 10cm
#define TURN_DIST COLL_DIST+10 // sets distance at which robot veers away from object (not reverse) to 20cm (10+10)
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); // sets up sensor library to use the correct pins to measure distance.

AF_DCMotor motor1(1, MOTOR12_1KHZ); // create motor #1 using M1 output on Motor Drive Shield, set to 1kHz PWM frequency
AF_DCMotor motor2(4, MOTOR12_1KHZ); // create motor #2, using M2 output, set to 1kHz PWM frequency
Servo myservo;  // create servo object to control a servo 

int pos = 0; // this sets up variables for use in the sketch (code)
  int maxDist = 0;
  int maxAngle = 0;
  int maxRight = 0;
  int maxLeft = 0;
  int maxFront = 0;
int course = 0;
int curDist = 0;
String motorSet = "";
int speedSet = 0;

void setup() {
  Serial.begin(9600);
  myservo.attach(9);  // attaches the servo on pin 9 (SERVO_2 on the Motor Drive Shield to the servo object 
  //myservo.write(0); // Face Right
  delay(2000); // delay for two seconds
  myservo.write(90); // Face Forward
  //delay(2000); // delay for two seconds
  //myservo.write(180); // Face Left
  checkPath(); // run the CheckPath routine to find the best path to begin travel
  motorSet = "FORWARD"; // set the director indicator variable to FORWARD
  moveForward(); // run function to make robot move forward
}

void loop() {
  checkForward(); // check that if the robot is supposed to be moving forward, that the drive motors are set to move forward - this is needed to overcome some issues with only using 4 AA NiMH batteries
  checkPath(); // set ultrasonic sensor to scan for any possible obstacles
}

void checkPath() {
  Serial.println("Checking path...");
  int curLeft = 0;
  int curFront = 0;
  int curRight = 0;
  int curDist = 0;
  myservo.write(144); // set servo to face left 54-degrees from forward
  delay(120); // wait 120milliseconds for servo to reach position
  for(pos = 144; pos >= 36; pos-=18)     // loop to sweep the servo (& sensor) from 144-degrees left to 36-degrees right at 18-degree intervals. 
  {
    myservo.write(pos);  // tell servo to go to position in variable 'pos' 
    delay(90); // wait 90ms for servo to get to position   
    checkForward(); // check the robot is still moving forward
    curDist = readPing(); // get the current distance to any object in front of sensor
    
    // If distances are too close///objects are nearby!!!
    if (curDist < COLL_DIST) { // if the current distance to object is less than the collision distance
      Serial.println("call checkCourse()");
      checkCourse(); // run the checkCourse function
      break; // jump out of this loop
    }
    if (curDist < TURN_DIST) { // if current distance is less than the turn distance
      Serial.println("call changePath()");
      changePath(); // run the changePath function
    }
    
    //ELSE set values
    if (curDist > curDist) {maxAngle = pos; Serial.println("curDist");}
    if (pos > 90 && curDist > curLeft) { curLeft = curDist; Serial.println("curLeft");}
    if (pos == 90 && curDist > curFront) {curFront = curDist; Serial.println("curFront");}
    if (pos < 90 && curDist > curRight) {curRight = curDist; Serial.println("curRight");}
  }
  maxLeft = curLeft;
  maxRight = curRight;
  maxFront = curFront;
  Serial.println("Done checking path...");
}

void setCourse() { // set direction for travel based on a very basic distance map, simply which direction has the greatest distance to and object - turning right or left? 
    Serial.println("Setting course...");
    if (maxAngle < 90) {turnRight();}
    if (maxAngle > 90) {turnLeft();}
    maxLeft = 0;
    maxRight = 0;
    maxFront = 0;
}

void checkCourse() { // we're about to hit something so move backwards, stop, find where the empty path is.
  Serial.println("Checking course...");
  moveBackward();
  delay(500);
  moveStop();
  setCourse();
}

void changePath() {
  Serial.println("Changing path...");
  if (pos < 90) {veerLeft();} // if current pos of sensor is less than 90-degrees, it means the object is on the right hand side so veer left
  if (pos > 90) {veerRight();} // if current pos of sensor is greater than 90-degrees, it means the object is on the left hand side so veer right
}

int readPing() { // read the ultrasonic sensor distance
  Serial.println("Reading ping...");
  delay(70);
  unsigned int uS = sonar.ping();
  int cm = uS/US_ROUNDTRIP_CM;
  Serial.println(cm);
  return cm;
}

void checkForward() { Serial.println("Checking forward..."); if (motorSet=="FORWARD") {motor1.run(FORWARD); motor2.run(FORWARD); } }     // make sure motors are going forward

void checkBackward() { Serial.println("Checking backward..."); if (motorSet=="BACKWARD") {motor1.run(BACKWARD); motor2.run(BACKWARD); } } // make sure motors are going backward

void moveStop() { Serial.println("Move stopping..."); motor1.run(RELEASE); motor2.run(RELEASE);}  // stop the motors.

void moveForward() {
  Serial.println("Moving forward...");
    motorSet = "FORWARD";
    motor1.run(FORWARD);      // turn it on going forward
    motor2.run(FORWARD);      // turn it on going forward
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    motor1.setSpeed(speedSet+MAX_SPEED_OFFSET);
    motor2.setSpeed(speedSet);
    delay(5);
  }
}

void moveBackward() {
  Serial.println("Moving backward...");
    motorSet = "BACKWARD";
    motor1.run(BACKWARD);      // turn it on going forward
    motor2.run(BACKWARD);     // turn it on going forward
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    motor1.setSpeed(speedSet+MAX_SPEED_OFFSET);
    motor2.setSpeed(speedSet);
    delay(5);
  }
}  

void turnRight() {
  Serial.println("Turning right...");
  motorSet = "RIGHT";
  motor1.run(FORWARD);      // turn motor 1 forward
  motor2.run(BACKWARD);     // turn motor 2 backward
  delay(400); // run motors this way for 400ms
  motorSet = "FORWARD";
  motor1.run(FORWARD);      // set both motors back to forward
  motor2.run(FORWARD);      
}  

void turnLeft() {
  Serial.println("Turning left...");
  motorSet = "LEFT";
  motor1.run(BACKWARD);     // turn motor 1 backward
  motor2.run(FORWARD);      // turn motor 2 forward
  delay(400); // run motors this way for 400ms
  motorSet = "FORWARD";
  motor1.run(FORWARD);      // turn it on going forward
  motor2.run(FORWARD);      // turn it on going forward
}  

void veerRight() {Serial.println("Veering right..."); motor2.run(BACKWARD); delay(400); motor2.run(FORWARD);} // veering right? set right motor backwards for 400ms

void veerLeft() {Serial.println("Veering left..."); motor1.run(BACKWARD); delay(400); motor1.run(FORWARD);} // veering left? set left motor backwards for 400ms

I was wondering if anyone had come across it and could tell me what the developer was thinking?

By guessing what some of the functions do?

The checkForward() function clearly shows that the developer was NOT thinking. That function does NOT check anything. It ASSURES that the motors are moving forward.

It would appear that:

    if (curDist > curDist) {maxAngle = pos; Serial.println("curDist");}

is supposed to be:

    if (curDist > maxDist)
    {
        maxDist = curDist;
        maxAngle = pos;
        Serial.println("curDist");
    }

with the curly braces where they belong.

I just noticed the sketch doesn't even use the var maxDist.

Well I guess my first problem was using someone else's sketch instead of writing my own.

The simple logic should be, if distance on left is > than that on the right, go left.

Marciokoko:
Well I try not to post the whole thing if its not relevant, but ok.

If you don’t understand the code then how do you know which parts are relevant for us to understand it?

Always just post the whole code. It makes things much easier for everyone. We’d rather see long code than try to guess what some function does or how some variable is defined.

Good point!

Ok, so please help me understand this code a bit more:

  1. For each of the 7 cycles in the for-loop, take a distance measurement.
  2. If the ping is < COLL_DIST (10cm), then run checkCourse() and break out of the loop because we are about to crash.
  3. If the ping is < TURN_DIST (20cm), then dont break out of the loop but changePath().

Q1: checkCourse() moves the car backwards and stops because collision is close. Then it calls setCourse().

But Im not clear how setCourse() works. setCourse() reads:
A- if maxAngle < 90, then turnRight(). maxAngle was set near the end of the checkPath method where if the curDist pinged was > 0, which it was, it set that left facing angle(144) to maxAngle. This means the sensor didnt find a nearby object on the left so its turning right? This is confusing to me.

Q2: 4. Then come the 3 pos tests where if > 90, which the first one is, and curDist(30) is > curLeft (0), it sets the curDist to curLeft? What for?

Please illuminate me while I try to run through a few more examples.

Thanks


  1. If the ping is < COLL_DIST (10cm), then run checkCourse() and break out of the loop because we are about to crash.

That’s not what it says. If the distance measured is less than the collision distance, call the poorly named function, checkCourse(), which actually changes course, and then break out of the loop, because the orientation of the servo with respect to the direction the robot is now facing is no longer correct. This simply causes the relative direction of the robot and servo to be reset.

The call to setCourse() in checkCourse() decides which way (left or right) had the greatest open distance and turns in that direction.

Ok, please explain a bit more what you mean by:

... call the poorly named function, checkCourse(), which actually changes course, and then break out of the loop, because the orientation of the servo with respect to the direction the robot is now facing is no longer correct. This simply causes the relative direction of the robot and servo to be reset.

I agree it is named incorrectly, but from what I understand:

checkCourse() first reverses, then stops and then sets new course (setCourse()).

But what is this about break out of the loop because the orientation of the servo w/r to robot direction is incorrect?

Think about it. The robot has no clue which way it is actually facing, so it only has a relative coordinate system, with forward, backward, left, and right being what we would consider +y, -y, -x and +x.

It waves the ping sensor around, with 90 on the arc of travel being forward. Now, it knows where obstacles are. It sees one close, so it stops. The relative distances and directions are all still good. It backs up, and the relative distances and directions all changed, because the objects didn't move but the robot did. There's still enough information, though, to choose a direction to turn.

But, once it makes a turn, the relative positions of the obstacles, with respect to robot, have all changed. So, the Arduino breaks out of the loop, and starts the loop over again, to once again determine where everything moved to. Obviously, nothing actually moved, but the relative positions with respect to the robot changed.

If you have a hard time following this. Stand in some position. Make a list of things around you, where they are, and how far they are.

Close your eyes. Step back 3 feet. Turn some direction. Left or right doesn't matter.

Now, that list of stuff around you. Is all the data you wrote down still accurate? No. The couch is still near you, but the distance to it, and the direction to it, has changed. The end table is still near you, but the distance to it, and the direction to it, has changed.

The cat probably ran like hell, but the dog is still there looking at you asking when dinner is.

Hahahaha! I cant wait to run this on the floor and watch the cat/dog! Thanks.

Ok I have 1 more doubt. In the checkPath method, the last 4 lines obviously get called on every servo position…so for every position:

//  Set some parameters (maxDist, maxAngle, maxLeft, maxRight, maxFront)
    if (curDist > maxDist) {
         maxAngle = pos;
         maxDist = curDist;
         Serial.println("maxDist has been set");}
    if (pos > 90 && curDist > curLeft) { 
         curLeft = curDist; 
         Serial.println("facing left and curDist > curLeft?");}
    if (pos == 90 && curDist > curFront) {
         curFront = curDist; 
         Serial.println("facing front and curDist > curFront?");}
    if (pos < 90 && curDist > curRight) {
         curRight = curDist; 
         Serial.println("facing left and curDist > curRight?");}
  }

So we test to see which position has the greatest distance and sets it as maxDist and record the corresponding angle of that maxDist as the maxAngle, which gives us the longest, clearest path available.

Now it checks to see if the pos its facing is left, center or right and if the distance found is > than stored left, right or front max values. This will tell us which is the longest path to the left, to the right and to the front. Presumably we will use this in some complex algorithm to determine if we want to go left, right or front depending on which path is clearer? But I thought thats what maxDist/maxAngle was for? Plus maxLeft, maxRight and maxFront never get used for anything?!

Presumably we will use this in some complex algorithm to determine if we want to go left, right or front depending on which path is clearer?

There is nothing complex about "if the distance to the left is less than the distance to the right, turn right".

Plus maxLeft, maxRight and maxFront never get used for anything?!

So, get rid of them. I didn't supply the code. I don't know why they are there. Presumably, this was scaled down from some application where the greatest distance(s), and the angle to it(them), mattered.

Well, i guess the algorithm could grow to be more complex :-)

Yes I did get rid of them after I noticed they werent getting used. I know you didnt put them there, I was just sharing my train of thought with you is all.

Thanks for all the help!