Ultrasonic Collision Avoidance

Hi;

I am building and testing a telepresence wheeled rover bot that has two methods of control:

1. Web Interface (Manual Mode): This consists of a control panel on a web page served using WebIOPi on a Raspberry Pi which, when activated, sends commands to an Arduino Nano AT Mega328P via a two wire i2c interface (Forward, Back etc).

2. Autonomous Mode: If no data is received by the Nano over the i2c interface after a specified time frame the Nano switches to ‘Autonomous Mode’ and calls an auto navigate function. This causes the rover to drive around autonomously navigating its environment using an Ultrasonic Sensor (HC-SR04) mounted on a servo.

The wheels are driven by 2 x DC Motors connected to an L298N H Bridge.

The web interface was working fine and using Serial Debugging I could see that the Autonomous Mode was being called however the rover would not move.

After trying to debug my original code I decided to modify and upload a new program consisting of only autonomous ultrasonic obstacle avoidance. The purpose of this was to get a working auto navigating code working and then merge this with the manual control element.

Attached is my modified autonomous collision avoidance code, when it is run the ultrasonic servo centres and only turns one way, the robot then backs up and spins one way, it then just keeps repeating this movement cycle.

I have successfully used obstacle avoidance before using the Nano and HC-SR04 but in the most recent code attached I have tried to use the NewPing Library which I have never used before, according to the project that I magpied this code from an HC-SR04 was used?

I have also had to modify the veer left and veer right as my PWM is not working.

Could any of you kind folk take a look at my code and see if you can spot any errors that would prevent normal navigation? If the code is OK then i’ll need to check my wiring and the sensor?

Many thanks

_20151114_Hammerstein_Avoidance_Test_v1.ino (5.54 KB)

/*MAEP 2.0 Navigation
by Noah Moroze, aka GeneralGeek
This code has been released under a Attribution-NonCommercial-ShareAlike license, more info at http://creativecommons.org/licenses/
PING))) code by David A. Mellis and Tom Igoe http://www.arduino.cc/en/Tutorial/Ping
*/

//Adapted by Bill Harvey for project Hammerstein
#include <Wire.h> //include i2c library
#include <Servo.h> //include Servo library
#define SLAVE_ADDRESS 0x04 //Set Arduino as slave and define the Serial bus address

//Setup Ultrasonic Sensor Pins
#define trigPin 13
#define echoPin 12

//Setup motor controller pins for L298N
//Right motor
int enA = 11;
int in1 = 8;
int in2 = 7;

//Left Motor
int enB = 10;
int in3 = 6;
int in4 = 5;

//const int RForward = 0; 
//const int RBackward = 180; 
//const int LForward = RBackward; 
//const int LBackward = RForward; 
//const int RNeutral = 90; 
//const int LNeutral = 90; //constants for motor speed
//const int pingPin = 7;
//const int irPin = 0;  //Sharp infrared sensor pin
const int dangerThresh = 10; //threshold for obstacles (in cm)
int leftDistance, rightDistance; //distances on either side

Servo sensor_servo; //Give servo a logical name 
Servo leftMotor;
Servo rightMotor; //declare motors
long duration; //time it takes to recieve PING))) signal
long distance; //Added by Bill Harvey

void setup()
{
//  rightMotor.attach(11);
//  leftMotor.attach(10);
//  sensor_servo.attach(6); //attach motors to proper pins
//  sensor_servo.write(90); //set PING))) pan to center
  delay(1000); // Sets a short delay after switching on
  Serial.begin(9600); // Start serial for output debugging
  Wire.begin(SLAVE_ADDRESS); // Initialise i2c as slave
//  Wire.onReceive(receiveData); //Define callbacks for i2c communication
//  Wire.onRequest(sendData); //Define callbacks for i2c communication
  sensor_servo.attach(2); //Attaches the servo to pin 2 of the Nano
  sensor_servo.write(90); //Set the servo to face front
  //Set modes for distance sensor pins 
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  //Set modes for motor controller pins
  pinMode(enA, OUTPUT);   
  pinMode(enB, OUTPUT);   
  pinMode(in1, OUTPUT);   
  pinMode(in2, OUTPUT);   
  pinMode(in3, OUTPUT);   
  pinMode(in4, OUTPUT);
  // Message to serial monitor declaring that robot is ready
  Serial.println("Setup Complete");
  Serial.println("Hammerstein Ready!");
  
}

void loop()
{
  int distanceFwd = ping();
  if (distanceFwd>dangerThresh) //if path is clear
  {
    Forward(); //move forward
  }
  else //if path is blocked
  {
    Stop(); 
    sensor_servo.write(0); 
    delay(500);
    rightDistance = ping(); //scan to the right
    delay(500);
    sensor_servo.write(180);
    delay(700);
    leftDistance = ping(); //scan to the left
    delay(500);
    sensor_servo.write(90); //return to center
    delay(100);
    compareDistance();
  }
}
  
