OBSTACLE AVOIDANCE-REFERENCE

Programming
Note: A tutorial for the newer version of our Arduino robotics kit is available here: Funduino UNO
Robotics Kit Guide

All right, now that we've finished building our robot, it's time to make it actually do something! We do this by
writing a program (or "sketch", in Arduino terms) and downloading it to the microcontroller. Head on over to
the getting started with Arduino tutorial to learn how to get the Arduino IDE and driver installed.

1. Hello World
Once you have the Arduino IDE and driver installed on your computer, we can go ahead and start writing our
first sketch, the classic "Hello World" program! Fire up the Arduino IDE and you'll be presented with a blank
sketch. Go ahead and copy and paste the code below into the IDE, then click the right arrow button in the upper
left hand corner to build and upload the program to your Arduino:

void setup()
{
 Serial.begin(9600);
}

void loop()
{
Serial.println("Hello World!");
}

The status text in the lower left hand corner of the Arduino IDE will say "Done uploading" when the program
has finished uploading to the Arduino. Once the program has been uploaded, click on the magnifying glass in
the upper right hand corner to open up the Serial Monitor. A window will open up, and you should see the text
"Hello World!" being printed over and over again in it. Congratulations, you just wrote your first Arduino
sketch!

Let's go over the program above. The first function is setup():

void setup()
{
Serial.begin(9600);
}

Setup() is a special function that the Arduino runs when it first turns on. This is where we put our initialization
code, which in this case was configuring and starting the serial port on the Arduino. The line Serial.begin(9600)
tells the Arduino to configure the onboard serial port to operate at 9600 baud and to start it.

The next function is loop():

void loop()
{
Serial.println("Hello World!");
}

Loop() is a special function that the Arduino keeps calling as long as it is turned on. This function is where the
body of Arduino programs reside. The body of this program is really simple and only has one line, which is the
command Serial.println(). This command tells the Arduino to send the string of text passed as a parameter over
the serial port (in this case, back to our computer).

2. Servos

Now lets write another program. This time, let's use the servo motors on our robot to make it drive forward!

#include <Servo.h>

// create servo objects

Servo leftMotor;
Servo rightMotor;

void setup()
{
leftMotor.attach(13);
rightMotor.attach(12);
}

void loop()
{
leftMotor.write(180);
rightMotor.write(180);
}

The first line of the program includes an Arduino library. There are many libraries that have been written for
Arduino and they allow us to easily do things that would otherwise require us to write a lot of code. In this
program, we're including the Servo library, which will allow us to easily use our servo motors.

The next two lines create Servo objects, which we name leftMotor and rightMotor (it's always a good idea to
name variables descriptively).

This time, instead of starting the serial port in the setup() function, we use the Servo .attach() method to tell the
servo objects what pins the servos are connected to on the Arduino. The .attach() function has an Arduino pin
number as a parameter, so since we connected our servos to pins 12 & 13 that's what we pass to the functions as
arguments.

Now that the servo objects know what pins our servos are connected to on the Arduino, we can use the Servo
.write() function to control the servo's direction and speed. The Servo .write() method takes a number between 0
and 180 as a parameter. The reason for this range is that a "standard servo" (one that is not continuous rotation
like ours) usually rotates about 180 degrees, so having the parameter be 0-180 makes sense (i.e. with a standard
servo passing 0 would make it turn all the way one direction, passing 90 would make it go to the middle, and
passing 180 would make it go all the way the other direction). With a continuous rotation servo, passing the
value 0 to the .write() function tells the servo to turn at full speed one direction, 90 makes it stop (also called
"neutral"), and 180 makes it turn full speed the other direction. Go ahead and upload the program and see what
happens!

Note: You might want to put something under the robot to keep the wheels from touching the ground, because
they're going to start turning once the program uploads to the Arduino!

With any luck, when the program finished uploading, the wheels started turning! But wait, look at the directions
of the wheels -- they're turning different directions! Why is this happening?

The reason is that one of the servos is "flipped" with respect to the other since it's pointing in the opposite
direction, so in order to make the wheels on the servos both turn in the same direction, we need to send opposite
values to them! Try changing the parameters in one of the .write() functions to 0, and after uploading you'll find
that both wheels turn in the same direction!

3. Ultrasonic sensor

The next thing we're going to do is learn to use the Ultrasonic sensor on the robot to avoid obstacles. Read the
tutorial on using an HC-SR04 ultrasonic sensor with Arduino to learn how the sensor works, then take a look at
the program below:

const int serialPeriod = 250; // only print to the serial console every 1/4 second
unsigned long timeSerialDelay = 0;

const int loopPeriod = 20; // a period of 20ms = a frequency of 50Hz

unsigned long timeLoopDelay = 0;

// specify the trig & echo pins used for the ultrasonic sensors
const int ultrasonic2TrigPin = 8;
const int ultrasonic2EchoPin = 9;
int ultrasonic2Distance;
int ultrasonic2Duration;

void setup()
{
Serial.begin(9600);

// ultrasonic sensor pin configurations

pinMode(ultrasonic2TrigPin, OUTPUT);
pinMode(ultrasonic2EchoPin, INPUT);
}

