Obstacle Avoiding Robot

This project was ultimately a failure, but one where I learned a great deal. From the outset I wanted to develop a robot that could run around the house, avoiding obstacles using ultrasonic sensors. I didn’t like the look of the other ones I saw online, they were slow and clunky, stopping to sense their environment then moving accordingly. I wanted to have my robot to drive, steer and sense simultaneously, an ambitious task given my limited experience. The result was a robot that can do these tasks, it was however in practice unable to successfully navigate round the house. The sensors were prone to junk readings and their field of view too small.

 

To get multiple processes to happen at once I used 3 arduino nanos, 1 to handle the 4 ultrasonic sensors, 1 to control the steering servo and 1 to control the ESC. To communicate between the arduinos the I2C  functionality on pins A4 and A5 was used ( you will need the I2CAnything library to get the codes below to work).

The steering servo had a type of force feedback. The closer the robot to the wall the further it would try and steer in the opposite direction. This was tricky to figure out but worked really well. I had to map the variables carefully as can be seen in the steering code. I used 2 sensors to control 1 servo.

The ESC was again problematic to set up. You have to first calibrate the ESC porperly. To make the ESC go in reverse you have to set it to zero speed then blip it into reverse otherwise it wont work. You can see how I did this in the motor code below.

 

Overall a fun fiddly and frustrating project. I’ve shared this stuff as someone may find a part of it helpful in their project.

SENSOR CODE:
#include <Wire.h>
#include <I2C_Anything.h>

#define echoPin 7
#define trigPin 8
#define echoPin2 12
#define trigPin2 11
#define echoPin3 9
#define trigPin3 10
#define echoPin4 4
#define trigPin4 5

const byte SERVO_ADDRESS = 42;
const byte MOTOR_ADDRESS = 43;
long duration, distance ;
long duration2, distance2 ;
long duration3, distance3 ;
long duration4, distance4;

void setup()
{
Wire.begin ();
Serial.begin(9600);
pinMode(echoPin, INPUT);
pinMode(trigPin, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin3, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin4, INPUT);
pinMode(trigPin4, OUTPUT);

}

void loop()
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);

digitalWrite(trigPin, HIGH);
delayMicroseconds(10);

digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);

distance = duration / 58.2;

digitalWrite(trigPin2, LOW);
delayMicroseconds(2);

digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);

digitalWrite(trigPin2, LOW);
duration2 = pulseIn(echoPin2, HIGH);

distance2 = duration2 / 58.2;

digitalWrite(trigPin3, LOW);
delayMicroseconds(2);

digitalWrite(trigPin3, HIGH);
delayMicroseconds(10);

digitalWrite(trigPin3, LOW);
duration3 = pulseIn(echoPin3, HIGH);

distance3 = duration3 / 58.2;

digitalWrite(trigPin4, LOW);
delayMicroseconds(2);

digitalWrite(trigPin4, HIGH);
delayMicroseconds(10);

digitalWrite(trigPin4, LOW);
duration4 = pulseIn(echoPin4, HIGH);

distance4 = duration4 / 58.2;

distance = constrain(distance,0,40);
distance2 = constrain(distance2,0,40);
distance3 = constrain(distance3,0,40);
distance4 = constrain(distance4,0,40);

Serial.println(“D1= “);
Serial.println(distance);
Serial.println(“D2 = “);
Serial.println(distance2);
Serial.println(“D3 = “);
Serial.println(distance3);
Serial.println(“D4 = “);
Serial.println(distance4);

// delay(50);
Wire.beginTransmission (SERVO_ADDRESS);
I2C_writeAnything (distance);
I2C_writeAnything (distance2);
I2C_writeAnything (distance3);
I2C_writeAnything (distance4);
Wire.endTransmission ();
Wire.beginTransmission (MOTOR_ADDRESS);
I2C_writeAnything (distance);
I2C_writeAnything (distance2);
I2C_writeAnything (distance3);
I2C_writeAnything (distance4);
Wire.endTransmission ();

delay (200);

}

MOTOR CODE

#include <Servo.h>
//motor slave
#include <Wire.h>
#include <I2C_Anything.h>
Servo myservo;
const byte MY_ADDRESS = 43;

