Wally – IR Detection Robot

My First ever Robot. It tracks objects using Infra-red sensors, and has basic AI that follows and avoid objects.

I will describe the how I made this robot, and upload some videos.

Version 1 (16 Dec 2011):

Version 2 (20 Dec 2011):

—————–The components used ———————-

I actually planned this robot before making
it. I didn’t use all the parts expected. here https://oscarliang.com/ir-…

I only used these compenents:

Arduino

2 x Servo

IR detector

Motor Driver

2 x Motors

4 x AA battery

2 x Wheels

some capacitors/resistors

———————- How did you implement the code ———————-

I divided the robot into 3 functional parts,
write the code and tested each part separately, and finally put them together
and integrate them to work together. For each part see:

———————- Explaining to me the whole idea of this robot ———————-

You should see what this robot can do from the videos. It’s
very simple, and it’s very responsive to interactions, therefore making it very
fun to play with (my friends love it!

=============================================================
update: 
18/12/2011  
 
Before I put them together, I first made a base with a recycled CD.

This is the detailed structure:

So this is the final result:

 
Here is my code:
 
  1. #include
  2. // Pins
  3. byte leftIRPin = A0;
  4. byte rightIRPin = A1;
  5. byte upIRPin = A2;
  6. byte downIRPin = A3;
  7. byte lrServoPin = 2;
  8. byte udServoPin = 3;
  9. byte motorPin1 = 4;
  10. byte motorPin2 = 5;
  11. byte motorPin3 = 6;
  12. byte motorPin4 = 7;
  13. byte irLEDPin = 8;
  14. // Servos
  15. Servo lrServo;
  16. Servo udServo;
  17. int lrServoPos = 1500;
  18. int udServoPos = 1500;
  19. //IR values
  20. int leftValue_amb;
  21. int rightValue_amb;
  22. int upValue_amb;
  23. int downValue_amb;
  24. int leftValue;
  25. int rightValue;
  26. int upValue;
  27. int downValue;
  28. int distance;   // from 0 – 1000, the larger the closer
  29. int minDistance = 165;
  30. int servoAdjust = 25;
  31. int minDif = 30;
  32. void ReadIR(){
  33.   digitalWrite(irLEDPin, HIGH);
  34.   delay(15);
  35.   // total values
  36.   leftValue = analogRead(leftIRPin);
  37.   rightValue = analogRead(rightIRPin);
  38.   upValue = analogRead(upIRPin);
  39.   downValue = analogRead(downIRPin);
  40.   digitalWrite(irLEDPin, LOW);
  41.   delay(15);
  42.   leftValue_amb = analogRead(leftIRPin);
  43.   rightValue_amb = analogRead(rightIRPin);
  44.   upValue_amb = analogRead(upIRPin);
  45.   downValue_amb = analogRead(downIRPin);
  46.   leftValue = leftValue  leftValue_amb;
  47.   rightValue = rightValue  rightValue_amb;
  48.   upValue = upValue  upValue_amb;
  49.   downValue = downValue  downValue_amb;
  50.   distance = (leftValue + rightValue + upValue +downValue)/4;
  51. }
  52. void setup()
  53. {
  54.   pinMode(irLEDPin, OUTPUT);
  55.   pinMode(motorPin1, OUTPUT);
  56.   pinMode(motorPin2, OUTPUT);
  57.   pinMode(motorPin3, OUTPUT);
  58.   pinMode(motorPin4, OUTPUT);
  59.   lrServo.attach(lrServoPin);
  60.   udServo.attach(udServoPin);
  61.   lrServo.writeMicroseconds(lrServoPos);
  62.   udServo.writeMicroseconds(udServoPos);
  63. }
  64. void loop()
  65. {
  66.   ReadIR();
  67.   // =============== Track Object =================
  68.   if (distance > minDistance){
  69.     if ((rightValue  leftValue) > minDif)
  70.       lrServoPos = lrServoPos  servoAdjust ;
  71.     else if ((leftValue  rightValue) > minDif)
  72.       lrServoPos = lrServoPos  + servoAdjust ;
  73.     if ((upValue  downValue) > minDif)
  74.       udServoPos = udServoPos + servoAdjust ;
  75.     else if ((downValue  upValue) > minDif)
  76.       udServoPos =  udServoPos  servoAdjust ;
  77.   }
  78.   /*
  79.   // =============== Track Object =================
  80.   int temp = leftValue – rightValue;
  81.   if (distance > 70){
  82.     if (rightValue > leftValue)
  83.       lrServoPos = lrServoPos – (rightValue – leftValue)/5 ;
  84.     else
  85.       lrServoPos = lrServoPos  + (leftValue – rightValue)/5 ;
  86.     if (upValue > downValue)
  87.       udServoPos = udServoPos + (upValue – downValue)/5 ;
  88.     else
  89.       udServoPos =  udServoPos – (downValue – upValue)/5 ;
  90.   }
  91. */
  92.   lrServoPos = constrain(lrServoPos,600,2300);
  93.   udServoPos = constrain(udServoPos,600,2300);
  94.   lrServo.writeMicroseconds(lrServoPos);
  95.   udServo.writeMicroseconds(udServoPos);
  96.   // =============== turn body =================
  97.   if (lrServoPos < 1100){
  98.     digitalWrite(motorPin1, LOW);
  99.     digitalWrite(motorPin2, HIGH);
  100.     digitalWrite(motorPin3, HIGH);
  101.     digitalWrite(motorPin4, LOW);
  102.   }
  103.   else if (lrServoPos > 1900){
  104.     digitalWrite(motorPin1, HIGH);
  105.     digitalWrite(motorPin2, LOW);
  106.     digitalWrite(motorPin3, LOW);
  107.     digitalWrite(motorPin4, HIGH);
  108.   }
  109.   else{
  110.     digitalWrite(motorPin1, LOW);
  111.     digitalWrite(motorPin2, LOW);
  112.     digitalWrite(motorPin3, LOW);
  113.     digitalWrite(motorPin4, LOW);
  114.   }
  115. delay(10);
  116. }

Summary

1. rebuild Tracking Head with Styrene plastic, to replace the cardboard one.
2. introduced libraries for motor control, and IR sensor, to increase user-friendliness
3. improved object-following algorithm, introduced “object-avoiding” as well.
 
=============================================================
update
18/12/2011

To Make Wally a better robot, I will do some modifications to it.  Let’s get started.

1. My styrene plastic sheets finally arrived. So I re-built my tracking head!

  

 
2. introduced libraries for motor control, and IR sensor, to increase user-friendliness
 
Basically, the IR class simply the process of measuring the reflected IR when IR LEDs are turned on and off.
The Motor driver class provide user friendly interface for user, so instruction of ‘go forward’, ‘turn left’ etc can be used directly instead of changing states of the inputs.
 
 
3. improved object-following code, introduced “object-avoiding” as well.
 
Principles:
 
object-following
switch on and off the IR very rapidly, to compare the difference. If the difference is larger than a minimum reading, means object is near the sensor, and it will compare the reading between up and down, left and right sensors, to adjust the position of the head until the readings are balanced. If the object is getting further, the car will move forward until the reading is within the stable range, and the same when it’s getting too close.
 
When the head turns to a certain angle horizontally, the body turns to that direction to help tracking the object. You might ask what happen if the object moves away when the body is turning? will it keep turning forever? The answer is yes, and i used a small trick here:  the head recovers to a default position when object is not detected.
 
 
object-avoiding
In this function, only the left and right sensors are used. When nothing is detected, the car keeps going forward. if obstacle appears and only affecting one sensor, it will turn. If the obstacle is affecting both sensor (right at the front), the head will wave around and take readings at right and left side, then decide which way to turn. If still, readings are high, it will turn around and go back.
If you want to discuss or share your ideas, you can post something in our forum here.
 
 
// =============================================
// ============== Source Code ====================
// =============================================

IR Sensor h file
Oscar’s projectIR 4 position sensor12/12/2011
decided add in servo control (from attach to actual control)11/12/2011
this library is used to control a 4 position (up down left right) IR sensor,
which contains both emitters and detectors.*/#ifndef WPROGRAM_H
#define WPROGRAM_H
#include “WProgram.h”  //standard types and constants of the Arduino language
#endif

class IRSensor {

private:

// Pins
byte irLEDPin;
byte calON_LEDPin;
byte calOFF_LEDPin;

byte leftIRPin;
byte rightIRPin;
byte upIRPin;
byte downIRPin;

// IR values
int leftValueBG;
int rightValueBG;
int upValueBG;
int downValueBG;

public:

int leftValue;
int rightValue;
int upValue;
int downValue;

int distance;  // from 0 – 1000, the larger the closer

public:

IRSensor(byte _irLEDPin, byte _leftIRPin, byte _rightIRPin, byte _upIRPin, byte _downIRPin);
void ReadIR();

};

 
IR Sensor cpp file:

/*

Oscar’s project

IR 4 position sensor

this library is used to control a 4 position (up down left right) IR sensor,
which contains both emitters and detectors.

*/

#include “IRSensor.h”

IRSensor::IRSensor(byte _irLEDPin, byte _leftIRPin, byte _rightIRPin, byte _upIRPin, byte _downIRPin) {

// setup all parameters

irLEDPin = _irLEDPin;
leftIRPin = _leftIRPin;
rightIRPin = _rightIRPin;
upIRPin = _upIRPin;
downIRPin = _downIRPin;

calON_LEDPin = 13;
calOFF_LEDPin = 12;

pinMode(irLEDPin, OUTPUT);
pinMode(calON_LEDPin, OUTPUT);
pinMode(calOFF_LEDPin, OUTPUT);

}

void IRSensor::ReadIR(){

digitalWrite(irLEDPin, HIGH);
delay(10);

// total values
leftValue = analogRead(leftIRPin);
rightValue = analogRead(rightIRPin);
upValue = analogRead(upIRPin);
downValue = analogRead(downIRPin);

digitalWrite(irLEDPin, LOW);
delay(10);

leftValueBG = analogRead(leftIRPin);
rightValueBG = analogRead(rightIRPin);
upValueBG = analogRead(upIRPin);
downValueBG = analogRead(downIRPin);

leftValue = leftValue  leftValueBG;
rightValue = rightValue  rightValueBG;
upValue = upValue  upValueBG;
downValue = downValue  downValueBG;

distance = (leftValue + rightValue + upValue +downValue)/4;

}

 
 
Motor Control h file : 

/*
Oscar’s projectMotor Driver control LibraryThis is used to simply motor operation,
so easy commands such as ‘forward, turn right’ etc
can be used to instruct robot what to do12/12/2011
added basic instructions (directions), with duration as inputs*/

#ifndef WPROGRAM_H
#define WPROGRAM_H
#include “WProgram.h”  //standard types and constants of the Arduino language
#endif

class MotorControl {

private:
byte lcPin1;        // 1A
byte lcPin2;        // 2A
byte rcPin1;        // 4A
byte rcPin2;        // 3A

public:
MotorControl();
MotorControl(byte _lcPin1, byte _lcPin2, byte _rcPin1, byte _rcPin2);

void Forward();
void Forward(int duration);

void Backward();
void Backward(int duration);

void TurnLeft();
void TurnLeft(int duration);

void TurnRight();
void TurnRight(int duration);

void TurnLeftDish();
void TurnLeftDish(int duration);

void TurnRightDish();
void TurnRightDish(int duration);

void Stop();
void Brake();

};

 

Motor Control cpp file : 
#include “MotorControl.h”MotorControl::MotorControl(){lcPin1 = 2;
lcPin2 = 3;
rcPin1 = 4;
rcPin2 = 5;pinMode(lcPin1, OUTPUT);
pinMode(lcPin2, OUTPUT);
pinMode(rcPin1, OUTPUT);
pinMode(rcPin2, OUTPUT);}
MotorControl::MotorControl(byte _lcPin1, byte _lcPin2, byte _rcPin1, byte _rcPin2){lcPin1 = _lcPin1;
lcPin2 = _lcPin2;
rcPin1 = _rcPin1;
rcPin2 = _rcPin2;

pinMode(lcPin1, OUTPUT);
pinMode(lcPin2, OUTPUT);
pinMode(rcPin1, OUTPUT);
pinMode(rcPin2, OUTPUT);

}

