طراحی روبات تشخیص موانع با سنسور مافوق صوت

این روبات با استفاده از Arduino Pro mini طراحی شده است. تصاویر مدار و مراحل کار، کاملا روشن و گویا هستند. از یک آردوینو پرو مینی 5 ولت و IC L293D جهت درایو موتورها استفاده شده و به منظور زیبا سازی کار led و اسپیکر هم مورد استفاده قرار گرفته‌اند.

پس از شروع حرکت، با توجه به توابع random که در کد برنامه استفاده شده است، عملکردهای جالب و اتفاقی مانند توقف یا پخش موزیک انجام می‌گردد. همچنین در صورت نزدیک شدن روبات به موانع، حرکت متوقف شده و پس از بازگشت روبات، جهت دیگری به صورت تصادفی انتخاب شده و روبات مجددا در جهت جدید شروع به حرکت می‌نماید.

Arduino Sonar Robot

قطعات الکترونیکی مورد استفاده:

  1.  1 عدد آردوینو پرو مینی
  2.  4 عدد باطری قلمی + جا باطری
  3.  1 عدد بلندگوی کوچک
  4.  LED عدد 1
  5.  1 عدد حسگر مافوق صوت SR-04
  6.  1 عدد مقاومت 470
  7.  1 عدد آی سی L293D
  8.  1 عدد سوییچ
  9.  2 عدد موتور DC ساده
  10.  مقداری پین هدر و کابل

 

قطعات مکانیکی مورد استفاده:

  1.  2 عدد گیربکس اسباب بازی دو چرخ
  2.  1 جعبه پلاستیکی
  3.  چسب فوری

کد زیر مثال ساده و مناسبی است از پیاده‌سازی رفتارهای متفاوت در زبان C برای اجرا بر روی Embed System هایی که دارای حافظه و قدرت پردازشی محدود هستند (مانند Arduino). با توجه به محدودیت‌های موجود، کد مورد استفاده باید تا جای ممکن ساده و دارای پیچیدگی زمانی کمی باشد.

حالات تقسیم state ها و استفاده از متغیرهای selector، امکان سوییچ کردن بین رفتارهای متفاوت را به سادگی فراهم می‌کند.

نکته: کتابخانه NewPing به همراه حسگر sonar ارائه می‌گردد.

 

/************************
* http://blog.mshams.ir *
*************************/

#include <NewPing.h>

//pin numbers
const byte pinMotor1=4, pinMotor2=5, pinMotor3=6, pinMotor4=7;
const byte pinLed=8, pinTone=10, pinSonar1=11, pinSonar2=12;
//sound freq
const int toneFreq = 800;
//sonar distances
const byte minDistance = 42, maxDistance = 80;
//motor states
const byte M_STOP = 0, M_GO = 1, M_BACK = 2, M_RIGHT = 3, M_LEFT = 4, M_RIGHT2X = 5, M_LEFT2X = 6;
//robot states
const byte R_START = 0, R_DRIVE = 1, R_BLOCK = 2, R_TURN = 3;
//detect hangover
const int hangoverWait = 5000;
//block detect go back
const int backDist = 700, turnDist = 200;
//random behaviour
const int randomChance = 300, randomStopWait = 10000, randomTurnWait = 200;

byte state = R_START;
int i, counter = 0;
int dist;
NewPing sonar(pinSonar1, pinSonar2, 3000);

void setup() {
  randomSeed(analogRead(1));

  //init motor pins
  pinMode(pinMotor1, OUTPUT);
  pinMode(pinMotor2, OUTPUT);
  pinMode(pinMotor3, OUTPUT);
  pinMode(pinMotor4, OUTPUT);  
  pinMode(pinSonar1, OUTPUT); 
  pinMode(pinSonar2, INPUT);

  //init LED pin
  pinMode(pinLed, OUTPUT);  
}

void loop() {

  switch (state){
  case R_START:
    Motor(M_GO);
    Melody();
    state = R_DRIVE;
    break;

  case R_DRIVE:
    dist = Pingu();
    if (dist>0 && dist < minDistance){
      Motor(M_STOP);
      state = R_BLOCK;
    }
    else if (random(0, randomChance) == 5) {
      RandomTone();
      Motor(M_STOP);
      delay(randomStopWait); 
      Motor(random(M_BACK, M_LEFT2X+1));
      delay(randomTurnWait);
      Motor(M_GO);
    }
    break;

  case R_BLOCK:
    Beep();
    Motor(M_BACK);
    delay(backDist);
    Motor(random(M_RIGHT2X, M_LEFT2X+1));
    delay(turnDist);
    state = R_TURN;
    break;

  case R_TURN:
    dist = Pingu();
    if (dist==0 || dist >= maxDistance){
      Motor(M_STOP);
      Motor(M_GO); 
      counter = 0;
      Beep();     
      state = R_DRIVE;
    }
    else{
      counter +=1;
      if (counter >= hangoverWait){
        counter = 0;
        Motor(M_STOP);
        RandomTone();
        state = R_BLOCK;
      }
    }
    break;
  }

}

void Blink(byte state){
  digitalWrite(pinLed, state);
}

void Beep(){
  Blink(1);
  SpeakTone(2);
  SpeakTone(1);
  Blink(0);  
}

void Melody(){
  Blink(1);
  for (int i=1; i<10; i++){
    SpeakTone(i);
  }
  Blink(0);
}

void RandomTone(){
  Blink(1);
  for (int i=1; i<10; i++){
    SpeakTone(random(1, 10));
  }
  Blink(0);
}

void SpeakTone(byte note){
  tone(pinTone, toneFreq * note);
  delay(100);
  noTone(pinTone);
}

void Motor(byte state){
  byte m1, m2, m3, m4;

  switch (state){
  case M_STOP: 
    m1 = m2 = m3 = m4 = LOW; 
    break;
  case M_GO: 
    m1=HIGH; 
    m2=LOW;
    m3=HIGH;
    m4=LOW;
    break;
  case M_BACK: 
    m1=LOW; 
    m2=HIGH;
    m3=LOW;
    m4=HIGH;
    break;
  case M_RIGHT: 
    m1=HIGH; 
    m2=LOW;
    m3=LOW;
    m4=LOW;
    break;
  case M_LEFT: 
    m1=LOW; 
    m2=LOW;
    m3=HIGH;
    m4=LOW;
    break;    
  case M_RIGHT2X: 
    m1=HIGH; 
    m2=LOW;
    m3=LOW;
    m4=HIGH;
    break;
  case M_LEFT2X: 
    m1=LOW; 
    m2=HIGH;
    m3=HIGH;
    m4=LOW;
    break;
  }
  digitalWrite(pinMotor1, m1);
  digitalWrite(pinMotor2, m2);
  digitalWrite(pinMotor3, m3);
  digitalWrite(pinMotor4, m4);   
}

int Pingu(){  
  return (sonar.ping_median(5)/29/2);
}

 

About محمد شمس

برنامه‌نویس، طراح انیمیشن و علاقمند به هوش مصنوعی

پاسخ دهید