Obstacle Avoiding Robot Code Help

Can anyone tell me what’s wrong with my code?
It’s an obstacle avoiding robot. Sometimes it turns and moves less than an inch before stopping when nothing is in front of it, sometimes it turns the right way, sometimes the wrong way. Sometimes it doesn’t stop at all and just keeps driving into a wall. Doesn’t seem to have any ryhme or reason behind it. No matter how many times or ways I look at the code nothing seems to work. I’m missing something. Sometimes it responds exactly how I want it to and others it does the complete opposite. Can anyone point out the problem? Thanks.

Side note -
Every time I’ve posted here all I get is unhelpful elitists who get seemingly mad that Arduino isn’t second nature to me. Please, if you can’t point out the EXACT problem and a way to fix it, please just don’t post. I just need to know why it’s not stopping when it should and why it sometimes turns the wrong way. I’m not looking to start an argument. This is not intended to piss on your cheerios. Just tired of the forum elitist “I’m better than you” crap and I’d really appreciate some genuine help.
If you post in regards to the side note out of butthurt, then you’ve missed the point of the side note.

#include <AFMotor.h>
#include <Servo.h>
#include <NewPing.h>
#define TRIG_PIN A8 
#define ECHO_PIN A9 
#define MAX_DISTANCE 100 
#define MAX_SPEED 180 
#define MAX_SPEED_OFFSET 40 
#define COLL_DIST 25 
#define TURN_DIST COLL_DIST 35 
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 

AF_DCMotor motor1(1); 
AF_DCMotor motor2(2); 
Servo servo; 
int led = 15;
int leftDistance, rightDistance;
int curDist;
String motorSet = "";
int speedSet = 0;


void setup() {
  pinMode(led,OUTPUT);
  servo.attach(9);  
  servo.write(90);
  delay(1000); 
}

void loop() {
  digitalWrite(led,HIGH);
  servo.write(90);
  delay(90);
  changePath();
}
//-------------------------------------------------------------------------------------------------------------------------------------

void changePath() {
  moveStop();   
  servo.write(20);  // check distance to the right
  delay(600);
  rightDistance = readPing(); //set right distance
  delay(600);
  servo.write(90);
  delay(600);
  servo.write(160);  // check distace to the left
  delay(600);
  leftDistance = readPing(); //set left distance
  delay(600);
  servo.write(90);
  delay(600);
  compareDistance();
  delay(600);
}


void compareDistance()   // find the longest distance
{
  if (leftDistance>rightDistance) //if left is less obstructed
  {
    turnLeft();
  }
  else if (rightDistance>leftDistance) //if right is less obstructed
  {
    turnRight();
  }
  else //if they are equally obstructed
  {
    turnAround();
  }
}


//-------------------------------------------------------------------------------------------------------------------------------------

int readPing() {
  delay(70);  
  unsigned int uS = sonar.ping();
  int cm = uS/US_ROUNDTRIP_CM;
  return cm;
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveStop() {
  motor1.run(RELEASE); 
  motor2.run(RELEASE);
  delay(800);
}  
//-------------------------------------------------------------------------------------------------------------------------------------
void moveForward() {
  curDist = readPing();

  motorSet = "FORWARD";
  motor1.run(FORWARD);     
  motor2.run(FORWARD);    
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)
  {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
  }
  while (curDist<COLL_DIST){
    moveForward();
  }

  if (curDist<COLL_DIST){
    changePath();
  }
}

//-------------------------------------------------------------------------------------------------------------------------------------
void turnRight() {
  motorSet = "RIGHT";
  motor1.run(FORWARD);     
  motor2.run(BACKWARD);  

  delay(300); 
  moveForward();     
} 
//-------------------------------------------------------------------------------------------------------------------------------------
void turnLeft() {
  motorSet = "LEFT";
  motor1.run(BACKWARD);     
  motor2.run(FORWARD);     
  delay(300);
  moveForward();
} 
//-------------------------------------------------------------------------------------------------------------------------------------
void turnAround() {
  motorSet = "RIGHT";
  motor1.run(FORWARD);      
  motor2.run(BACKWARD);     
  delay(600);
  moveForward();   
}