void loop()
{
debugOutput(); // prints debugging messages to the serial console

if(millis() - timeLoopDelay >= loopPeriod)

{
readUltrasonicSensors(); // read and store the measured distances

timeLoopDelay = millis();
}
}

void readUltrasonicSensors()
{
// ultrasonic 2
digitalWrite(ultrasonic2TrigPin, HIGH);
delayMicroseconds(10); // must keep the trig pin high for at least
10us
digitalWrite(ultrasonic2TrigPin, LOW);

ultrasonic2Duration = pulseIn(ultrasonic2EchoPin, HIGH);

ultrasonic2Distance = (ultrasonic2Duration/2)/29;
}

void debugOutput()
{
if((millis() - timeSerialDelay) > serialPeriod)
{
Serial.print("ultrasonic2Distance: ");
Serial.print(ultrasonic2Distance);
Serial.print("cm: ");
Serial.println();

timeSerialDelay = millis();
}
}

This program is from the using an HC-SR04 ultrasonic sensor with Arduino tutorial. It's more complicated than
our previous ones, but it's explained in that tutorial (so read it if you haven't already). Go ahead and upload the
program to your Arduino.

After uploading the program to your Arduino, open up the Serial Monitor again (click the magnifying glass in
the upper right-hand corner), and you should see something similar to the following:
The distance being printed out will depend on how far your robot is away from an object. Try putting your hand
in front of the ultrasonic sensor on your robot and moving it back and fourth and the distance being printed out
will change accordingly. Pretty cool, huh?

4. Navigating with the ultrasonic sensor

Now that we know how to use the servos to make the robot drive around, and how to use the ultrasonic sensor
to measure obstacles in front of us, let's combine this knowledge to write a program that has the robot drive
around and avoid obstacles!

