Let's Make Robots! | RobotShop

Ultrasonic Collision Avoidance Arduino and Raspberry Pi

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

[code]/*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);

}

[/code]

 

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

Oh sorry about posting the code, I couldn't remember how to embedd it :-(

 

Fixed it, I had coded the long ping() function incorrectly should have been:

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;

}

All working now, here is the video