برنامه‌نویسی آردوینو برای ساخت ربات تشخیص مانع
سورس کد همراه با یک فیلم آموزشی در پایان این پروژه ارائه شده است. این برنامه شامل تنظیم ماژول HC-SR04 و تنظیم سیگنال‌های ارسالی به موتور به منظور حرکت موتور در جهت مناسب است. لازم به ذکر است که از هیچ کتابخانه‌ای در این پروژه استفاده نخواهد شد.
در گام اول پین Trig و پین Echo ماژول HC-SR04 برای اتصال به GPIO9 و GPIO10 بورد آردوینو Nano تعریف می‌شوند.
int trigPin = 9; // trig pin of HC-SR04
int echoPin = 10; // Echo pin of HC-SR04
در این مرحله پین‌های ورودی ماژول درایور موتور LM298N که دارای 4 پایه است، برای کنترل جهت موتور متصل به آن تعیین می‌شود.
int revleft4 = 4; //REVerse motion of Left motor
int fwdleft5 = 5; //ForWarD motion of Left motor
int revright6 = 6; //REVerse motion of Right motor
int fwdright7 = 7; //ForWarD motion of Right motor
در تابع ()setup وضعیت پین‌های GPIO استفاده‌شده را تعیین کنید. چهار پین موتور و پین Trig به عنوان خروجی و پین Echo به عنوان ورودی تنظیم شده‌اند.
pinMode(revleft4, OUTPUT); // set Motor pins as output
pinMode(fwdleft5, OUTPUT);
pinMode(revright6, OUTPUT);
pinMode(fwdright7, OUTPUT);
pinMode(trigPin, OUTPUT); // set trig pin as output
pinMode(echoPin, INPUT); //set echo pin as input to capture reflected waves
در حلقه اصلی برنامه میزان فاصله از شی توسط ماژول HC-SR04 را بدست آورده و براساس آن جهت حرکت موتور را تغییر دهید.
* Obstacle Avoiding Robot Using Ultrasonic Sensor and Arduino NANO
*/
int trigPin = 9; // trig pin of HC-SR04
int echoPin = 10; // Echo pin of HC-SR04
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH); // send waves for 10 us
delayMicroseconds(10);
duration = pulseIn(echoPin, HIGH); // receive reflected waves
distance = duration / 58.2; // convert to distance
delay(10);
اگر فاصله بدست آمده از فاصله مجاز بیشتر باشد به این معناست که در مسیر ربات مانعی وجود ندارد و به صورت مستقیم حرکت می‌کند.
/* if (distance > 19)
{
digitalWrite(fwdright7, HIGH); // move forward
digitalWrite(revright6, LOW);
digitalWrite(fwdleft5, HIGH);
digitalWrite(revleft4, LOW);
}*/
اگر فاصله بدست آمده از فاصله مجاز کمتر باشد نشان دهنده‌ی وجود مانع در مسیر ربات است. در این شرایط ربات مدتی متوقف شده و سپس به سمت عقب حرکت می‌کند. مجددا مدتی متوقف شده و به جهت دیگری حرکت می‌کند.
/* if (distance < 18)
{
digitalWrite(fwdright7, LOW); //Stop
digitalWrite(revright6, LOW);
digitalWrite(fwdleft5, LOW);
digitalWrite(revleft4, LOW);
delay(500);
digitalWrite(fwdright7, LOW); //movebackword
digitalWrite(revright6, HIGH);
digitalWrite(fwdleft5, LOW);
digitalWrite(revleft4, HIGH);
delay(500);
digitalWrite(fwdright7, LOW); //Stop
digitalWrite(revright6, LOW);
digitalWrite(fwdleft5, LOW);
digitalWrite(revleft4, LOW);
delay(100);
digitalWrite(fwdright7, HIGH);
digitalWrite(revright6, LOW);
digitalWrite(revleft4, LOW);
digitalWrite(fwdleft5, LOW);
delay(500);
}
*/
کد کامل برنامه:
/*
int trigPin = 9; // trig pin of HC-SR04
int echoPin = 10; // Echo pin of HC-SR04
int revleft4 = 4; //REVerse motion of Left motor
int fwdleft5 = 5; //ForWarD motion of Left motor
int revright6 = 6; //REVerse motion of Right motor
int fwdright7 = 7; //ForWarD motion of Right motor
long duration, distance;
void setup() {
delay(random(500,2000)); // delay for random time
Serial.begin(9600);
pinMode(revleft4, OUTPUT); // set Motor pins as output
pinMode(fwdleft5, OUTPUT);
pinMode(revright6, OUTPUT);
pinMode(fwdright7, OUTPUT);
pinMode(trigPin, OUTPUT); // set trig pin as output
pinMode(echoPin, INPUT); //set echo pin as input to capture reflected waves
}
void loop() {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH); // send waves for 10 us
delayMicroseconds(10);
duration = pulseIn(echoPin, HIGH); // receive reflected waves
distance = duration / 58.2; // convert to distance
delay(10);
// If you dont get proper movements of your robot then alter the pin numbers
if (distance > 19)
{
digitalWrite(fwdright7, HIGH); // move forward
digitalWrite(revright6, LOW);
digitalWrite(fwdleft5, HIGH);
digitalWrite(revleft4, LOW);
}
if (distance < 18)
{
digitalWrite(fwdright7, LOW); //Stop
digitalWrite(revright6, LOW);
digitalWrite(fwdleft5, LOW);
digitalWrite(revleft4, LOW);
delay(500);
digitalWrite(fwdright7, LOW); //movebackword
digitalWrite(revright6, HIGH);
digitalWrite(fwdleft5, LOW);
digitalWrite(revleft4, HIGH);
delay(500);
digitalWrite(fwdright7, LOW); //Stop
digitalWrite(revright6, LOW);
digitalWrite(fwdleft5, LOW);
digitalWrite(revleft4, LOW);
delay(100);
digitalWrite(fwdright7, HIGH);
digitalWrite(revright6, LOW);
digitalWrite(revleft4, LOW);
digitalWrite(fwdleft5, LOW);
delay(500);
}
} */