void compareDistance()
{
  if (leftDistance>rightDistance) //if left is less obstructed 
  {
    Left(); //turn left
    delay(500); 
  }
  else if (rightDistance>leftDistance) //if right is less obstructed
  {
    Right(); //turn right
    delay(500);
  }
   else //if they are equally obstructed
  {
    Left(); //turn 180 degrees
    delay(1000);
  }
}

long ping()
{
  // Send out PING))) signal pulse
 //Trigger the sensor to send out a ping
 digitalWrite(trigPin, HIGH);
 delayMicroseconds(1000);
 digitalWrite(trigPin, LOW);
 duration = pulseIn(echoPin, HIGH);
 distance = (duration/2) /29.1;
 Serial.println(ping());
}

void Forward() //This function tells the robot to go forward 
{
  //for (int i=0; i <= 50; i++){
  Serial.println("");
  Serial.println("Moving forward");
  // turn on left motor   
  digitalWrite(in1, HIGH);   
  digitalWrite(in2, LOW);   
  // set speed out of possible range 0~255
  analogWrite(enA, 255);   
  // turn on right motor   
  digitalWrite(in3, LOW);   
  digitalWrite(in4, HIGH);   
  // set speed out of possible range 0~255   
  analogWrite(enB, 255);   
  delay(100);
  //}
  //Stop();
}

void Backward() //This function tells the robot to move backward
{
  //for (int i=0; i <= 50; i++){
  Serial.println("");
  Serial.println("Moving backward");
  // turn on left motor   
  digitalWrite(in1, LOW);   
  digitalWrite(in2, HIGH);   
  // set speed out of possible range 0~255
  analogWrite(enA, 255);   
  // turn on right motor   
  digitalWrite(in3, HIGH);   
  digitalWrite(in4, LOW);   
  // set speed out of possible range 0~255   
  analogWrite(enB, 255);   
  delay(100);
  //}
  //Stop();
}

void Left() //This function tells the robot to turn left
{
  //for (int i=0; i <= 50; i++){
  Serial.println("");
  Serial.println("Moving left");
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH); 
  //  set speed out of possible range 0~255
  analogWrite(enA, 240); 
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  //set speed out of possible range 0~255   
  analogWrite(enB, 255);   
  delay(100);
  //   }
  //   Stop(); 
}

void Right() //This function tells the robot to turn right
{
//   for (int i=0; i <= 50; i++){ 
   Serial.println("");
   Serial.println("Moving right");
   digitalWrite(in1, HIGH);
   digitalWrite(in2, LOW);
//  set speed out of possible range 0~255
   analogWrite(enA, 230); 
   digitalWrite(in3, HIGH); //Was High
   digitalWrite(in4, LOW);
   analogWrite(enB, 255);
   delay(100);
//    }
//     Stop();
}

void Stop() //This function tells the robot to stop moving
{
  Serial.println("");
  Serial.println("Stopping");
// now turn off motors   
  digitalWrite(in1, LOW);   
  digitalWrite(in2, LOW);
  //analogWrite(enA, 0);  
  digitalWrite(in3, LOW);   
  digitalWrite(in4, LOW);
  //analogWrite(enB, 0);
}
long ping()

You promised the compiler you were going to return a long, and you didn't.
Shame on you.
And then you went recursive.
Double shame on you

Ah I see..............

Not sure what you meant by cursive?

But here's what I think I understand.........

I start the function ping() by declaring it as a long variable ? but do not finish off the function by returning anything to store in the variable ping()?

I also declare a variable distance at the start of the programme and although I use it to store the result of (duration/2) /29.1 I do not use it anywhere??

So I have amended long ping() as follows:

long ping()
{
  // Send out PING))) signal pulse
 //Trigger the sensor to send out a ping
 digitalWrite(trigPin, HIGH);
 delayMicroseconds(1000);
 digitalWrite(trigPin, LOW);
 //Get duration it takes to receive echo
 duration = pulseIn(echoPin, HIGH);
 //Convert duration to into a distance in cm and return it
 return duration / 29 /2; 
 Serial.println(ping());
}

I didn't write "cursive", I wrote "recursive"

Sorry my BAD.

Oh and the Gnd connection at the Sensor had dislodged itself :slight_smile:

All working brilliantly now, many thanks

 return duration / 29 /2;
 Serial.println(ping());
}

There's very litlle point putting anything after a return.

Got that thanks.

I have amended the code and have removed the last print statement, the debugging print statements are now added elsewhere.

The ping() function now looks like this:

long ping()
{
  // Send out PING))) signal pulse
 //Trigger the sensor to send out a ping
 digitalWrite(trigPin, HIGH);
 delayMicroseconds(1000);
 digitalWrite(trigPin, LOW);
 //Get duration it takes to receive echo
 duration = pulseIn(echoPin, HIGH);
 //Convert duration to into a distance in cm and return it
 return duration / 29 /2;
}

And the working (well the avoidance bit) can be seen here.