void setup()
{
Wire.begin (MY_ADDRESS);
Serial.begin (9600);
Wire.onReceive (receiveEvent);
myservo.attach(6, 500, 2500); //the pin for the servo control, and range if desired

} // end of setup

volatile boolean haveData = false;
volatile long distance;
volatile long distance2;
volatile long distance3;
volatile long distance4;
void loop()
{
if (distance3 < 3)
{
myservo.write(1500); //stop
delay(2000); // //for 3s
myservo.write(1600);// kick into reverse
delay(500);
myservo.write(1500); // go backwards
delay(500); // for 10s
myservo.write(1600);
delay(3000);
myservo.write(1500);
delay(2000);
}
else
{myservo.write(1400);}

if (distance < 3)
{
myservo.write(1500); //stop
delay(2000); // //for 3s
myservo.write(1600);//blip into reverse
delay(500);
myservo.write(1500); // stop
delay(500); //
myservo.write(1600);//reverse
delay(3000);//for 3s
myservo.write(1500);
delay(2000);
}
else
{myservo.write(1400);}

if (distance2 < 3)
{
myservo.write(1500); //stop
delay(2000); // //for 3s
myservo.write(1600);// kick into reverse
delay(500);
myservo.write(1500); // go backwards
delay(500); // for 10s
myservo.write(1600);
delay(3000);
myservo.write(1500);
delay(2000);
}
else
{myservo.write(1400);}

if (haveData)
{
Serial.print (“Received D1 = “);
Serial.println (distance);
Serial.print (“Received D2 = “);
Serial.println (distance2);
Serial.print (“Received D3 = “);
Serial.println (distance3);
Serial.print (“Received D4 = “);
Serial.println (distance4);
haveData = false;
} // end if haveData

} // end of loop

// called by interrupt service routine when incoming data arrives
void receiveEvent (int howMany)
{
if (howMany >= (sizeof distance) + (sizeof distance2)+ (sizeof distance3)+ (sizeof distance4))
{
I2C_readAnything (distance);
I2C_readAnything (distance2);
I2C_readAnything (distance3);
I2C_readAnything (distance4);
haveData = true;
} // end if have enough data

}

STEERING SERVO

#include <Servo.h>

//servo slave with force feedback
#include <Wire.h>
#include <I2C_Anything.h>
Servo myservo;
unsigned long Timer;
const byte MY_ADDRESS = 42;
long D1;
long D2;
unsigned long interval=3000; // the time we need to wait
unsigned long previousMillis=0; // millis() returns an unsigned long.
int i = 1;

void setup()

{
Wire.begin (MY_ADDRESS);
Serial.begin (9600);
Wire.onReceive (receiveEvent);
myservo.attach(9, 500, 2500); //the pin for the servo control, and range if desired

} // end of setup

volatile boolean haveData = false;
volatile long distance;
volatile long distance2;
volatile long distance3;
volatile long distance4;
void loop()
{
if ( distance3 < 3)
{myservo.write(1500);
delay(6000);}

if ( distance < 600)
{myservo.write(2200);
delay(6000);
myservo.write(1500);}

if ( distance2 > 2400)
{myservo.write(800);
delay(6000);
myservo.write(1500);}

if ( D1 < D2)
{myservo.write(distance);
}

else
{myservo.write(distance2);
}

if (haveData)
{
Serial.print (“Received D1 = “);
Serial.println (distance);
Serial.print (“Received D2 = “);
Serial.println (distance2);
Serial.print (“Received D3 = “);
Serial.println (distance3);
Serial.print (“Received D4 = “);
Serial.println (distance4);
haveData = false;
} // end if haveData
} // end of loop

// called by interrupt service routine when incoming data arrives
void receiveEvent (int howMany)
{
if (howMany >= (sizeof distance) + (sizeof distance2)+ (sizeof distance3)+ (sizeof distance4))
{
I2C_readAnything (distance);
I2C_readAnything (distance2);
I2C_readAnything (distance3);
I2C_readAnything (distance4);
haveData = true;
} // end if have enough data
distance = map(distance, 0, 40 , 500 , 1500);
D1 = map(distance,500,1500,1,50);
D1 = constrain(D1,1,50);

distance2 = map (distance2, 0, 40 , 2500 , 1500);
D2 = map (distance2, 2500,1500,1,50);
D2 = constrain(D2,1,50);
}

// end of receiveEvent

 

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s