Hello,
This is a step by step guide to build an autonomous navigation robot. We use the Arduino microcontroller to control this robot. We have two different programs for this robot. The first enables the robot to drive around and avoid anything that gets in its way. This avoiding obstacles program uses two ultrasonic sensors. Our other program uses 2-D arrays to map out the surrounding area. Based on the values we input into the 2-D array, the robot knows where things around it are. Both programs are included.
This is a step by step guide to build an autonomous navigation robot. We use the Arduino microcontroller to control this robot. We have two different programs for this robot. The first enables the robot to drive around and avoid anything that gets in its way. This avoiding obstacles program uses two ultrasonic sensors. Our other program uses 2-D arrays to map out the surrounding area. Based on the values we input into the 2-D array, the robot knows where things around it are. Both programs are included.
Step 1: Materials
We used vex as the
frame for our robot, but you can use anything you want to make the
structure. In fact, we recommend making the frame from scratch. We also
use Vex sensors and vex motors, but even if you use other sensors and
motors, (which we recommend that you do) it will work almost exactly the
same way.- (2) ultrasonic sensors (4 for further mapping ability or just to have fun :) )
- (4) servos (5 for funsies)
- Arduino (we used Uno)
- Perf board (we used Radio Shack 276-150: http://www.radioshack.com/product/index.jsp?productId=2102845)
- Lots of Wires
- (2) 9.6 V Batteries (we used Vex batteries: http://www.vexrobotics.com/276-2220.html)
- (1) 9V Battery (to power the Arduino)
- (4) wheels (5” diameter)
- assorted hardware (nuts, bolts, etc.)
- duct tape is also quite helpful
This tutorial has three more steps.
The first section is about the mechanical aspect.
The second section is about the electronic aspect.
The third section is about the programming.
Step 2: Mechanical
First,
we need to build a solid base. We have attached pictures here, but you
can make it pretty much however you want. We made three different
prototypes of this robot. We will discuss the first two here. Mark I had
a truck-like shape. It was pretty big, but that also made it slower and
harder to turn. Also, for our purposes, the size was pretty much
unnecessary. Therefore, in our second model, we made it much smaller and
more compact.
Next, we need to add the servos underneath the base so that there is enough space on the outside to attach wheels. We used a four wheel drive. Depending on how strong your servos are, you can use two wheel drive if you want. However, there must be enough space on top of the base to fit the Arduino, PCB, and battery.
Next, add the wheels onto the servos, we added reinforcements outside the wheels to secure the other side of the shaft and keep them from coming off. The two extra wheels at the front are elevated so that if the robot runs into a curb or step, it will be able to climb on top of it. We put duct tape on the back two wheels to reduce the friction so that it turns easier.
Next, we added a battery holder. For us, it was easy to take a Vex battery charger and hack it so that it transmits power to the wheels instead of charging the batteries. Take out the circuit board inside and desolder the positive and negative leads that go to the charging port. Next you solder together the black wires from the two battery terminals and solder together the red wires as well. Next, solder a wire each to the red and black wire. This can plug straight into your PCB. Pictures of this battery holder are under the electronics section.
Next, make a mount at the front of the robot for the ultrasonic sensors. If you want to go above and beyond, you can make a mount in the center for a rotating ultrasonic sensor.
We have included pictures of our Mark II design which is a standard four wheel drive as well as our Mark III design which features a crab wheel design. Crab wheel is more complicated to program, but it enables holonomic drive.
Next, we need to add the servos underneath the base so that there is enough space on the outside to attach wheels. We used a four wheel drive. Depending on how strong your servos are, you can use two wheel drive if you want. However, there must be enough space on top of the base to fit the Arduino, PCB, and battery.
Next, add the wheels onto the servos, we added reinforcements outside the wheels to secure the other side of the shaft and keep them from coming off. The two extra wheels at the front are elevated so that if the robot runs into a curb or step, it will be able to climb on top of it. We put duct tape on the back two wheels to reduce the friction so that it turns easier.
Next, we added a battery holder. For us, it was easy to take a Vex battery charger and hack it so that it transmits power to the wheels instead of charging the batteries. Take out the circuit board inside and desolder the positive and negative leads that go to the charging port. Next you solder together the black wires from the two battery terminals and solder together the red wires as well. Next, solder a wire each to the red and black wire. This can plug straight into your PCB. Pictures of this battery holder are under the electronics section.
Next, make a mount at the front of the robot for the ultrasonic sensors. If you want to go above and beyond, you can make a mount in the center for a rotating ultrasonic sensor.
We have included pictures of our Mark II design which is a standard four wheel drive as well as our Mark III design which features a crab wheel design. Crab wheel is more complicated to program, but it enables holonomic drive.
Step 3: Electronics
The
electronics for this robot are not that hard. If you use the battery
holder idea from above, your 9.6 should be connected in parallel. If you
aren’t using that idea, connect your batteries in parallel. Then,
follow the picture of the circuit board posted here. However, be careful
because depending on the size of your base, your servo wires may not be
able to reach the circuit board. We are using the same signal wire for
servos 1 and 2 and a different signal wire for servos 3 and 4. This is
because servos 1 and 2 should always have the same signal while servos 3
and 4 should always have the same signal (because they are on the same
side).
If you want to add another sensor or another servo just follow the same pattern as shown in the picture connecting signal to an Arduino pin, 5V to red, and ground to black. Remember that the ground on motors has to be connected to both the Arduino black and the battery black.
We include a rotary encoder on one of our motors just to measure how far it turned. But, this is completely optional.
If you want to add another sensor or another servo just follow the same pattern as shown in the picture connecting signal to an Arduino pin, 5V to red, and ground to black. Remember that the ground on motors has to be connected to both the Arduino black and the battery black.
We include a rotary encoder on one of our motors just to measure how far it turned. But, this is completely optional.
Step 4: Programming
We are using the Arduino Uno programming language. It is based off of Java.
Here is our sample code for navigation using 2D arrays. Inside the 2D array grid, we input either a 0 or a 1. A 1 represents an object or an obstacle. A 0 represents free space. The robot travels through the 0’s.
Grid navigation:
// THIS IS A PROGRAM TO MAP OUT AN AREA WITH 2D ARRAYS AND THEN NAVIGATE ONLY THROUGH THE
// ZEROS PRESENT. ULTRASONIC SENSORS ARE NOT USED ALTHOUGH THEY ARE INITIALIZED.
// THE ENCODERS ARE ALSO NOT USED BUT ARE INITIALIZED
// @author Pranav
#include "Servo.h"
// Motors
Servo motor1;
Servo motor2;
// Rotary Encoders
int encoderAPin = 4; //Left // pin for the rotary encoderA
int encoderBPin = 2; //Right // pin for the rotary encoderB
int encoderAPosition = 0; // position counter for encoderA
int encoderBPosition = 0; // position counter for encoderB
int signalAInput1; // signal input 1 for encoderA
int signalAInput2; // signal input 2 for encoderA
int signalBInput1; // signal input 1 for encoderB
int signalBInput2; // signal input 2 for encoderB
// robot location service
// 0 is up
// 1 is right
// 2 is down
// 3 is left
int robotDirection = 2;
// this is the coordinates in the grid of where the robot is
// it is also the x and y indexes in the array.
// remember that the array starts at index 0.
int xcoordinate = 2;
int ycoordinate = 1;
// ultrasonic pins
const int Trig_pin = 5; // pin for triggering pulse INPUT
const int Echo_pin = 6; // pin for recieving echo OUPUT
long duration; // how long it takes for the sound to rebound
// motor pins
const int Motor1Pin = 9; // Left side
const int Motor2Pin = 10; // right side
// the array that it tracks with
// this can be an array of any size
// just make sure that the robot has a free space to move to from its initial position.
int arraything [6] [6] =
{
{ 1,1,1,1,1,1 }
,
{ 1,1,0,1,0,1 }
,
{ 1,1,0,1,0,1 }
,
{ 1,1,0,1,0,1 }
,
{ 1,1,0,1,1,1 }
,
{ 1,1,1,0,1,1 }
};
void setup () {
// Rotary encoder
pinMode (encoderAPin, INPUT);
pinMode (encoderBPin, INPUT);
Serial.begin(9600);
Serial.println ("Starting now...");
\
// ultrasonic
// pinMode(Trig_pin, OUTPUT); // initialize the pulse pin as output:
// pinMode(Echo_pin, INPUT); // initialize the echo_pin pin as an input:
// motorsmmmmmmmmm
motor1.attach(Motor1Pin);
motor2.attach(Motor2Pin);
}
// In English, this program sees if there is something in front of it.
// If no, it moves straight. If yes, it checks its Right.
// If its Right is free, it turns that way, or else, it checks its Left.
// If its Left is free, it turns that way, or else, it just turns 180 degrees
// and goes back the way it came.
//
// Next to do is to encorporate beginning and destination as well as optimum path finding
//
// Last is to use the ultrasonic with the grid.
void loop () {
while (1==1){
if (isFrontOpen() == true) {
moveForward();
delay (2000);
}
else
if (isRightOpen() == true) {
turnRight();
delay (2000);
}
else
if (isLeftOpen() == true) {
turnLeft();
delay (2000);
}
else {
turnAround();
delay (2000);
}
}
}
// Checks if there is something right in front of it using Grids
boolean isFrontOpen () {
int nextNumber = getFrontNumber();
if (nextNumber == 0){
return true;
}
else {
return false;
}
}
// Checks if there is something to the Right of it using Grids
boolean isRightOpen(){
int nextNumber = getRightNumber();
if (nextNumber == 0){
return true;
}
else {
return false;
}
}
// Checks if there is something to the Left of it using Grids
boolean isLeftOpen(){
int nextNumber = getLeftNumber();
if (nextNumber == 0){
return true;
}
else {
return false;
}
}
// Moves straight forward.
void moveForward () {
motor1.write(180);
motor2.write(0);
Serial.println("Forward");
if (robotDirection == 0)
ycoordinate = ycoordinate - 1;
if (robotDirection == 1)
xcoordinate = xcoordinate + 1;
if (robotDirection == 2)
ycoordinate = ycoordinate + 1;
if (robotDirection == 3)
xcoordinate = xcoordinate - 1;
delay (100);
/*Serial.print(" xcoordinate " );
Serial.print(xcoordinate);
delay (500);
Serial.print(" ycoordinate ");
Serial.print(ycoordinate);
delay (500);
Serial.print(" robot direction: ");
Serial.print(robotDirection);
delay(500);
Serial.println ();
delay(1000);
*/
delay(800);
}
// Turns 90 degrees to the Right
void turnRight () {
motor1.write (60);
motor2.write (60);
delay(178);
motor2.write(95) ;
delay(65) ;
motor1.write(90);
Serial.println("Right");
if (robotDirection == 0)
robotDirection = 1;
else if (robotDirection == 1)
robotDirection = 2;
else if (robotDirection == 2)
robotDirection = 3;
else if (robotDirection == 3)
robotDirection = 0;
delay (500);
Serial.print(" xcoordinate " );
Serial.print(xcoordinate);
delay (500);
Serial.print(" ycoordinate ");
Serial.print(ycoordinate);
delay (500);
Serial.print(" robot direction: ");
Serial.print(robotDirection);
delay (500);
Serial.println();
delay(1000);
}
// Turns 90 degrees to the Left
void turnLeft () {
motor1.write(120);
motor2.write(120);
delay(325);
motor2.write(95) ;
delay(65) ;
motor1.write(90);
Serial.println("Left");
if (robotDirection == 0)
robotDirection = 3;
else if (robotDirection == 1)
robotDirection = 0;
else if (robotDirection == 2)
robotDirection = 1;
else if (robotDirection == 3)
robotDirection = 2;
delay (500);
Serial.print(" xcoordinate " );
Serial.print(xcoordinate);
delay (500);
Serial.print(" ycoordinate ");
Serial.print(ycoordinate);
delay (500);
Serial.print(" robot direction: ");
Serial.print(robotDirection);
delay(500);
Serial.println();
delay(1000);
}
// Turns 180 degrees
void turnAround () {
// delay(1000);
Serial.println("Around");
if (robotDirection == 0)
robotDirection = 2;
else if (robotDirection == 1)
robotDirection = 3;
else if (robotDirection == 2)
robotDirection = 0;
else if (robotDirection == 3)
robotDirection = 1;
delay (500);
Serial.print(" xcoordinate " );
Serial.print(xcoordinate);
delay (500);
Serial.print(" ycoordinate ");
Serial.print(ycoordinate);
delay (500);
Serial.print(" robot direction: ");
Serial.print(robotDirection);
delay(500);
Serial.println();
delay(1000);
}
// Gets the number on the Grid of the space right in front of it.
int getFrontNumber() {
if (robotDirection == 0) {
return arraything [ycoordinate - 1][xcoordinate];
}
if (robotDirection == 1) {
return arraything [ycoordinate][xcoordinate + 1];
}
if (robotDirection == 2) {
return arraything [ycoordinate + 1][xcoordinate] ;
}
if (robotDirection == 3) {
return arraything [ycoordinate][xcoordinate - 1];
}
}
// Gets the number on the Grid of the space to the Right of it.
int getRightNumber() {
if (robotDirection == 0) {
return arraything [ycoordinate][xcoordinate + 1] ;
}
if (robotDirection == 1) {
return arraything [ycoordinate + 1][xcoordinate];
}
if (robotDirection == 2) {
return arraything [ycoordinate][xcoordinate - 1];
}
if (robotDirection == 3) {
return arraything [ycoordinate - 1][xcoordinate];
}
}
// Gets the number on the Grid of the Space to the Left of it.
int getLeftNumber() {
if (robotDirection == 0) {
return arraything [ycoordinate][xcoordinate - 1] ;
}
if (robotDirection == 1) {
return arraything [ycoordinate - 1][xcoordinate];
}
if (robotDirection == 2) {
return arraything [ycoordinate][xcoordinate + 1] ;
}
if (robotDirection == 3) {
return arraything [ycoordinate + 1][xcoordinate];
}
}
Here is a sample program for the ultrasonic sensors. This program will just avoid anything and everything that gets in the way.
Included in the program is programming for an extra servo simply spinning back and forth on the top of the robot. If you aren’t doing this on your robot, ignore that part of the code, it should have no effect on your program.
// THIS CODE AVOIDS OBSTACLES
// IT CAN EITHER ENCOUNTER AN OBSTACLE ON THE RIGHT OR THE LEFT SIDE
// THEREFORE, WE PROGRAMMED IT TO TURN TO THE LEFT IF IT ENCOUNTERED AN OBSTACLE ON THE RIGHT
// AND RIGHT IF WE ENCOUNTERED IT ON THE LEFT
// @author: Pranav
/*
UltraSonic 1
Pin 5 Triggers the Pulse (Yellow lead)
Pin 6 Recieves the Echo (Orange lead)
UltraSonic 2
Pin 7 Triggers the Pulse (Yellow lead)
Pin 8 Recieves the Echo (Orange lead)
*/
#include "Servo.h"
Servo motorR;
Servo motorL;
Servo motorS;
// ultrasonic pins
// ultrasonic Right
const int Trig1_pin = 5; // pin for triggering pulse INPUT
const int Echo1_pin = 6; // pin for recieving echo OUPUT
long duration1; // how long it takes for the sound to rebound
// ultrasonic Left
const int Trig2_pin = 7;
const int Echo2_pin = 8;
long duration2;
// motor pins
const int MotorLPin = 2; // Left
const int MotorRPin = 13; // Right
// spinning ultrasonic motor
const int MotorSpinPin = 3;
int spinCounter = 0;
// sensing or not
boolean isARightWallThere = false;
boolean isALeftWallThere = false;
// which way to turn
boolean Right = true;
void setup() {
Serial.begin(9600);
Serial.println ("Starting");
// ultrasonic 1
pinMode(Trig1_pin, OUTPUT); // initialize the pulse pin as output:
pinMode(Echo1_pin, INPUT); // initialize the echo_pin pin as an input:
//ultrasonic 2
pinMode(7, OUTPUT); // initialize the pulse pin as output:
pinMode(Echo2_pin, INPUT); // initialize the echo_pin pin as an input:
// motors
motorR.attach(MotorRPin);
motorL.attach(MotorLPin);
motorS.attach(MotorSpinPin);
}
void loop(){
while (1==1) {
if (spinCounter < 2) {
motorS.write(160);
}
else {
motorS.write(20);
}
if (spinCounter >= 3) {
spinCounter = 0;
}
spinCounter ++;
checkTheRightWall();
checkTheLeftWall();
if (isARightWallThere == false && isALeftWallThere == false) {
motorR.write(120);
motorL.write(72);
}
else {
if (isARightWallThere == true && isALeftWallThere == true){
motorR.write(0);
motorL.write(180);
delay(300);
motorR.write(80);
motorL.write(80);
delay(500);
motorR.write(0);
motorL.write(180);
}
else {
if (isARightWallThere == false && isALeftWallThere == true) {
motorR.write(50);
motorL.write(90);
}
else {
if (isARightWallThere == true && isALeftWallThere == false) {
motorR.write(90);
motorL.write(140);
}
}
}
}
}
}
void checkTheRightWall () {
digitalWrite(Trig1_pin, LOW);
delayMicroseconds(2);
digitalWrite(Trig1_pin, HIGH);
delayMicroseconds(10);
digitalWrite(Trig1_pin, LOW);
duration1 = pulseIn(Echo1_pin,10);
Serial.println("Duration1: ");
Serial.println(duration1, DEC);
if ((duration1 > 4000 || duration1 == 0)) {
isARightWallThere = false;
}
else {
isARightWallThere = true;
}
}
void checkTheLeftWall() {
digitalWrite(7, LOW);
delayMicroseconds(2);
digitalWrite(7, HIGH);
delayMicroseconds(10);
digitalWrite(7, LOW);
duration2 = pulseIn(Echo2_pin,10);
Serial.println("Duration2: ");
Serial.println(duration2, DEC);
if ((duration2 > 4000 || duration2 == 0)) {
isALeftWallThere = false;
}
else {
isALeftWallThere = true;
}
}
If you have the time, check out one of my friend's pages. He is the one who did all the programming for us.
Here's the link to his page: https://www.instructables.com/member/praznav/
The end,
Spark Industries,
Here is our sample code for navigation using 2D arrays. Inside the 2D array grid, we input either a 0 or a 1. A 1 represents an object or an obstacle. A 0 represents free space. The robot travels through the 0’s.
Grid navigation:
// THIS IS A PROGRAM TO MAP OUT AN AREA WITH 2D ARRAYS AND THEN NAVIGATE ONLY THROUGH THE
// ZEROS PRESENT. ULTRASONIC SENSORS ARE NOT USED ALTHOUGH THEY ARE INITIALIZED.
// THE ENCODERS ARE ALSO NOT USED BUT ARE INITIALIZED
// @author Pranav
#include "Servo.h"
// Motors
Servo motor1;
Servo motor2;
// Rotary Encoders
int encoderAPin = 4; //Left // pin for the rotary encoderA
int encoderBPin = 2; //Right // pin for the rotary encoderB
int encoderAPosition = 0; // position counter for encoderA
int encoderBPosition = 0; // position counter for encoderB
int signalAInput1; // signal input 1 for encoderA
int signalAInput2; // signal input 2 for encoderA
int signalBInput1; // signal input 1 for encoderB
int signalBInput2; // signal input 2 for encoderB
// robot location service
// 0 is up
// 1 is right
// 2 is down
// 3 is left
int robotDirection = 2;
// this is the coordinates in the grid of where the robot is
// it is also the x and y indexes in the array.
// remember that the array starts at index 0.
int xcoordinate = 2;
int ycoordinate = 1;
// ultrasonic pins
const int Trig_pin = 5; // pin for triggering pulse INPUT
const int Echo_pin = 6; // pin for recieving echo OUPUT
long duration; // how long it takes for the sound to rebound
// motor pins
const int Motor1Pin = 9; // Left side
const int Motor2Pin = 10; // right side
// the array that it tracks with
// this can be an array of any size
// just make sure that the robot has a free space to move to from its initial position.
int arraything [6] [6] =
{
{ 1,1,1,1,1,1 }
,
{ 1,1,0,1,0,1 }
,
{ 1,1,0,1,0,1 }
,
{ 1,1,0,1,0,1 }
,
{ 1,1,0,1,1,1 }
,
{ 1,1,1,0,1,1 }
};
void setup () {
// Rotary encoder
pinMode (encoderAPin, INPUT);
pinMode (encoderBPin, INPUT);
Serial.begin(9600);
Serial.println ("Starting now...");
\
// ultrasonic
// pinMode(Trig_pin, OUTPUT); // initialize the pulse pin as output:
// pinMode(Echo_pin, INPUT); // initialize the echo_pin pin as an input:
// motorsmmmmmmmmm
motor1.attach(Motor1Pin);
motor2.attach(Motor2Pin);
}
// In English, this program sees if there is something in front of it.
// If no, it moves straight. If yes, it checks its Right.
// If its Right is free, it turns that way, or else, it checks its Left.
// If its Left is free, it turns that way, or else, it just turns 180 degrees
// and goes back the way it came.
//
// Next to do is to encorporate beginning and destination as well as optimum path finding
//
// Last is to use the ultrasonic with the grid.
void loop () {
while (1==1){
if (isFrontOpen() == true) {
moveForward();
delay (2000);
}
else
if (isRightOpen() == true) {
turnRight();
delay (2000);
}
else
if (isLeftOpen() == true) {
turnLeft();
delay (2000);
}
else {
turnAround();
delay (2000);
}
}
}
// Checks if there is something right in front of it using Grids
boolean isFrontOpen () {
int nextNumber = getFrontNumber();
if (nextNumber == 0){
return true;
}
else {
return false;
}
}
// Checks if there is something to the Right of it using Grids
boolean isRightOpen(){
int nextNumber = getRightNumber();
if (nextNumber == 0){
return true;
}
else {
return false;
}
}
// Checks if there is something to the Left of it using Grids
boolean isLeftOpen(){
int nextNumber = getLeftNumber();
if (nextNumber == 0){
return true;
}
else {
return false;
}
}
// Moves straight forward.
void moveForward () {
motor1.write(180);
motor2.write(0);
Serial.println("Forward");
if (robotDirection == 0)
ycoordinate = ycoordinate - 1;
if (robotDirection == 1)
xcoordinate = xcoordinate + 1;
if (robotDirection == 2)
ycoordinate = ycoordinate + 1;
if (robotDirection == 3)
xcoordinate = xcoordinate - 1;
delay (100);
/*Serial.print(" xcoordinate " );
Serial.print(xcoordinate);
delay (500);
Serial.print(" ycoordinate ");
Serial.print(ycoordinate);
delay (500);
Serial.print(" robot direction: ");
Serial.print(robotDirection);
delay(500);
Serial.println ();
delay(1000);
*/
delay(800);
}
// Turns 90 degrees to the Right
void turnRight () {
motor1.write (60);
motor2.write (60);
delay(178);
motor2.write(95) ;
delay(65) ;
motor1.write(90);
Serial.println("Right");
if (robotDirection == 0)
robotDirection = 1;
else if (robotDirection == 1)
robotDirection = 2;
else if (robotDirection == 2)
robotDirection = 3;
else if (robotDirection == 3)
robotDirection = 0;
delay (500);
Serial.print(" xcoordinate " );
Serial.print(xcoordinate);
delay (500);
Serial.print(" ycoordinate ");
Serial.print(ycoordinate);
delay (500);
Serial.print(" robot direction: ");
Serial.print(robotDirection);
delay (500);
Serial.println();
delay(1000);
}
// Turns 90 degrees to the Left
void turnLeft () {
motor1.write(120);
motor2.write(120);
delay(325);
motor2.write(95) ;
delay(65) ;
motor1.write(90);
Serial.println("Left");
if (robotDirection == 0)
robotDirection = 3;
else if (robotDirection == 1)
robotDirection = 0;
else if (robotDirection == 2)
robotDirection = 1;
else if (robotDirection == 3)
robotDirection = 2;
delay (500);
Serial.print(" xcoordinate " );
Serial.print(xcoordinate);
delay (500);
Serial.print(" ycoordinate ");
Serial.print(ycoordinate);
delay (500);
Serial.print(" robot direction: ");
Serial.print(robotDirection);
delay(500);
Serial.println();
delay(1000);
}
// Turns 180 degrees
void turnAround () {
// delay(1000);
Serial.println("Around");
if (robotDirection == 0)
robotDirection = 2;
else if (robotDirection == 1)
robotDirection = 3;
else if (robotDirection == 2)
robotDirection = 0;
else if (robotDirection == 3)
robotDirection = 1;
delay (500);
Serial.print(" xcoordinate " );
Serial.print(xcoordinate);
delay (500);
Serial.print(" ycoordinate ");
Serial.print(ycoordinate);
delay (500);
Serial.print(" robot direction: ");
Serial.print(robotDirection);
delay(500);
Serial.println();
delay(1000);
}
// Gets the number on the Grid of the space right in front of it.
int getFrontNumber() {
if (robotDirection == 0) {
return arraything [ycoordinate - 1][xcoordinate];
}
if (robotDirection == 1) {
return arraything [ycoordinate][xcoordinate + 1];
}
if (robotDirection == 2) {
return arraything [ycoordinate + 1][xcoordinate] ;
}
if (robotDirection == 3) {
return arraything [ycoordinate][xcoordinate - 1];
}
}
// Gets the number on the Grid of the space to the Right of it.
int getRightNumber() {
if (robotDirection == 0) {
return arraything [ycoordinate][xcoordinate + 1] ;
}
if (robotDirection == 1) {
return arraything [ycoordinate + 1][xcoordinate];
}
if (robotDirection == 2) {
return arraything [ycoordinate][xcoordinate - 1];
}
if (robotDirection == 3) {
return arraything [ycoordinate - 1][xcoordinate];
}
}
// Gets the number on the Grid of the Space to the Left of it.
int getLeftNumber() {
if (robotDirection == 0) {
return arraything [ycoordinate][xcoordinate - 1] ;
}
if (robotDirection == 1) {
return arraything [ycoordinate - 1][xcoordinate];
}
if (robotDirection == 2) {
return arraything [ycoordinate][xcoordinate + 1] ;
}
if (robotDirection == 3) {
return arraything [ycoordinate + 1][xcoordinate];
}
}
Here is a sample program for the ultrasonic sensors. This program will just avoid anything and everything that gets in the way.
Included in the program is programming for an extra servo simply spinning back and forth on the top of the robot. If you aren’t doing this on your robot, ignore that part of the code, it should have no effect on your program.
// THIS CODE AVOIDS OBSTACLES
// IT CAN EITHER ENCOUNTER AN OBSTACLE ON THE RIGHT OR THE LEFT SIDE
// THEREFORE, WE PROGRAMMED IT TO TURN TO THE LEFT IF IT ENCOUNTERED AN OBSTACLE ON THE RIGHT
// AND RIGHT IF WE ENCOUNTERED IT ON THE LEFT
// @author: Pranav
/*
UltraSonic 1
Pin 5 Triggers the Pulse (Yellow lead)
Pin 6 Recieves the Echo (Orange lead)
UltraSonic 2
Pin 7 Triggers the Pulse (Yellow lead)
Pin 8 Recieves the Echo (Orange lead)
*/
#include "Servo.h"
Servo motorR;
Servo motorL;
Servo motorS;
// ultrasonic pins
// ultrasonic Right
const int Trig1_pin = 5; // pin for triggering pulse INPUT
const int Echo1_pin = 6; // pin for recieving echo OUPUT
long duration1; // how long it takes for the sound to rebound
// ultrasonic Left
const int Trig2_pin = 7;
const int Echo2_pin = 8;
long duration2;
// motor pins
const int MotorLPin = 2; // Left
const int MotorRPin = 13; // Right
// spinning ultrasonic motor
const int MotorSpinPin = 3;
int spinCounter = 0;
// sensing or not
boolean isARightWallThere = false;
boolean isALeftWallThere = false;
// which way to turn
boolean Right = true;
void setup() {
Serial.begin(9600);
Serial.println ("Starting");
// ultrasonic 1
pinMode(Trig1_pin, OUTPUT); // initialize the pulse pin as output:
pinMode(Echo1_pin, INPUT); // initialize the echo_pin pin as an input:
//ultrasonic 2
pinMode(7, OUTPUT); // initialize the pulse pin as output:
pinMode(Echo2_pin, INPUT); // initialize the echo_pin pin as an input:
// motors
motorR.attach(MotorRPin);
motorL.attach(MotorLPin);
motorS.attach(MotorSpinPin);
}
void loop(){
while (1==1) {
if (spinCounter < 2) {
motorS.write(160);
}
else {
motorS.write(20);
}
if (spinCounter >= 3) {
spinCounter = 0;
}
spinCounter ++;
checkTheRightWall();
checkTheLeftWall();
if (isARightWallThere == false && isALeftWallThere == false) {
motorR.write(120);
motorL.write(72);
}
else {
if (isARightWallThere == true && isALeftWallThere == true){
motorR.write(0);
motorL.write(180);
delay(300);
motorR.write(80);
motorL.write(80);
delay(500);
motorR.write(0);
motorL.write(180);
}
else {
if (isARightWallThere == false && isALeftWallThere == true) {
motorR.write(50);
motorL.write(90);
}
else {
if (isARightWallThere == true && isALeftWallThere == false) {
motorR.write(90);
motorL.write(140);
}
}
}
}
}
}
void checkTheRightWall () {
digitalWrite(Trig1_pin, LOW);
delayMicroseconds(2);
digitalWrite(Trig1_pin, HIGH);
delayMicroseconds(10);
digitalWrite(Trig1_pin, LOW);
duration1 = pulseIn(Echo1_pin,10);
Serial.println("Duration1: ");
Serial.println(duration1, DEC);
if ((duration1 > 4000 || duration1 == 0)) {
isARightWallThere = false;
}
else {
isARightWallThere = true;
}
}
void checkTheLeftWall() {
digitalWrite(7, LOW);
delayMicroseconds(2);
digitalWrite(7, HIGH);
delayMicroseconds(10);
digitalWrite(7, LOW);
duration2 = pulseIn(Echo2_pin,10);
Serial.println("Duration2: ");
Serial.println(duration2, DEC);
if ((duration2 > 4000 || duration2 == 0)) {
isALeftWallThere = false;
}
else {
isALeftWallThere = true;
}
}
If you have the time, check out one of my friend's pages. He is the one who did all the programming for us.
Here's the link to his page: https://www.instructables.com/member/praznav/
The end,
Spark Industries,
For more detail: Autonomous Autonavigation Robot using Arduino
No comments:
Post a Comment