"Wally" -- IR Detection Robot

"Wally" -- IR Detection Robot
"Wally" -- IR Detection Robot

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

"Wally" -- IR Detection Robot

"Wally" -- IR Detection Robot

Thanks for helping to keep our community civil!