OK, that kind of behaviour sounds like overflows. Are you sure that all of the library functions you are using always return integers? If any of them are using long integers, then that will overflow your short ints and random stuff will happen.

Put lots of debugging outputs into your code - get it to write the distance out on Serial so you can see what it thought the distance was when it did something wrong.

[elite mode on]
You are using all global variables. This is a bad practise. You are using ‘side effects’ to make your code work. This is also bad. When you name a function, the function should do what its name says it does and it shouldn’t do anything else. This snippet was particularly hard for me to understand:

  while (curDist<COLL_DIST){
    moveForward();
  }

Looking at that, it appears that curDist never gets updated inside the loop so the loop would never finish. This is a standard rookie mistake. But the moveForward() function does update the global variable. It’s just not obvious from the name of the function. Either rename the function to moveForwardAndUpdateDist() or change your code so it is more obvious how that variable gets updated.

This is a better way of doing the exact same thing:

  while (curDist<COLL_DIST){
    moveForward();
    curDist = readPing();
  }

The real advantage of doing it this way isn’t obvious now. In the future, you are going to change your sensors or your motors. If you have the sensor-sensing code mixed in with the motor code then you are going to have to rewrite the entire program. If you have those two activities separated into separate functions then you can probably just rewrite one function and the whole program will immediately use the new sensor or motor.

Try to swap your operation sign

while (curDist<COLL_DIST){
    moveForward();
}

to

while (curDist>COLL_DIST){
    moveForward();  
}

Hi,

Can you please post a copy of your circuit, in CAD or a picture of a hand drawn circuit in jpg, png or pdf?

How are you powering the motors and arduino?
What model aduino is it?
A picture of your project would help as well.

Tom...... :slight_smile:

Wow thanks for the quick and detailed responses. Much appreciated.

When you name a function, the function should do what its name says it does and it shouldn’t do anything else.

So does that mean, for example, in my turnLeft() loop, all I need is to tell it to turn left? I don’t need to tell it to go back to moving forward after it has turned?

This snippet was particularly hard for me to understand: while (curDist<COLL_DIST){
moveForward();
}

I used while because I was trying to make the bot stay in motion unless it detected an object. At first I was using :
if (curDist<COLL_DIST){ continue; } , but it kept saying continue; wasn’t in a loop. Couldn’t figure it out so I tried while (curDist<COLL_DIST){ continue; } but it wasn’t getting the desired result either so I switched continue; to moveForward();

Either rename the function to moveForwardAndUpdateDist() or change your code so it is more obvious how that variable gets updated.

Ok that makes sense. I’ll try it out. Thanks :wink:

Try to swap your operation sign

Oh wow I think you’re right. It’s backwards isn’t it? I’ll try that.

Can you please post a copy of your circuit, in CAD or a picture of a hand drawn circuit in jpg, png or pdf?

How are you powering the motors and arduino?
What model aduino is it?
A picture of your project would help as well.

Well, I don’t have CAD but I took a few (crappy) pictures. And I can try to explain the wiring.
It’s an Arduino Mega 2560 with a prototype shield on top of it and a motor shield on top of that. I have a 6 AA battery pack that leads into the external power slot on the motor shield. Left motor is connected to M1, right to M2. The servo is attached to a servo pin, which I believe is pin 9. The LED has a 220 ohm resistor attached to it. A8 and A9 to the ultrasonic sensor.

Well…I took pictures but I can’t figure out how to upload them without first having to put them online somewhere and use the url.

Thanks for all the help everyone.

Hi, use REPLY not QUICK REPLY, you will have in the lower left hand corner , ATTACHMENTS and other OPTIONS.
This will let you add your files as attachments.
Please don't load your files off site, makes trying to follow the thread difficult.

Tom..... :slight_smile:

Oh nice. Thanks.
Not very good pictures. All I have is a webcam on a laptop so it’s hard to get a good light and angle.

Snapshot_20141214.jpg

Snapshot_20141214_1.jpg

Snapshot_20141214_2.jpg