void MotorControl::Forward(){
digitalWrite(lcPin1, LOW);
digitalWrite(lcPin2, HIGH);
digitalWrite(rcPin1, LOW);
digitalWrite(rcPin2, HIGH);
}
void MotorControl::Forward(int duration){
digitalWrite(lcPin1, LOW);
digitalWrite(lcPin2, HIGH);
digitalWrite(rcPin1, LOW);
digitalWrite(rcPin2, HIGH);
delay(duration);
Stop();
}

void MotorControl::Backward(){
digitalWrite(lcPin1, HIGH);
digitalWrite(lcPin2, LOW);
digitalWrite(rcPin1, HIGH);
digitalWrite(rcPin2, LOW);
}
void MotorControl::Backward(int duration){
digitalWrite(lcPin1, HIGH);
digitalWrite(lcPin2, LOW);
digitalWrite(rcPin1, HIGH);
digitalWrite(rcPin2, LOW);
delay(duration);
Stop();
}

void MotorControl::TurnLeft(){
digitalWrite(lcPin1, LOW);
digitalWrite(lcPin2, HIGH);
digitalWrite(rcPin1, LOW);
digitalWrite(rcPin2, LOW);
}
void MotorControl::TurnLeft(int duration){
digitalWrite(lcPin1, LOW);
digitalWrite(lcPin2, HIGH);
digitalWrite(rcPin1, LOW);
digitalWrite(rcPin2, LOW);
delay(duration);
Stop();
}

