Galileo's Sky

DIY Arduibot (8) --- 自製 Arduino 機器人 (8) --- IR object avoidance

紅外線障礙物迴避



有了紅外線做障礙物偵測, 那 Arduino 機器人就可以具有 障礙物迴避 的能力了.



就如之前所說的, 剛開始時, 我是用 VISHA 大顆的那種 IR receiver module,
後來嫌它大,  所以就換掉了.





這是第一版具有紅外線障礙物偵測的 arduino 機器人.
除了紅外線障礙物偵測外, 還有地面亮度偵測.






後來因為接線太多, 太過雜亂, 所以地面亮度偵測的電路先拆掉,
專心做紅外線的部分, 同時也把 IR receiver module 換成小的.







接收器很容易就收到一些反射光, 所以把 紅外線 LED 用電火布 (絕緣膠帶) 裹一裹,
控制照射範圍,  減少一些誤判的機會





很克難,  很陽春, 但是會動喔!







下面是具有障礙物迴避能力的 arduino 機器人程式,
動作很基本很簡單:

沒有障礙物時, 機器人保持前進,
左前方遇到障礙物時, 機器人順時鐘自旋, 繼續前進;
右前方遇到障礙物時, 機器人逆時鐘自旋, 繼續前進;
左和右前方都偵測到障礙物時, 機器人先後退, 順時鐘自旋, 繼續前進;

因為我把紅外線 LED 的光束限制在很小的範圍內,
所以比較低的障礙物會不容易偵測到, 機器人就會直接給他 勇往直前 衝!衝!衝!


晚點再拍個短片貼上來, 看看 Arduibot 會怎麼跑.

原理很簡單, 要怎麼發揮, 就看個人想像囉!




/******************************************************************
 IR_ObjectDetection

 Arduibot control
 v1.0
 
 Galileo   (http://tw.myblog.yahoo.com/galileo-sky)
 2010/6/6  (The IR object detection is improved.

******************************************************************/
#include <servo.h>



//---------------------------------------------------------------
// Digital OUTPUT/INPUT pins
//---------------------------------------------------------------
#define  PIN_IR_E_RIGHT  6    //IR Emitter
#define  PIN_IR_E_LEFT   7

#define  PIN_IR_R_RIGHT  4    //IR Receiver
#define  PIN_IR_R_LEFT   5

//---------------------------------------------------------------
//
//---------------------------------------------------------------
#define  MAX_POS  180//170
#define  MIN_POS  0//10

//For IR detection
#define  OBJECT_DETECTED  0
#define  OBJECT_NONE      1


#define  DIFF_THRESHOLD  15
#define  DETECT_THRESHOLD  4000
#define  ADJ_TIME        1000        //ms
#define  STOP_TIME        100        //ms

#define  IR_FREQUENCY    38000      //IR carrier frequency


//---------------------------------------------------------------
// Servo objects
//---------------------------------------------------------------
Servo myservos[2];  // create servo object to control a servo
int ledPin =  13;    // LED connected to digital pin 13




//---------------------------------------------------------------
//
//---------------------------------------------------------------
void setup() {
  Serial.begin(9600);

  // initialize the digital pin as an output:
  pinMode(ledPin, OUTPUT);    
 
  myservos[0].attach(2);  // Left  servo
  myservos[1].attach(3);  // Right servo
 
  pinMode(PIN_IR_E_RIGHT, OUTPUT);
  pinMode(PIN_IR_E_LEFT,  OUTPUT);

  pinMode(PIN_IR_R_RIGHT, INPUT);
  pinMode(PIN_IR_R_LEFT,  INPUT);
  digitalWrite(PIN_IR_R_RIGHT, HIGH);
  digitalWrite(PIN_IR_R_LEFT, HIGH);


  //tone(PIN_IR_E_LEFT, IR_FREQUENCY);
 
  // prints title
  Serial.println("Arduibot v0.1");
}