Snapshot_20141214_3.jpg

Snapshot_20141214_4.jpg

kjhkjh.jpg

Recursive function without obviously visible way to exit from it???
But it updates curDist…

//-------------------------------------------------------------------------------------------------------------------------------------
void moveForward() {
  curDist = readPing();

  motorSet = "FORWARD";
  motor1.run(FORWARD);     
  motor2.run(FORWARD);    
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)
  {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
  }


while (curDist<COLL_DIST){
    moveForward();
  }




  if (curDist<COLL_DIST){
    changePath();
  }
}

Moderator edit: removed extraneous highlighting tags from inside code tags. {sigh}

Recursive function without obviously visible way to exit from it???
But it updates curDist…

Yeah that’s the problem. Can’t figure out a way around it. In my mind this code was suppose to say if current distance is greater than the colliding distance, keep moving forward. If current distance is less than colliding distance, change path. I’m obviously not doing it right. I overlooked the error of both saying curDist<COLL_DIST. I fixed that. But it’s still not performing like I’d expect.

I thought telling it “if (curDist<COLL_DIST){ changePath();}” would tell it to recalculate the distance and choose the right path.

The order should be -
moveForward() until blocked
changePath()
compareDistance()
turnLeft()/ turnRight()/ turnAround() then resume moveForward() until blocked.

Then it should just repeat the process.
But the decisions it makes seem random.
What would be an obviously visible way to exit from the recursive function?

Few suggestions - change moveForward to accelerateForward , remove commented out code and modify rest of the code accordingly.

To keep checking for obstacles try


accelerateForward();

while ( readPing() > COLL_DIST)
{
// stay course and do whatever … or nothing

}

// now do correction

I would recommend to add some return values, true / false would be better than flying blind, to your functions indicating HARDWARE success.
I think you would be better off doing ALL functions in setup where YOU control the code flow instead of relying on loop itself.

void moveForward() {
  //curDist = readPing();

  motorSet = "FORWARD";
  motor1.run(FORWARD);     
  motor2.run(FORWARD);    
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)
  {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet+MAX_SPEED_OFFSET); what is this MAX_SPEED_OFFSET for here ???
  }

/* 
  [b]while (curDist<COLL_DIST){  OK but in wrong place / sequence [/b]
    moveForward();                   [b] wrong - you continually accelerate from 0 to max speed [/b]
  }

  if (curDist<COLL_DIST){
    changePath();
  }
*/

}

change moveForward to accelerateForward

Isn’t that just the title of the loop? I thought it could be named anything. Not arguing just curious.

I would recommend to add some return values, true / false would be better than flying blind, to your functions indicating HARDWARE success.
I think you would be better off doing ALL functions in setup where YOU control the code flow instead of relying on loop itself.

So you’re saying instead of having a bunch of different void () processes, put them all in setup?
I’ll give it a shot but I don’t fully understand what you mean. And how do I use true/false to determine which way to turn? Return values? Sorry I don’t really understand what you mean by this. I’ll look it up and play around with it. But as of right now I don’t really understand what you mean. I have a vague idea of what you COULD mean lol.

what is this MAX_SPEED_OFFSET for here ???

Honestly I’m not sure. I saw it on another code I was using to learn and it doesn’t seem to work as well without it. I’ll try taking it out and implementing your suggestions. At least the way I understand it lol.

Thanks for your help.

EDIT–
Here’s my new code.
I still have the old one saved. It’s a tad different but still only inches. It makes a decission, then moves about an inch forward before stopping. It DOES respond if I put my hand in front of it but it’s not suppose to stop if nothing is in front of it. Is it maybe because of the delays? I didn’t know how to use true/false for this so I just put everything in void loop except readPing().

#include <AFMotor.h>
#include <Servo.h>
#include <NewPing.h>
#define TRIG_PIN A8 
#define ECHO_PIN A9 
#define MAX_DISTANCE 100 
#define MAX_SPEED 180 
#define MAX_SPEED_OFFSET 40 
#define COLL_DIST 20 
#define TURN_DIST COLL_DIST +15 
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 