Using the ultrasonic code above as a starting point (since it's more complicated than the servo program), let's
add the servo code to it.

The first step is include the servo library by adding the following to the top of the file:

#include <Servo.h>

Next, add the lines to create the servo objects:

// create servo objects

Servo leftMotor;
Servo rightMotor;

Then configure the servo objects in the setup() function:

leftMotor.attach(13);
rightMotor.attach(12);

At this point, you should have the following:

#include <Servo.h>

// create servo objects

Servo leftMotor;
Servo rightMotor;
const int serialPeriod = 250; // only print to the serial console every 1/4 second
unsigned long timeSerialDelay = 0;

const int loopPeriod = 20; // a period of 20ms = a frequency of 50Hz

unsigned long timeLoopDelay = 0;

// specify the trig & echo pins used for the ultrasonic sensors
const int ultrasonic2TrigPin = 8;
const int ultrasonic2EchoPin = 9;

int ultrasonic2Distance;
int ultrasonic2Duration;

void setup()
{
Serial.begin(9600);

// ultrasonic sensor pin configurations

pinMode(ultrasonic2TrigPin, OUTPUT);
pinMode(ultrasonic2EchoPin, INPUT);

leftMotor.attach(13);
rightMotor.attach(12);
}

void loop()
{
debugOutput(); // prints debugging messages to the serial console

if(millis() - timeLoopDelay >= loopPeriod)

{
readUltrasonicSensors(); // read and store the measured distances

timeLoopDelay = millis();
}
}

void readUltrasonicSensors()
{
// ultrasonic 2
digitalWrite(ultrasonic2TrigPin, HIGH);
delayMicroseconds(10); // must keep the trig pin high for at least
10us
digitalWrite(ultrasonic2TrigPin, LOW);

ultrasonic2Duration = pulseIn(ultrasonic2EchoPin, HIGH);

ultrasonic2Distance = (ultrasonic2Duration/2)/29;
}

void debugOutput()
{
if((millis() - timeSerialDelay) > serialPeriod)
{
Serial.print("ultrasonic2Distance: ");
Serial.print(ultrasonic2Distance);
Serial.print("cm: ");
Serial.println();
 timeSerialDelay = millis();
}
}

Now we need to add the code to make the robot drive around. Before we jump in to writing the code, let's think
about what we want to happen:

 If there's nothing in front of the robot, drive forward

 If there is an obstacle in front of the robot, turn left

We will use a finite state machine to accomplish this.

Start by adding a new function to the file, named stateMachine(), then add a call to this function inside of loop()
so that it gets called every loop. Now, add the following towards the top of the file where the other variables are
(above the setup() function):

// define the states

#define DRIVE_FORWARD 0
#define TURN_LEFT 1

int state = DRIVE_FORWARD; // 0 = drive forward (DEFAULT), 1 = turn left

We are going to use the state variable to keep track of whether the robot should be driving forward or turning
left (you can add more states, like turning right, later if you want to). Now we can begin working on the
stateMachine() function itself.

To implement the state machine, we're going to use if-else statements, like this:

if(state == DRIVE_FORWARD) // no obstacles detected

{

}
else if(state == TURN_LEFT) // obstacle detected -- turning left
{

We set the state variable to DRIVE_FORWARD by default, so when the robot turns on, the program will branch
into the first if statement. Inside the DRIVE_FORWARD state, we need to check whether there is an obstacle in
front of us and if there isn't, we want to drive forward. If there is an obstacle in front of us, we want to change
the state to TURN_LEFT. So let's go ahead and add the code to do that:

if(ultrasonic2Distance > 6 || ultrasonic2Distance < 0) // if there's nothing in front of

us (note: ultrasonicDistance will be negative for some ultrasonics if there's nothing in
range)
{
// drive forward
rightMotor.write(180);
leftMotor.write(0);
}
else // there's an object in front of us
{
state = TURN_LEFT;
}
Next, we need to write the code for the TURN_LEFT state. In this state, we want to start the robot turning left
and keep it turning left until it has turned 90 degrees. Here's the code for the TURN_LEFT state:

unsigned long timeToTurnLeft = 1100; // it takes around 1.1 seconds to turn 90 degrees

unsigned long turnStartTime = millis(); // save the time that we started turning

while((millis()-turnStartTime) < timeToTurnLeft) // stay in this loop until

timeToTurnLeft (1.1 seconds) has elapsed
{
// turn left
rightMotor.write(180);
leftMotor.write(180);
}

state = DRIVE_FORWARD;

This may be a little confusing, so let's break it down line by line:

 timeToTurnLeft: we put the amount of time it takes our robot to turn 90 degrees in here (in milliseconds)

 turnStartTime: we store the amount of time that the Arduino has been powered on in here

 millis()-turnStartTime: this gives us the amount of time that the robot has been turning left

 while((millis()-turnStartTime) < timeToTurnLeft): the program will stay in the while loop until the robot
has been turning for timeToTurnLeft milliseconds

Inside the while loop, we make the robot turn. The while loop keeps looping until the proper time has elapsed,
then the program exists the while loop and the state is set back to DRIVE_FORWARD.

Note: Using a loop in this manner is quick and easy, but it's not a good practice because it stops other things
from happening in the program. A loop like this is called "blocking" because the program gets "stuck" in it for a
certain amount of time. We will show a non-blocking example later.

OK, so at this point you should have something similar to the following:

#include <Servo.h>

// create servo objects

Servo leftMotor;
Servo rightMotor;

const int serialPeriod = 250; // only print to the serial console every 1/4 second
unsigned long timeSerialDelay = 0;

const int loopPeriod = 20; // a period of 20ms = a frequency of 50Hz

unsigned long timeLoopDelay = 0;

// specify the trig & echo pins used for the ultrasonic sensors
const int ultrasonic2TrigPin = 8;
const int ultrasonic2EchoPin = 9;

int ultrasonic2Distance;
int ultrasonic2Duration;

// define the states

#define DRIVE_FORWARD 0
#define TURN_LEFT 1

int state = DRIVE_FORWARD; // 0 = drive forward (DEFAULT), 1 = turn left

void setup()
{
Serial.begin(9600);

// ultrasonic sensor pin configurations

pinMode(ultrasonic2TrigPin, OUTPUT);
pinMode(ultrasonic2EchoPin, INPUT);

leftMotor.attach(13);
rightMotor.attach(12);
}

void loop()
{
debugOutput(); // prints debugging messages to the serial console

if(millis() - timeLoopDelay >= loopPeriod)

{
readUltrasonicSensors(); // read and store the measured distances

stateMachine();

timeLoopDelay = millis();
}
}

void stateMachine()
{
if(state == DRIVE_FORWARD) // no obstacles detected
{
if(ultrasonic2Distance > 6 || ultrasonic2Distance < 0) // if there's nothing in
front of us (note: ultrasonicDistance will be negative for some ultrasonics if there's
nothing in range)
{
// drive forward
rightMotor.write(180);
leftMotor.write(0);
}
else // there's an object in front of us
{
state = TURN_LEFT;
}
}
else if(state == TURN_LEFT) // obstacle detected -- turn left
{
unsigned long timeToTurnLeft = 1100; // it takes around 1.1 seconds to turn 90
degrees

unsigned long turnStartTime = millis(); // save the time that we started turning

while((millis()-turnStartTime) < timeToTurnLeft) // stay in this loop until

timeToTurnLeft (1.1 seconds) has elapsed
{
// turn left
rightMotor.write(180);
 leftMotor.write(180);
}

state = DRIVE_FORWARD;
}
}

void readUltrasonicSensors()
{
// ultrasonic 2
digitalWrite(ultrasonic2TrigPin, HIGH);
delayMicroseconds(10); // must keep the trig pin high for at least
10us
digitalWrite(ultrasonic2TrigPin, LOW);

ultrasonic2Duration = pulseIn(ultrasonic2EchoPin, HIGH);

ultrasonic2Distance = (ultrasonic2Duration/2)/29;
}

void debugOutput()
{
if((millis() - timeSerialDelay) > serialPeriod)
{
Serial.print("ultrasonic2Distance: ");
Serial.print(ultrasonic2Distance);
Serial.print("cm");
Serial.println();

timeSerialDelay = millis();
}
}

Go ahead and upload the program to the Arduino and see what happens!