void MotorControl::TurnRight(){
digitalWrite(lcPin1, LOW);
digitalWrite(lcPin2, LOW);
digitalWrite(rcPin1, LOW);
digitalWrite(rcPin2, HIGH);
}
void MotorControl::TurnRight(int duration){
digitalWrite(lcPin1, LOW);
digitalWrite(lcPin2, LOW);
digitalWrite(rcPin1, LOW);
digitalWrite(rcPin2, HIGH);
delay(duration);
Stop();
}

void MotorControl::TurnLeftDish(){
digitalWrite(lcPin1, LOW);
digitalWrite(lcPin2, HIGH);
digitalWrite(rcPin1, HIGH);
digitalWrite(rcPin2, LOW);
}
void MotorControl::TurnLeftDish(int duration){
digitalWrite(lcPin1, LOW);
digitalWrite(lcPin2, HIGH);
digitalWrite(rcPin1, HIGH);
digitalWrite(rcPin2, LOW);
delay(duration);
Stop();
}

void MotorControl::TurnRightDish(){
digitalWrite(lcPin1, HIGH);
digitalWrite(lcPin2, LOW);
digitalWrite(rcPin1, LOW);
digitalWrite(rcPin2, HIGH);
}
void MotorControl::TurnRightDish(int duration){
digitalWrite(lcPin1, HIGH);
digitalWrite(lcPin2, LOW);
digitalWrite(rcPin1, LOW);
digitalWrite(rcPin2, HIGH);
delay(duration);
Stop();
}