void loop() {
  int    nRightIR, nLeftIR;
  int    nIrResult;

 nLeftIR = CheckLeftIR();
 delay(10);
 nRightIR = CheckRightIR();
 
 nIrResult = nRightIR;
 nIrResult  |= nLeftIR<<1;


 switch (nIrResult) {
   case  0:        //Both
     Stop();
     delay(STOP_TIME);
     Backward();
     delay(ADJ_TIME);
     Stop();
     delay(STOP_TIME);
     RotateLeft();
     delay(ADJ_TIME);
     break;
    
   case  1:        //Left
     Stop();
     delay(STOP_TIME);
     RotateRight();
     delay(ADJ_TIME);
     break;
    
   case  2:        //Right
     Stop();
     delay(STOP_TIME);
     RotateLeft();
     delay(ADJ_TIME);
     break;
    
   case  3:        //None
     Forward();
     //delay(500);
     break;
 }




 Serial.print("nIrResult = "); 
 Serial.println(nIrResult); 

 delay(100);
 
}


void Delay(int d) {
  volatile int  i, j=0;
 
   for (i=0; i<d; i++)
    j +=1;
}


//---------------------------------------------------------------
// Servo control
//---------------------------------------------------------------
void Stop() {
  myservos[0].write(90);          // tell servo to go to position in variable 'servoPos'
  myservos[1].write(90);          // tell servo to go to position in variable 'servoPos'
}

void Forward() {
  myservos[0].write(150);          // tell servo to go to position in variable 'servoPos'
  myservos[1].write(30);          // tell servo to go to position in variable 'servoPos'
}

void Backward() {
  myservos[0].write(30);          // tell servo to go to position in variable 'servoPos'
  myservos[1].write(150);          // tell servo to go to position in variable 'servoPos'
}


void TurnRight() {
  myservos[0].write(150);          // tell servo to go to position in variable 'servoPos'
  myservos[1].write(80);          // tell servo to go to position in variable 'servoPos'
}


void TurnLeft() {
  myservos[0].write(110);          // tell servo to go to position in variable 'servoPos'
  myservos[1].write(30);          // tell servo to go to position in variable 'servoPos'
}



void RotateRight() {
  myservos[0].write(120);          // tell servo to go to position in variable 'servoPos'
  myservos[1].write(120);          // tell servo to go to position in variable 'servoPos'
}

void RotateLeft() {
  myservos[0].write(60);          // tell servo to go to position in variable 'servoPos'
  myservos[1].write(60);          // tell servo to go to position in variable 'servoPos'
}




//---------------------------------------------------------------
// IR sensors
//---------------------------------------------------------------
int CheckLeftIR() {
  int    i;
  //Pin 7
 
  for (i=0; i<20; i++) {
    //digitalWrite(7, HIGH);
    PORTD |= 0x80;
    asm volatile ("nop\n\t"
                  "nop\n\t"
                  "nop\n\t"
                  "nop\n\t"
                  ::);
    //j +=1;
    delayMicroseconds(13);
    //digitalWrite(7, LOW);
    PORTD ^= 0x80;
    asm volatile ("nop\n\t"
                  "nop\n\t"
                  "nop\n\t"
                  "nop\n\t"
                  ::);
    //j +=1;
    delayMicroseconds(13);
  }
 
  return digitalRead(PIN_IR_R_LEFT);
}


int CheckRightIR() {
  int    i;
  //Pin 6
 
  for (i=0; i<20; i++) {
    //digitalWrite(6, HIGH);
    PORTD |= 0x40;
    asm volatile ("nop\n\t"
                  "nop\n\t"
                  "nop\n\t"
                  "nop\n\t"
                  ::);
    //j +=1;
    delayMicroseconds(13);
    //digitalWrite(6, LOW);
    PORTD ^= 0x40;
    asm volatile ("nop\n\t"
                  "nop\n\t"
                  "nop\n\t"
                  "nop\n\t"
                  ::);
    //j +=1;
    delayMicroseconds(13);
  }
 
  return digitalRead(PIN_IR_R_RIGHT);
}