AF_DCMotor motor1(1); 
AF_DCMotor motor2(2); 
Servo servo; 
int led = 15;
int leftDistance, rightDistance;
String motorSet = "";
int speedSet = 0;


void setup() {
  pinMode(led,OUTPUT);
  servo.attach(9);  
  servo.write(90);
  delay(1000); 
}

void loop() {
  digitalWrite(led,HIGH);
  servo.write(90);
  delay(90);
  motor1.run(RELEASE); 
  motor2.run(RELEASE);
  delay(800); 
  servo.write(20);  // check distance to the right
  delay(600);
  rightDistance = readPing(); //set right distance
  delay(600);
  servo.write(160);  // check distace to the left
  delay(600);
  leftDistance = readPing(); //set left distance
  delay(600);
  servo.write(90);
  delay(600);
  if (leftDistance>rightDistance) //if left is less obstructed
  {
    motorSet = "LEFT";
    motor1.run(BACKWARD);     
    motor2.run(FORWARD);     
    delay(300);
    motorSet = "FORWARD";
    motor1.run(FORWARD);     
    motor2.run(FORWARD);    
    for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)
    {
      motor1.setSpeed(speedSet);
      motor2.setSpeed(speedSet);
    }
    while (readPing()> COLL_DIST){
      continue;
    }

    if (readPing()<COLL_DIST){
      void loop();
    }
  }
  else if (rightDistance>leftDistance) //if right is less obstructed
  {
    motorSet = "RIGHT";
    motor1.run(FORWARD);     
    motor2.run(BACKWARD);  

    delay(300); 
    motorSet = "FORWARD";
    motor1.run(FORWARD);     
    motor2.run(FORWARD);    
    for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)
    {
      motor1.setSpeed(speedSet);
      motor2.setSpeed(speedSet);
    }
    while (readPing()> COLL_DIST){
      continue;
    }

    if (readPing()<COLL_DIST){
      void loop();
    }
  }
  else //if they are equally obstructed
  {
    motorSet = "RIGHT";
    motor1.run(FORWARD);      
    motor2.run(BACKWARD);     
    delay(600);
    motorSet = "FORWARD";
    motor1.run(FORWARD);     
    motor2.run(FORWARD);    
    for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)
    {
      motor1.setSpeed(speedSet);
      motor2.setSpeed(speedSet);
    }

    while (readPing()> COLL_DIST){
      continue;
    }

    if (readPing()<COLL_DIST){
      void loop();
    }
  }
}

//-------------------------------------------------------------------------------------------------------------------------------------

int readPing() {
  delay(70);  
  unsigned int uS = sonar.ping();
  int cm = uS/US_ROUNDTRIP_CM;
  return cm;
}

Looking at terms and trying several different things. Nothing is working. Same results nearly every time. It seems the sensor is working right, but when the robot moves it moves for an inch and then stops. Randomly it'll just take off forward like I want it to about 9 out of 10 times. Like I said there doesn't seem to be any ryhme or reason. Looked at a few other codes but they're so different from mine that they don't help too much. Batteries are full. No idea what I'm doing wrong. The last code I posted was worse than the first.

if (readPing()<COLL_DIST){
      void loop();
    }

That doesn’t look the best place to put a function prototype.

Why don’t you get your sketch to tell you what it is doing with some prints?

Why don't you get your sketch to tell you what it is doing with some prints?

How do I do that? I'm assuming you mean Serial.print?
But how do I make it tell me what it's doing? I don't follow.

Floyd555:
How do I do that? I'm assuming you mean Serial.print?
But how do I make it tell me what it's doing? I don't follow.

You haven't discovered the serial monitor? It's on the Tools menu. Open the AnalogReadSerial example (in the File menu) and load it onto your Arduino. Then start the serial monitor.

The overall trend of the code seems to be piling more crap into the loop() function. This needs to be better organised. Read the first 3 posts in Planning and Implementing and Arduino Program.

Sorry , I forgot to add.
You are already using function readPing return value so you could compare the values before and after a turn , as an example, and if they are different return boolean true. So the function definition would look something
like this:
Not too precision / scientific, but one method to verify hardware did something.