void MotorControl::Stop(){
digitalWrite(lcPin1, LOW);
digitalWrite(lcPin2, LOW);
digitalWrite(rcPin1, LOW);
digitalWrite(rcPin2, LOW);
}

void MotorControl::Brake(){
digitalWrite(lcPin1, HIGH);
digitalWrite(lcPin2, HIGH);
digitalWrite(rcPin1, HIGH);
digitalWrite(rcPin2, HIGH);
delay(100);
Stop();
}

 
 
Main Program: 
 
/*Oscar’s IR Robot V2
18 Dec 2011This is an open source project
feel free to modify and distribute*/#ifndef SERVO_H
#define SERVO_H
#include  
#endif#include “IRSensor.h”
#include “MotorControl.h”

IRSensor sensor(8,A0,A1,A2,A3);
MotorControl motors(4,5,6,7);

// modes
enum ModeType { FOLLOWOBJECT, AVOIDOBJECT };
enum ModeType mode = FOLLOWOBJECT;

// Servos
Servo lrServo;
Servo udServo;

byte lrServirLEDPin = 2;
byte udServirLEDPin = 3;

byte followLEDPin = 13;
byte avoidLEDPin = 12;
byte modePin = 11;

int lrServoPos = 1500;
int udServoPos = 1200;

int lrServoPosMin = 600;
int udServoPosMin = 600;

int lrServoPosMax = 2400;
int udServoPosMax = 1800;

int lrServoPosMid = (lrServoPosMin + lrServoPosMax) / 2;
int udServoPosMid = (udServoPosMin + udServoPosMax) / 2;

boolean assending = true;

// ============ Performance Parameters ============

const int distanceMin = 230;
const int servoAdjust = 25;
const int minDif = 30;

const int distanceClosest = 650;
const int distanceFurthest = 450;

// ============= End Of Parameters ============

void setup()
{

pinMode(followLEDPin, OUTPUT);
pinMode(avoidLEDPin,OUTPUT);
pinMode(modePin, INPUT);

lrServo.attach(lrServirLEDPin);
udServo.attach(udServirLEDPin);
lrServo.writeMicroseconds(lrServoPosMid);
udServo.writeMicroseconds(udServoPosMid);

delay(1000);

//Serial.begin(9600);

}

