In this tutorial, we’ll learn how to create a ROS package. Software in ROS is organized into packages. Each package might contain a mixture of code (e.g. ROS nodes), data, libraries, images, documentation, etc. Every program you write in ROS will need to be inside a package.
Each ROS package is designed to produce some functionality. ROS packages promote code reuse.
ROS packages promote software reuse. The goal of a ROS package is to be large enough to provide a specific useful functionality, but not so large and complicated that nobody wants to reuse it for their own project.
ROS packages are organized as follows:
launch folder: Contains launch files
src folder: Contains the source code (C++, Python)
CMakeLists.txt: List of cmake rules for compilation
package_name is the name of the package you want to make, and depend1, depend2, depend3, etc., are the names of other ROS packages that your package depends on.
Now, open a new terminal window, and move to the source directory of the workspace you created. If you don’t already have a workspace set up, check out this tutorial.
cd ~/catkin_ws/src
Create a ROS package named ‘hello_world’ inside the src folder of the catkin workspace (i.e. catkin_ws). This package will have three dependencies (i.e. libraries the package depends on in order for the code inside the package to run properly): roscpp (the ROS library for C++), rospy (the ROS library for Python), and std_msgs (common data types that have been predefined in ROS … “standard messages”).
Type dir , and you will see that we now have a package named hello_world inside the source folder of the workspace.
Change to the hello_world package.
cd hello_world
Note, we could have also used the ROS command to move to the package.
roscd hello_world
Type:
dir
You can see that we have four files:
CMakeLists.txt: This text file contains the commands to compile the programs that you write in ROS. It also has the commands to convert your source code and other files into an executable (i.e. the code that your computer can run).
include: Contains package header files. You might remember when we wrote header files in the C++ tutorial…well this folder is where your header files would be stored.
src: This folder will contain the C++ source code. If you are doing a project in Python, you can create a new folder named scripts that will contain Python code. To create this new folder type:
mkdir scripts
package.xml: This is an Extensible Markup Language (XML) file. An XML file is a text file written in a language (called XML) that is easy to read by both humans and computers. An XML file does not do anything other than store information in a structured way.
Our package.xml file contains information about the hello_world package. You can see what is inside it by typing:
gedit package.xml
That’s it! You’re all done.
Note that if you ever want to go straight to a package without typing the full path, ROS has a command to enable you to do that. In the terminal window, type the following:
roscd <package_name>
For example, if we want to go straight to the hello_world package, we would open a new terminal window and type the following command:
roscd hello_world
Compiling a Package in ROS
When you create a new package in ROS, you need to compile it in order for it to be able to run. The command to compile a package in ROS is as follows:
catkin_make
The command above will compile all the stuff inside the src folder of the package.
You must be inside your catkin_ws directory in order for the catkin_make command to work. If you are outside the catkin_ws directory, catkin_make won’t work.
To get to your catkin_ws directory, you can type this command anywhere inside your terminal:
roscd
cd ..
Then to compile all the packages inside the catkin workspace, you type:
catkin_make
If you just want to compile specific packages (in cases where your project is really big), you type this command:
catkin_make --only-pkg-with-deps
For example, if you have a package named hello_world, you can compile just that package:
In this tutorial, we will launch a virtual robot called TurtleBot3. TurtleBot3 is a low-cost, personal robot kit with open-source software. You can read more about TurtleBot here at the ROS website.
The official instructions for launching the TurtleBot3 simulation are at this link, but we’ll walk through everything below.
Below is a demo of what you will create in this tutorial. You will get experience with SLAM (Simultaneous localization and mapping) and autonomous navigation.
TurtleBot3 is designed to run using just ROS and Ubuntu. It is a popular robot for research and educational purposes.
roslaunch is the command in ROS that enables us to launch a program. The syntax is as follows:
roslaunch <name_of_package> <name_of_launch_file>
The first parameter is the name of the package. The second parameter is the name of the launch file that is inside the package.
What is a Package?
ROS packages are the way software is organized in ROS. They are the smallest thing you can build in ROS.
A package is a directory that contains all of the files, programs, libraries, and datasets needed to provide some useful functionality. ROS packages promote software reuse. Every program that you write in ROS will need to be inside a package.
The goal of a ROS package is to be large enough to be useful but not so large and complicated that nobody wants to reuse it for their own project.
ROS packages are organized as follows:
launch folder: Contains launch files
src folder: Contains the source code (C++, Python)
CMakeLists.txt: List of cmake rules for compilation
package.xml: Package information and dependencies
To go straight to a ROS package from a terminal window, the syntax is as follows:
roscd <name_of_package>
For example, to go to the turtlebot3_teleop package, type in a new terminal window:
roscd turtlebot3_teleop
If you type the following command, you can see what is inside there:
ls
What is a Launch File?
From within the turtlebot3_teleop package, move inside the launch file.
cd launch
Let’s take a look inside it.
gedit turtlebot3_teleop_key.launch
All launch files start off with the <launch> tag and end with the </launch> tag. Inside these tags, you have the <node> tag that contains the following parameters:
pkg=”package_name”: This is the name of the package that has the code we want ROS to execute.
type=”python_file_name.py”: This is the name of the program we’d like to execute.
name=”node_name”: This is the name of the ROS node we want to launch our program.
output=”type_of_output”: Where you will print the output of the program.
Continuing On…
OK, now that we know what packages are, let’s continue on.
Click the terminal window and use the keys below to control the movement of your TurtleBot (e.g. press W key to move forward, X key to move backward and S to stop).
And remember, use the keyboard to move the robot around.
How to Change the Simulation Environment for TurtleBot3
Let’s look at our TurtleBot3 in a different environment. This environment is often used for testing SLAM and navigation algorithms. Simultaneous localization and mapping (SLAM) concerns the problem of a robot building or updating a map of an unknown environment while simultaneously keeping track its location in that environment.
Autonomous Navigation and Obstacle Avoidance With TurtleBot3
Now let’s implement obstacle avoidance for the TurtleBot3 robot. The goal is to have TurtleBot3 autonomously navigate around a room and avoid colliding into objects.
Let’s take a look at how we can simulate SLAM with TurtleBot3. As a refresher, Simultaneous localization and mapping (SLAM) concerns the problem of a robot building or updating a map of an unknown environment while simultaneously keeping track its location in that environment.
If you’re tired of working with ROS using a simulated robot, check out this tutorial on how to build a real, physical autonomous, obstacle-avoiding wheeled robot from scratch using ROS.
You have the Arduino IDE (Integrated Development Environment) installed on either your PC (Windows, MacOS, or Linux).
If you have experience building a basic wheeled robot using Arduino, you will find this tutorial easier to follow. If you don’t have that experience, don’t worry. I’ll explain everything as we go.
Grab the Arduino and attach it just behind the 9V battery on the front of the robot (Ignore all those wires in the photo for now. You’ll get to those in a bit).
Breadboard (at left) and Arduino (in center) with a protective casing over it
You can use the Velcro squares to make sure the Arduino stays put. Make sure the black power input on the Arduino can easily be reached by the 9V battery connector (don’t plug anything in yet).
Attach the 400-point solderless breadboard just behind the Arduino using some Velcro squares.
Now that the robot has its brain (Arduino mounted on the back of the robot) and a body, it needs a “nervous system,” communication lines that enable the brain to transmit signals to and from different parts of its body. In the context of this project, those communication lines are the wires that we need to connect between the different parts of the robot we’re building.
Design the Circuit Diagram
Before we connect any wires or components, it is good practice to design the circuit diagram. For all my Arduino projects, I use a software called Fritzing, but you can even plan out your circuits by hand.
To test your motor connection, you will need to connect the 9V battery (the one at the back of the robot sandwiched between the upper and lower base pieces) to the 400-point solderless breadboard as shown in the circuit diagram.
I used alligator clips and some small pieces of male-to-male wire to connect the + and – terminals of the 9V battery to the red (positive) and blue (ground) rails of the solderless breadboard.
Here is the code to test the motors. Upload it to your Arduino from your computer.
/**
* Addison Sears-Collins
* March 21, 2020
* This code is used to test the DC motors
* with the Dual H-Bridge Motor Driver (L293D)
**/
/*---------------------------Definitions-------------------------------------*/
//Define pins for Motor A
#define ENABLE_A 5
#define MOTOR_A1 6
#define MOTOR_A2 4
// Define pins for Motor B
#define ENABLE_B 8
#define MOTOR_B1 7
#define MOTOR_B2 9
/*---------------------------Helper-Function Prototypes----------------------*/
void disableMotors(void);
void enableMotors(void);
void goForward(void);
void goLeft(void);
void goRight(void);
void setupPins(void);
/*---------------------------Module Code-------------------------------------*/
void setup() {
setupPins();
// Set the data rate in bits per second
Serial.begin(9600);
}
void loop() {
enableMotors();
goForward();
delay(2000);
goRight();
delay(500);
goForward();
delay(2000);
goLeft();
delay(500);
goForward();
delay(2000);
disableMotors();
delay(3000);
}
void disableMotors(){
digitalWrite(ENABLE_A, LOW);
digitalWrite(ENABLE_B, LOW);
}
void enableMotors(){
digitalWrite(ENABLE_A, HIGH);
digitalWrite(ENABLE_B, HIGH);
}
void goForward(){
digitalWrite(MOTOR_A1, LOW);
digitalWrite(MOTOR_A2, HIGH);
digitalWrite(MOTOR_B1, LOW);
digitalWrite (MOTOR_B2, HIGH);
}
void goLeft(){
digitalWrite(MOTOR_A1, LOW);
digitalWrite(MOTOR_A2, HIGH);
digitalWrite(MOTOR_B1, HIGH);
digitalWrite (MOTOR_B2, LOW);
}
void goRight(){
digitalWrite(MOTOR_A1, HIGH);
digitalWrite(MOTOR_A2, LOW);
digitalWrite(MOTOR_B1, LOW);
digitalWrite (MOTOR_B2, HIGH);
}
void setupPins(){
// Configure motor pins
pinMode(ENABLE_A, OUTPUT);
pinMode(MOTOR_A1, OUTPUT);
pinMode(MOTOR_A2, OUTPUT);
pinMode(ENABLE_B, OUTPUT);
pinMode(MOTOR_B1, OUTPUT);
pinMode(MOTOR_B2, OUTPUT);
}
Place the robot on the ground, somewhere with a lot of space, and power up the Arduino board using the 9V battery. If the robot starts moving, everything is working properly.
Connect the BNO055 9-DOF Absolute Orientation IMU Fusion Breakout
The BNO055 has some pins that come with it. These pins need to be soldered to the BNO055. To see how to set up the BNO055 and make sure it is working, check out this video.
Sink the pins of the BNO055 down into the 400-point solderless breadboard so that it is at the other end of the L293D motor controller.
Connect the HC-SR04 Ultrasonic Sensors (the “Eyes”)
Attach the four HC-SR04 ultrasonic sensors to the front of the robot. These sensors are the “eyes” of the robot and are used to detect obstacles.
The pins of the sensors should slip right through the groove of the upper base piece.
Notice how I used some of the Velcro squares to keep the sensors in place and upright.
Wire the sensors exactly as shown in the circuit diagram. Take your time wiring everything up. There are a lot of wires, and you want to make sure everything is wired up correctly.
I used some Velcro squares at the point where the wires slip over the pins of the ultrasonic sensors so that everything stays firm in place.
Test Your HC-SR04 Ultrasonic Sensors
Upload this code to your Arduino:
/**
* This program tests the ultrasonic
* distance sensor (HC-SR04)
*
* @author Addison Sears-Collins
* @version 1.0 2020-03-22
*/
/*---------------------------Definitions-------------------------------------*/
// Right sensor
#define TRIG_RIGHT A2
#define ECHO_RIGHT A1
// Right-center sensor
#define TRIG_RIGHT_CTR 12
#define ECHO_RIGHT_CTR 13
// Left-center sensor
#define TRIG_LEFT_CTR 10
#define ECHO_LEFT_CTR 11
// Left sensor
#define TRIG_LEFT 3
#define ECHO_LEFT 2
/*---------------------------Helper-Function Prototypes----------------------*/
void doPingRight(void);
void doPingRightCtr(void);
void doPingLeftCtr(void);
void doPingLeft(void);
void getDistances(void);
void setupPins(void);
/*---------------------------Module Variables----------------------*/
int distance_right;
int distance_rightctr;
int distance_leftctr;
int distance_left;
/*---------------------------Module Code-------------------------------------*/
void setup(){
setupPins();
// Setup serial communication
Serial.begin(9600);
}
void loop(){
getDistances();
// Print the distances in inches
Serial.println((String)"Distance Right: " + distance_right);
Serial.println((String)"Distance Right Center: " + distance_rightctr);
Serial.println((String)"Distance Left Center: " + distance_leftctr);
Serial.println((String)"Distance Left: " + distance_left);
Serial.println("");
// Pause for 1.0 second
delay(1000);
}
void doPingRight() {
/*
* Returns the distance to the obstacle as an integer in inches
*/
// Make the Trigger LOW (0 volts) for 2 microseconds
digitalWrite(TRIG_RIGHT, LOW);
delayMicroseconds(2);
// Emit high frequency 40kHz sound pulse
// (i.e. pull the Trigger)
// by making Trigger HIGH (5 volts)
// for 10 microseconds
digitalWrite(TRIG_RIGHT, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_RIGHT, LOW);
// Detect a pulse on the Echo pin.
// pulseIn() measures the time in
// microseconds until the sound pulse
// returns back to the sensor.
distance_right = pulseIn(ECHO_RIGHT, HIGH);
// Speed of sound is:
// 13511.811023622 inches per second
// 13511.811023622/10^6 inches per microsecond
// 0.013511811 inches per microsecond
// Taking the reciprocal, we have:
// 74.00932414 microseconds per inch
// Below, we convert microseconds to inches by
// dividing by 74 and then dividing by 2
// to account for the roundtrip time.
distance_right = distance_right / 74 / 2;
distance_right = abs(distance_right);
}
void doPingRightCtr(){
/*
* Returns the distance to the obstacle as an integer in inches
*/
// Make the Trigger LOW (0 volts) for 2 microseconds
digitalWrite(TRIG_RIGHT_CTR, LOW);
delayMicroseconds(2);
// Emit high frequency 40kHz sound pulse
// (i.e. pull the Trigger)
// by making Trigger HIGH (5 volts)
// for 10 microseconds
digitalWrite(TRIG_RIGHT_CTR, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_RIGHT_CTR, LOW);
// Detect a pulse on the Echo pin.
// pulseIn() measures the time in
// microseconds until the sound pulse
// returns back to the sensor.
distance_rightctr = pulseIn(ECHO_RIGHT_CTR, HIGH);
// Speed of sound is:
// 13511.811023622 inches per second
// 13511.811023622/10^6 inches per microsecond
// 0.013511811 inches per microsecond
// Taking the reciprocal, we have:
// 74.00932414 microseconds per inch
// Below, we convert microseconds to inches by
// dividing by 74 and then dividing by 2
// to account for the roundtrip time.
distance_rightctr = distance_rightctr / 74 / 2;
distance_rightctr = abs(distance_rightctr);
}
void doPingLeftCtr(){
/*
* Returns the distance to the obstacle as an integer in inches
*/
// Make the Trigger LOW (0 volts) for 2 microseconds
digitalWrite(TRIG_LEFT_CTR, LOW);
delayMicroseconds(2);
// Emit high frequency 40kHz sound pulse
// (i.e. pull the Trigger)
// by making Trigger HIGH (5 volts)
// for 10 microseconds
digitalWrite(TRIG_LEFT_CTR, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_LEFT_CTR, LOW);
// Detect a pulse on the Echo pin.
// pulseIn() measures the time in
// microseconds until the sound pulse
// returns back to the sensor.
distance_leftctr = pulseIn(ECHO_LEFT_CTR, HIGH);
// Speed of sound is:
// 13511.811023622 inches per second
// 13511.811023622/10^6 inches per microsecond
// 0.013511811 inches per microsecond
// Taking the reciprocal, we have:
// 74.00932414 microseconds per inch
// Below, we convert microseconds to inches by
// dividing by 74 and then dividing by 2
// to account for the roundtrip time.
distance_leftctr = distance_leftctr / 74 / 2;
distance_leftctr = abs(distance_leftctr);
}
void doPingLeft(){
/*
* Returns the distance to the obstacle as an integer in inches
*/
// Make the Trigger LOW (0 volts) for 2 microseconds
digitalWrite(TRIG_LEFT, LOW);
delayMicroseconds(2);
// Emit high frequency 40kHz sound pulse
// (i.e. pull the Trigger)
// by making Trigger HIGH (5 volts)
// for 10 microseconds
digitalWrite(TRIG_LEFT, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_LEFT, LOW);
// Detect a pulse on the Echo pin.
// pulseIn() measures the time in
// microseconds until the sound pulse
// returns back to the sensor.
distance_left = pulseIn(ECHO_LEFT, HIGH);
// Speed of sound is:
// 13511.811023622 inches per second
// 13511.811023622/10^6 inches per microsecond
// 0.013511811 inches per microsecond
// Taking the reciprocal, we have:
// 74.00932414 microseconds per inch
// Below, we convert microseconds to inches by
// dividing by 74 and then dividing by 2
// to account for the roundtrip time.
distance_left = distance_left / 74 / 2;
distance_left = abs(distance_left);
}
void getDistances() {
// Take distance readings on the HC-SR04
doPingRight();
doPingRightCtr();
doPingLeftCtr();
doPingLeft();
}
void setupPins(){
// Configure HC-SR04 pins
pinMode(TRIG_RIGHT, OUTPUT);
pinMode(ECHO_RIGHT, INPUT);
pinMode(TRIG_RIGHT_CTR, OUTPUT);
pinMode(ECHO_RIGHT_CTR, INPUT);
pinMode(TRIG_LEFT_CTR, OUTPUT);
pinMode(ECHO_LEFT_CTR, INPUT);
pinMode(TRIG_LEFT, OUTPUT);
pinMode(ECHO_LEFT, INPUT);
}
While the program is running, click the magnifying glass in the upper right corner of the Arduino IDE. You should see the distances from each sensor to the closest object (in inches). If you wave your hand in front of the sensors, you can see how the readings change.
Connect the Reflectance Sensor
The reflectance sensor comes with some pins. Make sure those pins are soldered to the reflectance sensor.
Cut out a tiny piece of velcro and attach the reflectance sensor between the right and right-center HC-SR04 ultrasonic sensors.
Wire the reflectance sensor exactly as shown in the circuit diagram.
Test Your Reflectance Sensor
Upload this code to your Arduino:
/**
* Addison Sears-Collins
* March 25, 2020
* This code is used to test the QTR-1A Reflectance Sensor
**/
/*---------------------------Definitions-------------------------------------*/
//Define pin for the QTR-1A Reflectance Sensor
#define IR_SENSOR A3
/*---------------------------Helper-Function Prototypes----------------------*/
void blinkLED(void);
int readIRSensor(void);
/*---------------------------Module Variables--------------------------------*/
// Store sensor readings here
int ir_reflect_previous;
int ir_reflect_current;
// Try values between 100 and 600.
// Helps determine if the robot crosses the reflective tape
int threshold = 200;
// Keep track of the number of times the robot crosses over
// the reflective tape
int stage = 0;
void setup() {
// initialize digital pin LED_BUILTIN as an output.
pinMode(LED_BUILTIN, OUTPUT);
// Setup serial communication
Serial.begin(9600);
// Get initial readings on the reflectance sensor
ir_reflect_previous = readIRSensor();
ir_reflect_current = ir_reflect_previous;
}
void loop() {
// Read the reflectance sensor
ir_reflect_current = readIRSensor();
// Check to see if we have crossed over the reflective tape
if ((ir_reflect_previous - ir_reflect_current) >= threshold) {
blinkLED();
// Update the number of crosses
stage = stage + 1;
// Print the stage
Serial.println("----------------------------------------");
Serial.println((String)"Stage: " + stage);
Serial.println("----------------------------------------");
}
// Print the readings
Serial.println((String)"Previous: " + ir_reflect_previous);
Serial.println((String)"Current: " + ir_reflect_current);
Serial.println("");
// Update the previous reading
ir_reflect_previous = ir_reflect_current;
delay(500);
}
void blinkLED(){
for (int i = 0; i < 5; i++) {
digitalWrite(LED_BUILTIN, HIGH); // turn the LED on
delay(50); // wait for half a second
digitalWrite(LED_BUILTIN, LOW); // turn the LED off
delay(50); // wait for half a second
}
}
int readIRSensor(){
/*
* Returns the sensor reading
*/
int temp_reading = 0;
int average = 0;
// Grab four measurements and calculate the average
for (int i = 0; i < 4; i++) {
temp_reading = analogRead(IR_SENSOR);
// Compute running sum
average += temp_reading;
delayMicroseconds(2);
}
// Return the average
return (average / 4);
}
Click the magnifying glass in the upper right corner of the Arduino IDE. You should see the reflectance sensor readings printing out to the screen.
Place a long piece of 3M silver reflective tape down on the floor. This is the starting line. As soon as the robot detects this tape, it will begin detecting obstacles (i.e. pink lemonade cans in my case).
Finish Line
15-20 feet away from the Starting Line, place a long piece of 3M silver reflective tape down on the floor. This is the finish line. As soon as the robot detects this tape, it will begin maintaining the same heading it had when it crossed the Starting line. It will maintain this heading for two seconds before coming to a complete stop.
Multi-Obstacle Environment
Place a bunch of soda cans within your course…before the finish line, but after the starting line. Make sure the cans are at least 18 inches apart from each other.
Now go back to your computer and upload the following code to your Arduino. If you’re interested in understanding what is going on inside the code, just go through it one line at a time, starting from the top, reading all my comments along the way.
#include <Wire.h> // Library for the BNO055
#include <Adafruit_Sensor.h> // Library for the BNO055
#include <Adafruit_BNO055.h> // Library for the BNO055
#include <utility/imumaths.h> // Library for the BNO055
/**
* Addison Sears-Collins
* April 3, 2020
* This code enables a robot to navigate through
* a multi-obstacle environment (MOE).
* The robot starts detecting obstacles once it crosses
* 3M silver reflective tape. The robot finishes once it
* crosses 3M silver reflective tape for the second time.
*
* To calibrate the BNO055 Orientation Sensor,
* start out with the robot facing
* due North (0 degrees).
**/
/*----------Definitions----------*/
//Define pins for Motor A
#define ENABLE_A 5
#define MOTOR_A1 6
#define MOTOR_A2 4
// Define pins for Motor B
#define ENABLE_B 8
#define MOTOR_B1 7
#define MOTOR_B2 9
// Define pin for the QTR-1A Reflectance Sensor
#define IR_SENSOR A3
// Right sensor
#define TRIG_RIGHT A2
#define ECHO_RIGHT A1
// Right-center sensor
#define TRIG_RIGHT_CTR 12
#define ECHO_RIGHT_CTR 13
// Left-center sensor
#define TRIG_LEFT_CTR 10
#define ECHO_LEFT_CTR 11
// Left sensor
#define TRIG_LEFT 3
#define ECHO_LEFT 2
// Avoidance delay
// Number of 50ms increments we want
// to move forward after
// moving away from the object
/***** Try 1-5 depending on battery strength********/
#define AVOIDANCE_DELAY 1
/*---Helper-Function Prototypes---*/
// Motors
void disableMotors(void);
void enableMotors(void);
void goForward(void);
void goLeft(void);
void goRight(void);
void leftAvoid(void);
void rightAvoid(void);
void setupPins(void);
// Headings
void calculateHeadingError(void);
void correctHeading(void);
// Ultrasonic sensors
void doPingRight(void);
void doPingRightCtr(void);
void doPingLeftCtr(void);
void doPingLeft(void);
void getDistances(void);
// IR sensor
void readIRSensor(void);
// Obstacle avoidance
void avoidObstacles(void);
/*--------Module Variables--------*/
bool crossed_the_tape = false;
bool done = false;
// Keep track of the headings
int desired_heading;
int current_heading;
int heading_threshold = 60; // 120 degree cone until stage 2
int heading_error;
// Store sensor readings here
int ir_reflect_previous;
int ir_reflect_current;
// Try values between 100 and 600.
// Helps determine if the robot crosses the reflective tape
int threshold = 200;
// For Ultrasonic sensor distance readings
int distance_right;
int distance_rightctr;
int distance_leftctr;
int distance_left;
int limit = 9; // Inches, try 5-10 depending on battery strength
bool right_or_left = false; // Switch
// Keep track of the time in milliseconds
unsigned long start_time;
unsigned long time_elapsed_threshold = 2000;
Adafruit_BNO055 bno = Adafruit_BNO055(55);
/*----------Module Code-----------*/
void setup(void) {
// Start the car
setupPins();
enableMotors();
// Get initial readings on the IR sensor
ir_reflect_previous = analogRead(IR_SENSOR);
ir_reflect_current = ir_reflect_previous;
// Initialize the orientation sensor
if(!bno.begin()) {
// There was a problem detecting the
// BNO055 ... check your connections
//Serial.print("Ooops, no BNO055 detected ");
//Serial.print("... Check your wiring or I2C ADDR!");
while(1);
}
bno.setExtCrystalUse(true);
}
void loop(void) {
// Stage 0 - before the robot enters the
// multi-obstacle environment
while(!crossed_the_tape) {
delay(50);
// Read desired heading
sensors_event_t event;
bno.getEvent(&event);
desired_heading = event.orientation.x;
goForward();
readIRSensor();
}
crossed_the_tape = false;
// Stage 1
while(!crossed_the_tape) {
// Read all four HC-SR04 sensors
getDistances();
// Avoid any obstacle along the way
avoidObstacles();
}
heading_threshold = 10;
// Capture the time
start_time = millis();
// Stage 2
while(!done) {
calculateHeadingError();
// Correct the heading if needed
if (abs(heading_error) <= abs(heading_threshold)){
goForward();
}
else {
correctHeading();
goForward();
}
// Check to see if we are done
if (((millis()) - start_time) > time_elapsed_threshold) {
done = true;
}
}
while(done) {
disableMotors();
delay(1000);
}
}
void avoidObstacles(){
// Avoid any objects
if ((distance_leftctr < limit) && (distance_rightctr < limit)){
// Switch back and forth
if (right_or_left) {
rightAvoid();
}
else {
leftAvoid();
}
right_or_left = !(right_or_left);
}
else if((distance_left < limit) || (distance_leftctr < limit)) {
rightAvoid();
}
else if((distance_right < limit) || (distance_rightctr < limit)) {
leftAvoid();
}
else {
calculateHeadingError();
// Correct the heading if needed
if (abs(heading_error) <= abs(heading_threshold)){
goForward();
}
else {
correctHeading();
goForward();
}
// Check to see if we have crossed the tape
readIRSensor();
delay(50);
}
}
void calculateHeadingError() {
// Read the current heading
sensors_event_t event;
bno.getEvent(&event);
current_heading = event.orientation.x;
// Calculate the heading error
heading_error = current_heading - desired_heading;
if (heading_error > 180) {
heading_error -= 360;
}
if (heading_error < -180) {
heading_error += 360;
}
}
void correctHeading(){
// Turn the vehicle until it is facing the correct
// direction
if (heading_error < -heading_threshold) {
while (heading_error < -heading_threshold) {
goRight();
delay(4);
calculateHeadingError();
}
}
else {
while (heading_error > heading_threshold) {
goLeft();
delay(4);
calculateHeadingError();
}
}
}
void doPingRight() {
/*
* Returns the distance to the obstacle as an integer in inches
*/
// Make the Trigger LOW (0 volts) for 2 microseconds
digitalWrite(TRIG_RIGHT, LOW);
delayMicroseconds(2);
// Emit high frequency 40kHz sound pulse
// (i.e. pull the Trigger)
// by making Trigger HIGH (5 volts)
// for 10 microseconds
digitalWrite(TRIG_RIGHT, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_RIGHT, LOW);
// Detect a pulse on the Echo pin.
// pulseIn() measures the time in
// microseconds until the sound pulse
// returns back to the sensor.
distance_right = pulseIn(ECHO_RIGHT, HIGH);
// Speed of sound is:
// 13511.811023622 inches per second
// 13511.811023622/10^6 inches per microsecond
// 0.013511811 inches per microsecond
// Taking the reciprocal, we have:
// 74.00932414 microseconds per inch
// Below, we convert microseconds to inches by
// dividing by 74 and then dividing by 2
// to account for the roundtrip time.
distance_right = distance_right / 74 / 2;
distance_right = abs(distance_right);
}
void doPingRightCtr(){
/*
* Returns the distance to the obstacle as an integer in inches
*/
// Make the Trigger LOW (0 volts) for 2 microseconds
digitalWrite(TRIG_RIGHT_CTR, LOW);
delayMicroseconds(2);
// Emit high frequency 40kHz sound pulse
// (i.e. pull the Trigger)
// by making Trigger HIGH (5 volts)
// for 10 microseconds
digitalWrite(TRIG_RIGHT_CTR, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_RIGHT_CTR, LOW);
// Detect a pulse on the Echo pin.
// pulseIn() measures the time in
// microseconds until the sound pulse
// returns back to the sensor.
distance_rightctr = pulseIn(ECHO_RIGHT_CTR, HIGH);
// Speed of sound is:
// 13511.811023622 inches per second
// 13511.811023622/10^6 inches per microsecond
// 0.013511811 inches per microsecond
// Taking the reciprocal, we have:
// 74.00932414 microseconds per inch
// Below, we convert microseconds to inches by
// dividing by 74 and then dividing by 2
// to account for the roundtrip time.
distance_rightctr = distance_rightctr / 74 / 2;
distance_rightctr = abs(distance_rightctr);
}
void doPingLeftCtr(){
/*
* Returns the distance to the obstacle as an integer in inches
*/
// Make the Trigger LOW (0 volts) for 2 microseconds
digitalWrite(TRIG_LEFT_CTR, LOW);
delayMicroseconds(2);
// Emit high frequency 40kHz sound pulse
// (i.e. pull the Trigger)
// by making Trigger HIGH (5 volts)
// for 10 microseconds
digitalWrite(TRIG_LEFT_CTR, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_LEFT_CTR, LOW);
// Detect a pulse on the Echo pin.
// pulseIn() measures the time in
// microseconds until the sound pulse
// returns back to the sensor.
distance_leftctr = pulseIn(ECHO_LEFT_CTR, HIGH);
// Speed of sound is:
// 13511.811023622 inches per second
// 13511.811023622/10^6 inches per microsecond
// 0.013511811 inches per microsecond
// Taking the reciprocal, we have:
// 74.00932414 microseconds per inch
// Below, we convert microseconds to inches by
// dividing by 74 and then dividing by 2
// to account for the roundtrip time.
distance_leftctr = distance_leftctr / 74 / 2;
distance_leftctr = abs(distance_leftctr);
}
void doPingLeft(){
/*
* Returns the distance to the obstacle as an integer in inches
*/
// Make the Trigger LOW (0 volts) for 2 microseconds
digitalWrite(TRIG_LEFT, LOW);
delayMicroseconds(2);
// Emit high frequency 40kHz sound pulse
// (i.e. pull the Trigger)
// by making Trigger HIGH (5 volts)
// for 10 microseconds
digitalWrite(TRIG_LEFT, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_LEFT, LOW);
// Detect a pulse on the Echo pin.
// pulseIn() measures the time in
// microseconds until the sound pulse
// returns back to the sensor.
distance_left = pulseIn(ECHO_LEFT, HIGH);
// Speed of sound is:
// 13511.811023622 inches per second
// 13511.811023622/10^6 inches per microsecond
// 0.013511811 inches per microsecond
// Taking the reciprocal, we have:
// 74.00932414 microseconds per inch
// Below, we convert microseconds to inches by
// dividing by 74 and then dividing by 2
// to account for the roundtrip time.
distance_left = distance_left / 74 / 2;
distance_left = abs(distance_left);
}
void getDistances() {
// Take distance readings on the HC-SR04
doPingRight();
doPingLeftCtr();
doPingRightCtr();
doPingLeft();
}
void disableMotors(){
digitalWrite(ENABLE_A, LOW);
digitalWrite(ENABLE_B, LOW);
}
void enableMotors(){
digitalWrite(ENABLE_A, HIGH);
digitalWrite(ENABLE_B, HIGH);
}
void goForward(){
digitalWrite(MOTOR_A1, LOW);
digitalWrite(MOTOR_A2, HIGH);
digitalWrite(MOTOR_B1, LOW);
digitalWrite (MOTOR_B2, HIGH);
}
void goLeft(){
digitalWrite(MOTOR_A1, LOW);
digitalWrite(MOTOR_A2, HIGH);
digitalWrite(MOTOR_B1, HIGH);
digitalWrite (MOTOR_B2, LOW);
}
void goRight(){
digitalWrite(MOTOR_A1, HIGH);
digitalWrite(MOTOR_A2, LOW);
digitalWrite(MOTOR_B1, LOW);
digitalWrite (MOTOR_B2, HIGH);
}
void leftAvoid(){
// Go to the left when an object is detected
// on the right-side of the vehicle
while((distance_right < limit) || (distance_rightctr < limit)) {
goLeft();
delay(4);
doPingRight();
doPingRightCtr();
}
goForward();
for (int i = 0; i < AVOIDANCE_DELAY; i++) {
// Read the reflectance sensor
ir_reflect_current = analogRead(IR_SENSOR);
// Check to see if we have crossed over the reflective tape
if ((ir_reflect_previous - ir_reflect_current) >= threshold) {
// Update if we crossed the tape
crossed_the_tape = true;
break;
}
// Update the previous reading
ir_reflect_previous = ir_reflect_current;
delay(50);
}
}
void readIRSensor() {
// Read the reflectance sensor
ir_reflect_current = analogRead(IR_SENSOR);
// Check to see if we have crossed over the reflective tape
if ((ir_reflect_previous - ir_reflect_current) >= threshold) {
// Update if we crossed the tape
crossed_the_tape = true;
}
// Update the previous reading
ir_reflect_previous = ir_reflect_current;
}
void rightAvoid(){
// Go to the right when an object is detected
// on the left-side of the vehicle
while((distance_left < limit) || (distance_leftctr < limit)) {
goRight();
delay(4);
doPingLeft();
doPingLeftCtr();
}
goForward();
for (int i = 0; i < AVOIDANCE_DELAY; i++) {
// Read the reflectance sensor
ir_reflect_current = analogRead(IR_SENSOR);
// Check to see if we have crossed over the reflective tape
if ((ir_reflect_previous - ir_reflect_current) >= threshold) {
// Update if we crossed the tape
crossed_the_tape = true;
break;
}
// Update the previous reading
ir_reflect_previous = ir_reflect_current;
delay(50);
}
}
void setupPins(){
// Configure motor pins
pinMode(ENABLE_A, OUTPUT);
pinMode(MOTOR_A1, OUTPUT);
pinMode(MOTOR_A2, OUTPUT);
pinMode(ENABLE_B, OUTPUT);
pinMode(MOTOR_B1, OUTPUT);
pinMode(MOTOR_B2, OUTPUT);
// Configure HC-SR04 pins
pinMode(TRIG_RIGHT, OUTPUT);
pinMode(ECHO_RIGHT, INPUT);
pinMode(TRIG_RIGHT_CTR, OUTPUT);
pinMode(ECHO_RIGHT_CTR, INPUT);
pinMode(TRIG_LEFT_CTR, OUTPUT);
pinMode(ECHO_LEFT_CTR, INPUT);
pinMode(TRIG_LEFT, OUTPUT);
pinMode(ECHO_LEFT, INPUT);
}
Now, we’ve come to the time you’ve been waiting for. It’s time to move the robot through the obstacle course from start to finish.
Set your robot on the floor 3-5 feet before the starting line.
Plug in the Arduino, and within a second or two, you should see your robot take off, moving through the obstacle all by itself! Two seconds after it crosses the finish line, it should come to a complete stop.
If your robot is not moving through the obstacle course, go back to the beginning of this post and make sure that everything is wired up and connected as I have shown.
Also, you can try tweaking the defined constants in the final code to see if your robot behaves better.
That’s it for now. Congratulations, you have built an autonomous mobile robot from scratch that can sense, think, and act.
