20 Dec 2011

"Wally" -- IR Detection Robot enhanced version V2

Result:






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.





First Version of this robot:
http://arduin0.blogspot.com/2011/12/i-will-update-functionality-of-this.html

Let's get rocking.


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.


// =============================================
// ============== 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, 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=temp-30){
      servo.writeMicroseconds(temp);
      delay(30);
    }
 
  }
  else {
 
    for (temp; temp<X; temp=temp+30){
      servo.writeMicroseconds(temp);
      delay(20);
    }
 
  }
 
}