void loop()
{
int modeTemp = digitalRead(modePin);
if (modeTemp == HIGH){
mode = AVOIDOBJECT;
digitalWrite(avoidLEDPin, HIGH);
digitalWrite(followLEDPin, LOW);  // indicate operation
}
else {
mode = FOLLOWOBJECT;
digitalWrite(avoidLEDPin, LOW);
digitalWrite(followLEDPin, HIGH);  // indicate operation
}

if (mode == FOLLOWOBJECT){

FollowObject();
//Serial.print(sensor.distance,DEC);
//Serial.print(lrServoPos,DEC);
//Serial.println(udServoPos, DEC);

// ========= turn body if head turns too much =====

if (lrServoPos < 1100)        motors.TurnLeftDish();
else if (lrServoPos > 1900)   motors.TurnRightDish();

// ========= go forward or backward depends on =====

else {
if (sensor.distance > distanceClosest)
motors.Backward();
else if ((sensor.distance < distanceFurthest) && (sensor.distance > distanceFurthest*0.6))
motors.Forward();
else
motors.Stop();
}
}

else if (mode == AVOIDOBJECT){

AvoidObject();

}

}

// =============== Object tracking ==============
// ==============================================

void FollowObject(){

sensor.ReadIR();

// when Object is detected

if (sensor.distance > distanceMin){

if ((sensor.rightValue  sensor.leftValue) > minDif)
lrServoPos = lrServoPos  servoAdjust ;
else if ((sensor.leftValue  sensor.rightValue) > minDif)
lrServoPos = lrServoPos  + servoAdjust ;

if ((sensor.upValue  sensor.downValue) > minDif)
udServoPos = udServoPos + servoAdjust ;
else if ((sensor.downValue  sensor.upValue) > minDif)
udServoPos =  udServoPos  servoAdjust ;

}

// when Object is NOT detected

else {

// horizontal servo move toward centre
if (lrServoPos < lrServoPosMid  100)       lrServoPos += servoAdjust;
else if (lrServoPos > lrServoPosMid+100)  lrServoPos = servoAdjust;

// vertical servo move toward centre
if (udServoPos < udServoPosMid  100)     udServoPos += servoAdjust;
else if ( udServoPos > udServoPosMid+100) udServoPos = servoAdjust;

}

lrServoPos = constrain(lrServoPos,600,2300);
udServoPos = constrain(udServoPos,600,2300);

lrServo.writeMicroseconds(lrServoPos);
udServo.writeMicroseconds(udServoPos);

delay(20);

}

// =============== Object Avoiding ==============
// ==============================================

void AvoidObject(){

boolean detectedLeft = false;
boolean detectedRight = false;

sensor.ReadIR();

// object not detected
if (sensor.distance < distanceMin){

motors.Forward();

}

// object detected
else {

// check where the obstacle is
if (sensor.leftValue  sensor.rightValue > 50)
motors.TurnLeftDish(700);
else if (sensor.leftValue  sensor.rightValue < 50)
motors.TurnRightDish(700);

// if it’s too large
else {

motors.Stop(); // stop movement to allow analysis

// look around, analyse location of obstacles

// check right side
TurnHead(lrServo, 10000);
sensor.ReadIR();
if (sensor.distance > distanceMin)  detectedRight = true;

// check left side
TurnHead(lrServo, 20000);
sensor.ReadIR();
if (sensor.distance > distanceMin)  detectedLeft = true;

// reset head position
TurnHead(lrServo, 15000);

// if obstacle is too big, back off and turn around
if (detectedLeft && detectedRight){
motors.Backward(500);
motors.TurnRightDish(3000);
}
// if obstacle is mainly on the left, turn a big right 
else if (detectedLeft == true && detectedRight == false)
motors.TurnLeftDish(1400);
// if it’s mainly on the right, turn a big left
else if (detectedLeft == false && detectedRight == true)
motors.TurnRightDish(1400);
// if it’s not at either side, will just turn left a little
else
motors.TurnLeftDish(500);
}
}
}

void TurnHead(Servo &servo, int X, int Y){

int temp = servo.read();
temp = map(temp, 01806002400);

if (temp > X){

for (temp; temp>X; temp=temp30){
servo.writeMicroseconds(temp);
delay(30);
}

}
else {

for (temp; temp<X; temp=temp+30){
servo.writeMicroseconds(temp);
delay(20);
}

}

}

 

 

Leave a Reply

Your email address will not be published. Required fields are marked *

Are you Robot? *

I only check blog comments once or twice a week, if you want a quick reply you can post your question on this forum IntoFPV.com... You might get a faster response from me there (multirotor related only).