"Wally" -- IR Detection Robot

oscar · August 30, 2012, 6:14pm

I made this robot a while ago, thought it might be nice to share it with you!

Source Code, and Detail:
http://blog.oscarliang.net/wally-ir-detection-robot/

=============================================
16/12/2011

update:

Object following:

Things need to be done:
1. head needs to be rebuild with better material for more smooth performance.
2. coding needs improvement.

today I simply assembled all the parts I built previously. the Main thing I did was on the coding.

the Parts can be found here:

Motor Driver

Servo Head

IR detector

Before I put them together, I first made a base with a recycled CD.

=============================================
20/12/2011

update:

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.

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.

how it works:

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.

// =============================================

// ============== Source Code ====================

// =============================================

IR Sensor h file

/*

Oscar's project

IR 4 position sensor

12/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 project

Motor Driver control Library

This is used to simply motor operation,
so easy commands such as 'forward, turn right' etc
can be used to instruct robot what to do

12/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 2011

This is an open source project
feel free to modify and distribute

*/

#ifndef SERVO_H
  #define SERVO_H
  #include <Servo.h>
#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, 1000, 0);
sensor.ReadIR();
  if (sensor.distance > distanceMin)  detectedRight = true;

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

  // reset head position
TurnHead(lrServo, 1500, 0);

  // 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, 0, 180, 600, 2400);

  if (temp > X){

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

  }
  else {

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

  }

}

Sensors / input devices: IR infra red

This is a companion discussion topic for the original entry at https://community.robotshop.com/robots/show/wally-ir-detection-robot

ahollis70 · August 30, 2012, 10:16pm

Excellent work so far. Would

Excellent work so far. Would love to see more updates.

ofir060 · October 1, 2012, 2:39pm

nice

Nice robot, Could you upload a more detailed picture for how you put the arduino and electronics on the cd?

oscar · October 2, 2012, 7:38am

There you go… let me know if you are still not clear about it.

ofir060 · October 2, 2012, 6:43pm

THANKS

Thank you

me_jef · February 8, 2013, 3:12pm

Nice! but…

Nice! but the libraries won’t work with the new version of arduino… what version did you use to upload it to your arduino board? Because I made a robot simular to it but the program always shows errors ;o

oscar · February 8, 2013, 4:39pm

Thanks Buddy~you are right,

Thanks Buddy~

you are right, I didn’t use the latest Arduino, I used Arduino 0023 which i feel most confortable with.

There is nothing version-dependant in the code, so it should be fairly easy to migrate the code to work in the latest version.

If not, Arduino 0023 should be avaiable on the Arduino official website for download.

thomas143 · February 13, 2013, 11:52am

very kool

like the work well put together