boolean TurnRobie( void)
{
int iStart Distance,iEndDistance;
iStartDistance = readPing();
.... turning

iEndDistance = readPing();

if(iStartDistance != iEndDistance)
return true;
return false;
}

Here’s what I have. I’ve tried to include most of what you’ve all told me.

#include <AFMotor.h>
#include <Servo.h>
#include <NewPing.h>
#define TRIG_PIN A8 
#define ECHO_PIN A9 
#define MAX_SPEED 180  
#define COLL_DIST 40 
#define TURN_DIST COLL_DIST +20 
NewPing sonar(TRIG_PIN, ECHO_PIN); 

AF_DCMotor motor1(1); 
AF_DCMotor motor2(2); 
Servo servo; 
int led = 15;
int leftDistance, rightDistance;
int curDist;
String motorSet = "";
int speedSet = 0;


void setup() {
  Serial.begin(9600);
  pinMode(led,OUTPUT);
  servo.attach(9);  
  servo.write(90);
  delay(500); 
  Serial.println("Start Forward");
  accelerateForward();
  
}

void loop() {
  Serial.println(readPing());
  digitalWrite(led,HIGH);
  if (readPing()<COLL_DIST){
    Serial.println("Collision Detected");
    moveStop();
    changePath();
    compareDistance();
    accelerateForward();
  }
  
    
   
}


void changePath() { 
  servo.write(20);  
  delay(600);
  rightDistance = readPing(); 
  delay(600);
  servo.write(90);
  delay(600);
  servo.write(160);  
  delay(600);
  leftDistance = readPing();
  delay(600);
  servo.write(90);
  delay(600);
}


void compareDistance()  
{
  if (leftDistance>rightDistance) 
  {
    turnLeft();
  }
  else if (rightDistance>leftDistance)
  {
    turnRight();
  }
  else 
  {
    turnAround();
  }
}



int readPing() {
  delay(70);  
  unsigned int uS = sonar.ping();
  int cm = uS/US_ROUNDTRIP_CM;
  return cm;
  Serial.print(cm);
  Serial.println(" cm");
}

void moveStop() {
  Serial.println("Stopped");
  motor1.run(RELEASE); 
  motor2.run(RELEASE);
  delay(800);
}  

void accelerateForward() {
  Serial.println("Start Forward");
  motorSet = "FORWARD";
  motor1.run(FORWARD);     
  motor2.run(FORWARD);    
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)
  {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet);
  }
}


void turnRight() {
  Serial.println("Turn Right");
  motorSet = "RIGHT";
  motor1.run(FORWARD);     
  motor2.run(BACKWARD);  
  delay(800);      
} 

void turnLeft() {
  Serial.println("Turn Left");
  motorSet = "LEFT";
  motor1.run(BACKWARD);     
  motor2.run(FORWARD);     
  delay(800);
} 

void turnAround() {
  Serial.println("Turn Around");
  motorSet = "RIGHT";
  motor1.run(FORWARD);      
  motor2.run(BACKWARD);     
  delay(1600);
}

It’s still not quite there. But at least it IS avoiding obstacles. It hasn’t hit any walls.
Thanks for all the info so far. It’s been a great help. I’m sorry if I seem slow on the uptake.

I already posted about this but I couldn’t find the topic. Looked through about 20 pages. So sorry.

I need to know what the problem is here. It’s driving me crazy >_<
I’ve had several suggestions already but they either just didn’t work or I didn’t understand it.

(That’s another thing. Can anyone recommend a good tutorial video or place where I can learn arduino from the ground up? The learning tab makes no sense to me. It explains it’s terms using it’s terms. I need to see it and play with it to understand it.)

The biggest problem is that the right motor sometimes does not respond. It just sits there. No where in my code other than moveStop() does it say motor.run(RELEASE);. Sometimes it works fine and others it just doesn’t move. The left motor always responds. Is it possible this is a result of my horrible soldering skills? Is it not receiving enough power? (6 AA’s) Or is it the code?

