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, 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); } } } |