The other problem is that when I checked out what the code is doing with Serial Monitor, I noticed that it never turns left or turns right. It always skips straight to turn around. Turn around should be the last option. Before I went messing with the code to fix the problem with the motor, it turned left and right, usually how I expected. But now it just moves forward until it sees an object, then it tries to back up then move forward. I have no idea what’s goin on. I would like it to -

move forward until collision detected
stop
scan directions
choose path
turn to the appropriate direction
continue forward

As always, I appreciate your help. Sorry again for reposting the same topic. I know I’m gonna get yelled at for that but I couldn’t find it.

#include <AFMotor.h>
#include <Servo.h>
#include <NewPing.h>
#define TRIG_PIN A8 
#define ECHO_PIN A9 
#define MAX_SPEED 180  
#define COLL_DIST 40 
#define TURN_DIST COLL_DIST +20 
NewPing sonar(TRIG_PIN, ECHO_PIN); 

AF_DCMotor motor1(1); 
AF_DCMotor motor2(2); 
Servo servo; 
int led = 15;
int leftDistance, rightDistance;
int curDist;
String motorSet = "";
int speedSet = 0;


void setup() {
  Serial.begin(9600);
  pinMode(led,OUTPUT);
  servo.attach(9);  
  servo.write(90);
  delay(500); 
  Serial.println("Start Forward");
  accelerateForward();
  
}

void loop() {
  Serial.println(readPing());
  digitalWrite(led,HIGH);
  if (readPing()<COLL_DIST){
    Serial.println("Collision Detected");
    moveStop();
    changePath();
    compareDistance();
    accelerateForward();
  }
  
    
   
}


void changePath() { 
  servo.write(20);  // check distance to the right
  delay(600);
  rightDistance = readPing(); //set right distance
  delay(600);
  servo.write(90);
  delay(600);
  servo.write(160);  // check distace to the left
  delay(600);
  leftDistance = readPing(); //set left distance
  delay(600);
  servo.write(90);
  delay(600);
}


void compareDistance()  
{
  if (leftDistance>rightDistance) 
  {
    turnLeft();
  }
  else if (rightDistance>leftDistance)
  {
    turnRight();
  }
  else 
  {
    turnAround();
  }
}



int readPing() {
  delay(70);  
  unsigned int uS = sonar.ping();
  int cm = uS/US_ROUNDTRIP_CM;
  Serial.print(cm);
  Serial.println(" cm");
}

void moveStop() {
  Serial.println("Stopped");
  motor1.run(RELEASE); 
  motor2.run(RELEASE);
  delay(400);
}  

void accelerateForward() {
  Serial.println("Start Forward");
  motorSet = "FORWARD";
  motor1.run(FORWARD);     
  motor2.run(FORWARD);    
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)
  {
    motor1.setSpeed(speedSet);
    motor2.setSpeed(speedSet);
  }
}


void turnRight() {
  Serial.println("Turn Right");
  motorSet = "RIGHT";
  motor1.run(FORWARD);     
  motor2.run(BACKWARD);  
  delay(500);      
} 

void turnLeft() {
  Serial.println("Turn Left");
  motorSet = "LEFT";
  motor1.run(BACKWARD);     
  motor2.run(FORWARD);     
  delay(500);
} 

void turnAround() {
  Serial.println("Turn Around");
  motorSet = "RIGHT";
  motor1.run(FORWARD);      
  motor2.run(BACKWARD);     
  delay(800);
}

Again…I don’t understand Arduino terms. Please try to keep it in lamest terms, or at least explain the terms you use. Greatful for your help. Thanks.

check code properly

int readPing() {
  delay(70);  
  unsigned int uS = sonar.ping();
  int cm = uS/US_ROUNDTRIP_CM;
  Serial.print(cm);
  Serial.println(" cm");
}

here you are returning nothing. return(cm) value.please update & check.

also put Serial monitor window. I could not able to get which part is not working

Floyd555:
I already posted about this but I couldn't find the topic. Looked through about 20 pages. So sorry.

Click on your own profile. Then click on "posts". This will take you to the list of every post you've made, which should quickly get you to the right area, even if it's slipped off the front page of the main forum.