برنامهنویسی و کنترل ربات حل Maze
5-1- ایجاد یک برنامه کنترلی جدید
برای برنامهنویسی و کنترل ربات حل Maze در محیط Webots ابتدا یک برنامه کنترلی جدید را از منوی Wizard گزینه New Robot Controler را انتخاب میکنیم. این کار باعث ایجاد فایلهای لازم برای کنترل ربات میشود. بعد از کلیک این گزینه باید نوع زبان برنامهنویسی که در شکل 5-1 نشان داده شده است و نام برنامه کنترلی خود را انتخاب کنیم تا در نهایت یک پنجره جدید در ویرایشگر کدها برای تایپ برنامه باز شود. این پنجره خطاهای برنامه را پیدا میکند و فایلهای لازم را بوسیله دکمه Build میسازد که در پوشه کنترل زیر مجموعه پوشه ربات قرار میگیرد. گاهی لازم است برای محیط هم ویژگیهای خاصی تعریف شود که در همین پنجره بوسیله برنامهنویسی و استفاده از موتور دینامیکی باز یا ode قابل تعریف است. این موتور دارای یک سری از دستورات سطح بالای زبان C برای دسترسی به اجزا ربات است.
شکل 5-1- انخاب زبان برنامهنویسی در ایجاد یک برنامه کنترلی
5-2- طرح کلی برنامه
هر برنامه کنترل ربات از ترتیب یکسانی ساخته شده است. دستو اولیه Wb_robot_init() برای شروع کار ربات لازم است. سپس باید وسایل همراه ربات را معرفی و آنها را روشن نمود. کار بعدی این است که یک حلقه نامحدود که در هر گام زمانی تگرار میشود را ایجاد کرد که این گام زمانی با دستور Wb_robot_step() معرفی میشود که بر حسب میلی ثانیه است. بطور کلی کار حلقهی نامحدود این است که اطلاعات را از سنسور ها میگیرد، آنها را پردازش میکند و خروجی لازم را به موتورها میدهد و نهایتا دستور Wb_robot_step() را فرا میخواند. دستور Wb_robot_cleanup() در پایان برنامه باید درج شود تا خروج از برنامه با پاک کردن حافظهها انجام گیرد.
5-3- اجرای برنامه از دیدگاه Webots
هر ربات در Webots به پوشه کنترلر پروژه نگاه میکند تا فایل کنترلی مورد نظر خود را بیابد. برای مثال اگر برنامه کنترلی نام آن Simple باشد، Webots سعی میکند تا نخست فایل Simple.exe را اجرا کند. اگر چنین فایلی پیدا نشد دنبال فایل Simple.class میگردد و آن را به عنوان یک برنامه جاوا آغاز میکند. اگر این فایل نیز پیدا نشد دنبال فایل Simple.jar میگردد و در نهایت فایل Simple.py را جستجو میکند. اگر این فایل نیز نباشد آنگاه Webots دیگر قادر نیست برنامه کنترلی را آغاز کند و از برنامه کنترلی Void به عنوان پیش فرض استفاده میکند.
5-4- کنترلهای همزمان و غیرهمزمان
هر نوع رباتی کنترل آن یا همزمان است یا غیر همزمان. Webots قبل از اینگه زمان شبیهسازی را جلو ببرد منتظر برنامه کنترلی همزمان میماند. بنابر این یک برنامه کنترلی غیرهمزان ممکن است دیرتر اجرا شود، توصیه میشود از کنترلهای همزمان استفاده کنید و تنها زمانی از کنترل غیر همزمان استفاده کنید که محاسبات ربات بالا باشد و یا محدیدیت کامپیوتری وجود داشته باشد و اینکه از یک شبیه سازی با چندین ربات استفاده میکنید که زمان تاخیرش نامعلوم است مانند شبکه اینترنت.
5-5- خواندن اطلاعات سنسورها
برای جمع آوری اطلاعات سنسور باید فعالیتهای زیر را انجام دهیم:
1- معرفی بشود: که بوسیله دستور Wb_robot_get_device() انجام میشود که وسیله را بر اساس فیلد نامش جستجو میکند. این کار فقط یکبار کافیست اجرا شود آن هم قبل از حلقه اصلی، خود ربات نیز یک وسیله است ولی نیازی به معرفی شدن ندارد چون بطور پیش فرض معرفی شده است.
2- فعال باشد: که با دستور مانند Wb_distance_sensor_enable() فعال میشود. این کار نیز یکبار قبل از حلقه اصلی باید انجام شود. البته اگر قصد غیرفعال کردن آنها را دارید میتوانید در حلقه اصلی این سنسورها را چندین بار فعال و غیر فعال کنید.
3- اجرا شود: که بطور خودکار توسط دستور Wb_robot_step() اجرا میشود.
4- خوانده شود: در نهایت شما میتوانید مقادیر سنسورها را با استفاده از دستور خاص خودش بخوانید مانند: Wb_distance_sensor_get_value() که در حلقه اصلی این دستورات قرار میگیرند.
5-6- کنترل محرکها
یک محرک آسانتر از سنسور کنترل میشود. آنها نیازی به فعال شدن ندارد. برای کنترل یک محرک باید فعالیتهای زیر را انجام دهیم:
1- معرفی بشود: این کار با دستور Wb_robot_get_device() اجرا میشود که محرک را با استفاده از فیلد نامشان جستجو میکند و فرامین را به آن باز میگرداند. این کار فقط باید یکبار قبل از حلقه اصلی انجام شود.
2- تنظیم شود: این کار با دستور تنظیم سرعت یا موقعیت یا شتاب و غیره انجام میشود مانند: Wb_differential_Wheels_set_speed() این دستورات در حلقه اصلی درج میشوند.
3- اجرا شوند: این کار در طول حلقه و بعد از دستور Wb_robot_step() بطور خودکار اجرا میشود.
5-7- موتور کشف برخورد
موتور کشف برخورد قادر به تشخیص برخورد میان دو جسم صلبی میباشد. هدف از این موتور آن است که اشیا با هم تداخل نداشته باشند. این کار بوسیله نیروهای تماسی که دو جسم را دور از هم نگه میدارند انجام میپذیرد. توجه کنید هر جسمی که دارای مرز باشد هنگام برخورد، موتور کشف برخورد آن را تشخیص میدهد و داشتن جرم بدین منظور الزامی نیست.
موتور کشف برخورد فقط با اشیاء مرزی سروکار دارد که در فیلد boundingObject هر جسم صلبی یا هر ربات یا سروو موتوری مشخص میشود. توجه کنید که شی مرزی میتواند بزرگتر از جسم باشد یا اصلا جسمی درون آن نباشد ولی در واقعیت همواره مرز پیرامون محیط جسم قرار دارد و نه بزرگتر از آن. اشیا مرزی میتوانند از یک شکل هندسی با یک گروه از اشیا هندسی مانند کره، مکعب، استوانه، هرم و رویهها درست شده باشند ولی گره extrusion نمیتواند مرز باشد. اگر فیلد مرز شی خالی باشد، هیچ برخوردی کشف نمیشود و همه چیز میتواند از درون هم عبور کند. هر جسم صلبی میتواند شامل اجسام صلب دیگری در قسمت children خود باشد که هر یک مرز خود را دارد. مانند مدل کردن بازوهای مکانیکی.
5-8- برنامه کنترلی ربات حل Maze
برنامه کنترلی برای ربات حل Maze باید اطلاعات محیط را به کمک سنسورهای فاصلهیاب ، قطبنما ، GPS و دوربین دریافت کند و شرایط فعلی خود را در محیط پیدا کند. وابسته به این که ربات در چه مکانی قرار دارد که به کمک سنسور موقعیت جهانی GPS میتوان موقعیت روبات را در محیط شبیهسازی بدست آورد، همچنیین بستگی به اینکه ربات رو به کدام جهت میباشد مانند: شمال، جنوب، شرق و غرب که به کمک سنسور قطب نما میتوان جهت ربات را تعیین کرد، همچنین بستگی به اینکه کدام سمت از ربات دیوار وجود دارد میتوان جهت حرکت ربات را تعیین نمود. و باید در حافظه خود مسیرهایی که رفته است را نگهداری کند که از چه مسیرهایی رفته است که هنگام برگشت از مسیری برود که تاکنون نرفته است و سعی کنیم کوتاهترین مسیر را برای رسیدن به مقصد پیدا کنیم و از آن مسیر به مقصد برسیم.
در این پروژه مقصد را مکانی قرار داده که رنگ کف آن سفید باشد. زمین محیط Maze یک سطح چهارخونهای است که تعداد این خانهها 16 خانه برای طول و 16 خانه برای عرض است و اندازه هرکدام از این خانهها 2سانتی متر برای طول و 2سانتی متر برای عرض میباشد. دیوارههای این محیط رنگی غیر از سفید دارند و طول آنها به آن اندازه بلند میباشد که سنسورهای ربات با آنها برخورد کنند و دیوارها را تشخیص دهند. چراغهایی نیز در این محیط وجود دارد که محیط را روشن کرده اند و دوربین ربات میتواند رنگهای کف را از هم تشخیص بدهد.
5-8-1- فراخوانی وسایل ربات در برنامه
برای فراخوانی وسایل ربات باید فایلهای سرایند یا همان کتابخانهای مربوط به وسایل را در Webots فراخوانی کنیم تا بتوان از دستورات مربط به آن وسیله استفاده کنیم. مانند: دریافت اطلاعات سنسورها و انجام دادن عملی به وسیله عملگر ربات.
وسیله هایی که در این ربات استفاده کردهایم و باید فایل سرایند آنها را به برنامه اعلان کنیم، شامل: چرخهای ربات، سنسور فاصلهیاب، دوربین، سنسور موقعیت جهانی GPS ، قطبنما.
|
#include #include #include #include #include #include #include |
در ابتدا فایل سرایند ربات را اضافه میکنیم که شامل فایلهای کتابخانهای مربوط به دستورات Webots میباشد را به برنامه اضافه میکند و میتوان از دستورات اختصاصی برنامه استفاده کرد. همچنین فایلهای سرایند مربوط به چرخها را باید به برنامه اضافه کنیم که برنامه به کمک آن بتواند چرخها را بچرخاند و ربات به حرکت در بیاید. فایلهای سرایند مربوط به سنسورهای فاصلهیاب را باید به برنامه اضافه کنیم که بتوانیم اطلاعات مربوط به فاصله ربات تا مانع را به برنامه بازگرداند. فایلهای سرایند مربوط به دوربین را به برنامه اضافه میکنیم تا بتوان تصاویر دریافتی از دوربینها را در برنامه برسی کنیم و بتوان اطلاعاتی را از تصاویر دریافتی استخراج کنیم. فایلهای سرایند مربوط به سنسور موقعیت جهانی GPS را به برنامه اضافه میکنیم تا بتوان از اطلاعات موقعیتی خود در برنامه استفاده کرد و عمل مورد نظر خود را در آن موقعیت انجام داد. فایلهای سرایند مربوط به قطبنما را در برنامه فراخوانی میکنیم تا در برنامه بتوانیم اطلاعات مربوط به جهت ربات را از قطب نما دریافت کنیم و بستگی به اینکه ربات رو به چه جهتی است تعیین کنیم چه عملی انجام دهد. از فایل سرایند Stdio نیز برای چاپ گزارشات از اطلاعات داخل برنامه در قسمت Consol استفاده میکنیم.
5-8-2- تعریف ثابتهای برنامه
در هر برنامه یک سری اطلاعاتی وجود دارد که باید در تمام طول برنامه ثابت باشند و تغییر پیدا نکنند و بتوان از اطلاعات آنها در برنامه استفاده کرد. بدین منظور این اطلاعات از نوع ثابت در برنامه تعریف میکنیم که کاربر نتواند آنها را تغییر دهد.
|
#define SPEED 60 #define TIME_STEP 128 |
در این برنامه دو ثابت بیشتر وجود ندارد یکی مربوط به سرعت چرخها و دیگری گام زمانی میباشد که در هرگام اطلاعات مربوط به سنسورها فراخوانی و عمل مورد نظر انجام میشود. گام زمانی باید ضریبی از 64 باشد تا بتواند اطلاعات کامل سنسورها را دریافت و عمل مورد نظر را انجام دهد و به صورت پیش فرض برابر 64 میباشد.
5-8-3- تابع اصلی برنامه
همه برنامههای Webots از یک تابع اصلی تشکیل شدهاند که هنگام فراخوانی برنامه ابتدا این تابع اجرا میشود.
|
int main() { wb_robot_init();
return 0; } |
اسم تابع اصلی برنامه Main میباشد که مقدار بازگشتی آن بطور پیش فرض از نوع عددی میباشد. در ابتدای این تابع یک دستور وجود دارد که وجود آن برای درست اجرا کردن برنامه الزامی میباشد و در پایان یک مقدار عددی مثلا صفر را به تابع اصلی باز میگرداند.
5-8-4- حلقه بی پایان برنامه
در هر برنامه Webos یک حلقه بیپایان وجود دارد که در هر بار فراخوانی آن اطلاعاتی از محیط دریافت میکند و عملی روی محیط انجام میدهد.
|
while(wb_robot_step(TIME_STEP)!=-1) {
} |
این حلقه هیچ وقت پایان پیدا نمیکند و همیشه داخل آن شرطی را قرار میدهیم که همیشه درست میباشد مثلا در مثال بالا گام زمانی که یک ثابت همیشه نا مساوی 1- است و حلقه دوبازه تکرار میشود.
5-8-5- فراخوانی و روشن کردن سنسورهای فاصلهیاب
در برنامه ما به اطلاعات مربوط به فاصله ربات با موانع نیاز داریم و بنابر این سنسورهای فاصله یاب را در برنامه فراخوانی کرده و آنها را فعال کنیم تا بتوان از اطلاعات آنها استفاده کنیم.
|
WbDeviceTag ir0 = wb_robot_get_device("ir0"); WbDeviceTag ir1 = wb_robot_get_device("ir1"); WbDeviceTag ir2 = wb_robot_get_device("ir2"); WbDeviceTag ir3 = wb_robot_get_device("ir3"); wb_distance_sensor_enable(ir0, TIME_STEP); wb_distance_sensor_enable(ir1, TIME_STEP); wb_distance_sensor_enable(ir2, TIME_STEP); wb_distance_sensor_enable(ir3, TIME_STEP); |
در دستورات بالا ابتدا سنسورهای فاصله یاب که در محیط شبیه سازی قرار دارد را در برنامه فراخوانی میکند و آنها را به شیهایی از نوع WbDeviceTag مربوط میکند. به کمک این شی میتوان به اطلاعات سنسور دسترسی پیدا کرد و سنسور مورد نظر را فعال کرد. این عمل به کمک دستورات موجود در فایل کتابخانهای سنسور فاصله یاب انجام میشود. برای فعال کردن سنسور مورد نظر باید گام زمانی برنامه را نیز داشته باشیم.
5-8-6- فراخوانی و روشن کردن دوربین
در برنامه ما تشخیص اینکه ربات در مقصد قرار دارد یا نه از دوربین استفاده میکنیم و تصاویری که به وسیله دوربین گرفته شده است را در برنامه میآوریم تا آنها را آنالیز کنیم بدین منظور نیاز داریم که دوربین را در برنامه فراخوانی و آن را روشن کنیم.
|
WbDeviceTag camera = wb_robot_get_device("camera"); wb_camera_enable(camera, 2 * TIME_STEP); wb_camera_move_window(camera, 10, 50); |
در دستورات بالا ابتدا دوربین را از محیط شبیه سازی فراخوانی میکنیم و آن را در برنامه به یک شی ربط میدهیم. به کمک آن شی دوربین را فعال میکنیم و تصاویر دریافتی را به برنامه انتقال میدهیم. برای فعال کردن باید گام زمانی برنامه را نیز به عنوان آرگومانها قرار دهیم. همچنین موقعیت نمایش تصاویر دوربین را محیط شبیه سازی تعیین میکنیم.
5-8-7- فراخوانی و روشن کردن قطبنما
برای تشخیص جهت ربات در برنامه باید اطلاعات قطب نما را از محیط شبیهسازی به برنامه انتقال دهیم. بدین منظور ابتدا باید قطب نما را در برنامه فراخوانی کنیم و سپس آن را فعال کنیم.
|
WbDeviceTag compass = wb_robot_get_device("compass"); wb_compass_enable(compass, TIME_STEP); |
در کد بالا ابتدا گره قطب نما را از محیط شبیه سازی فراخوانی میکند و آن را به شی در برنامه مرتبط میکند. به کمک آن شی قطب نما را فعال میکنیم و اطلاعات آن را به برنامه انتقال میدهیم. برای فعال کردن قطب نما باید گام زمانی برنامه را نیز داشته باشیم.
5-8-8- فراخوانی و روشن کردن موقعیت یاب جهانی GPS
برای بدست آوردن موقعیت ربات در برنامه باید اطلاعات GPS را از محیط شبیهسازی به برنامه انتقال دهیم و به کمک این اطلاعات موقعیت فعلی ربات در محیط شبیهسازی را به دست آورد و بر اساس اینکه در چه موقعیتی است تعیین کنیم چه عملی انجام دهد. بدین منظور GPS را باید در برنامه فراخوانی کنیم و سپس آن را روشن کنیم تا بتوان از اطلاعات آن استفاده کرد.
|
WbDeviceTag gps = wb_robot_get_device("gps"); wb_gps_enable(gps, TIME_STEP); |
در دستورات بالا ابتدا گره موقعیت یاب جهانی GPS را از محیط شبیهسازی فراخوانی میکنیم و آن را به یک شی در برنامه مرتبط میکنیم. به کمک آن شی و گام زمانی برنامه موقعیت یاب جهانی GPS را فعال میکنیم و از اطلاعات آن برای انجام فعالیتها مختلف استفاده میکنیم.
5-8-9- ایجاد آرایه دو بعدی برای ثبت مسیرهای رفته
برای اینکه مشخص کنیم ربات قبلا از چه مسیرهای رفته که برای مسیرهایی که تاکنون نرفته یا کمتر رفته اولویت بیشتری قرار دهیم استفاده میکنیم. به کمک این روش میتوان کاری کرد ربات زودتر به مقصد برسد و مسیری که قبلا رفته و به مقصد نرسیده را بست. در محیط Maze ما چون از 16 سطر و 16 ستون استفاده شده است میتوان از یک آرایه دو بعدی با 16 سطر و 16ستون میتوان استفاده کرد. اما استفاده از این آرایه ممکن است مشکلاتی را در پی داشته باشد، زیرا این آرایه فقط به اندازه تعداد خانههای داخلی محیط است و اطراف محیط را شامل نمیشود، مقادیر اطراف آن به صورت تصادفی میباشد و ممکن است مقدار منفی یا کمتر از مقدار آن خانه باشد بنابر این اولویت خانه بیرون از محیط بیشتر از داخل محیط میباشد و ربات به طرف دیوار میرود. بنابر این ما از یک آرایه دو بعدی بزرگتر استفاده میکنیم که خانههای اطراف ربات را نیز شامل شود. بنابر این برای محیط Maze ما از یک آرایه دو بعدی با 18 سطر و 18 ستون استفاده میکنیم که خانههای آن طرف دیوار را نیز شامل شود و مقادیر این خانهها را بالا قرار میدهیم که هیچوقت اولویت آن بیشتر از خانههای داخلی نشود.
|
int maze[18][18]={{9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9}, {9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9,9}}; |
در دستورات بالا یک آرایه با نام Maze از نوع عدد صحیح ایجاد کرده و این آرایه شامل 18 سطر و 18 ستون میباشد. مقادیر اولیه آن را برای خانههای اطراف آن مقدار 9 قرار میدهیم که در برنامه الویت کمتری داشته باشد و برای خانههای داخلی مقدار صفر را قرار میدهیم که اولویت بیشتری داشته باشد. برای دسترسی به مقادیر آرایه دو بعدی باید ابتدا شماره سطر را تعیین کنیم و سپس شماره ستون، مثلا برای دسترسی به خانهای در سطر دوم و ستون چهارم باید از دستور Maze[2][4] استفاده کنیم. در خانههای آرایه محور Z نشان دهنده سطر خانهها و محور X نشان دهنده ستون خانهها میباشد بنابر این برای دسترسی به خانههای معادل با خانه موجود در محیط شبیه سازی باید ابتدا موقعیت ربات را در محور Z و سپس موقعیت ربات در محور X را تعیین کنیم. یعنی برای خانهای که در موقعیت دوم محور X قرار دارد و در موقعیت چهارم محور Z به صورت Maze[4][2] عمل میکنیم.
شاید ممکن است با خود بگویید میتوان این کار را با کدهای کمتری و به کمک حلقه تو در تو For یا شرط While تو در تو مقدار دهی اولیه کنیم ولی با این کار این حلقههای تو در تو زمانی زیادی را میگیرد و بیشتر از گام زمانی است بنابر این تمام خانههای آرایه مقدار اولیه نمیگیرند و در برنامه ایجاد مشکل میکند. بنابر این مجبوریم که مقدار اولیه آرایه را به صورت دستی وارد کنیم.
5-8-10- دریافت اطلاعات سنسور فاصلهیاب
برای تشخیص فاصله ربات با مانع در برنامه باید اطلاعات مربوط به سنسور فاصله یاب را از محیط دریافت کنیم. اطلاعاتی که سنسور فاصلهیاب میفرستد بر اساس مقادیر موجود در جدول LookupTable سنسور فاصله یاب میباشد. بر اساس این مقدار میتوان فاصله تا مانع را تعیین نمود و عمل مناسب را انجام داد.
|
double ir0_value = wb_distance_sensor_get_value(ir0); double ir1_value = wb_distance_sensor_get_value(ir1); double ir2_value = wb_distance_sensor_get_value(ir2); double ir3_value = wb_distance_sensor_get_value(ir3); |
کد بالا اطلاعات سنسورها را به کمک شی که سنسور را به آن مرتبط کردهایم در برنامه دریافت میکند. مقادیر این سنسورها را در شی از نوع Double قرار میدهیم. میتوان از این شی برای شرایط مختلف استفاده کرد که در فاصلههای مختلف عملیات مختلفی انجام دهد.
5-8-11- دریافت تصاویر از دوربین
برای تعیین اینکه ربات به مقصد رسیده است یا خیر باید تصاویر دوربین را در برنامه دریافت کنیم.
|
const unsigned char *image = wb_camera_get_image(camera); |
کد بالا تصاویر را از دوربین دریافت میکند به کمک شی که به گره دوربین ربط دادیم. تصویر دریافتی را در ثابتی با نام Image از نوع کاراکتری بدون علامت یا Unsigned Char استفاده میکنیم و از این تصویر میتوان برای تشخیص شی که در جلو ربات قرار دارد استفاده کرد.
5-8-12- بررسی تصاویر دوربین برای تشخیص مقصد
در این پروژه مقصد مکانی میباشد که رنگ کف آن سفید است، بنابر این ما باید به دنبال رنگ سفید در تصاویر باشیم و هنگامی که رنگ سفید را پیدا کرد متوقف شود. در Webots تصاویر دریافتی را میتوان به سه رنگ قرمز، سبز و آبی یا همان RGB تفکیک کرد. مقادیر هر کدام از این رنگها از صفر میباشد تا 125 و هرچه مقدار آن بیشتر باشد به سفید نزدیک تر میشود. اگر مقدار هر کدام از این سه رنگ بزرگتر از 200 باشد بدین معنی است که در تصویر رنگ سفید زیاد است. و هنگامی که رنگ سفید را پیدا کرد سرعت چرخ ها صفر شود یعنی در جای خود متوقف شود.
|
int x = 0; int y = 0; int flag=0;
while (x < 50) { while (y < 40) { int r = wb_camera_image_get_red(image,50, x, y); int g = wb_camera_image_get_green(image,40, x, y); int b = wb_camera_image_get_blue(image,50, x, y); if (r>200 && g>200 && b>200) { flag=1; } y++; } x++; } |
در دستوات بالا ابتدا سه متغییر را از نوع عدد صحیح تعریف می کنیم. از متغییر X برای برسی پیکسلهای ستونی و Y برای برسی پیکسلهای سطری استفاده میشود و همچنین متغییر Flag نیز برای تعیین اینکه آیا رنگ سفید در تصویر پیدا کرده است یا خیر استفاده میشود. اگر مقدار Flag برابر صفر باشد بدین معنی است که رنگ سفید پیدا نشده، در ابتدا نیز صفر میباشد و اگر مقدارش برابر یک باشد بدین معنی است که رنگ سفید پیدا شده است.
از دو حلقه تو در تو برای برسی کل تصویر استفاده میکنیم، حلقه بیرونی ستونهای تصویر را از ستون صفر تا آخرین ستون بررسی میکند و حلقه داخلی سطرهای تصویر را از سطر صفر تا آخرین سطر برسی میکند، و تصویر دریافتی را به سه رنگ RGB تفکیک و مقدار آنها را بررسی میکند و اگر همزمان سه مقدار رنگ آن بیشتر از 200 باشد مقدار پرچم را برابر یک قرار میدهد یعنی به مقصد رسیده است.
|
if (flag == 1) { left_speed = 0; right_speed = 0; break; } |
در قطعه کد بالا اگر پرچم برابر یک قرار گیرد سرعت چرخ چپ و راست را برابر صفر قرار میدهیم تا ربات متوقف شود و از حلقه بینهایت خارج میشویم، تا برنامه به پایان برسد.
5-8-13- دریافت اطلاعات سنسور قطبنما
برای مشخص شدن جهت ربات در برنامه کنترلی باید اطلاعات مربوط به قطبنما را در برنامه فراخوانی کنیم. در این برنامه ما فقط از محور اول و سوم یا همان محور X و Z استفاده میکنیم. هر کدام از مقادیر این محورها میتواند از 1- تا 1 باشند که اگر مقدار محور X برابر یک باشد جهت شمال مجازی و اگر منفی یک باشد جهت جنوب مجازی را نشان میدهد و همچنین اگر مقدار محور Z بربر یک باشد جهش غرب و اگر منفی یک باشد جهت شرق را نشان میدهد.
|
const double *compass_values = wb_compass_get_values(compass); compass_x = compass_values[0]; compass_z = compass_values[2]; |
در قطعه کد بالا ابتدا اطلاعات قطب نما را از به کمک شی که به گره قطب نما ربط دادهایم در برنامه دریافت میکنیم و در ثابتی با نام Compass_value از نوع Double قرار میدهید. این ثابت دارای سه مقدار میباشد که مقدار اولی آن مربوط به محور X ، مقدار دوم مربوط به محور Y، مقدار سوم آن مربوط به محور Z میباشد. مقادیر این محورها را در متغییرهای از نوع Double که در اول برنامه اعلان کردهایم قرار میدهیم تا در طول برنامه از آنها برای تشخیص جهت ربات استفاده کنیم.
5-8-14- دریافت اطلاعات سنسور موقعیت جهانی GPS
برای دریافت موقعیت ربات در برنامه کنترلی باید اطلاعات مربوط به سنسور GPS را در برنامه دریافت کنیم. اطلاعات دریافتی شامل اطلاعات مربوط به هرکدام از سه محور اصلی میباشد که چون ارتفاع ربات ما تغییر نمیکند از محور دوم آن یا Y استفاده نمیشود و همیشه برابر صفر میباشد. مقادیر محور اول و سوم در بالا و چپ محیط مجازی برابر صفر و صفر میباشد. هرچه به سمت راست حرکت کنیم مقدار X آن بر اساس متر زیاد میشود و هر چه به سمت پایین حرکت کنیم مقدار Z بر اساس متر زیاد میشود. در محیط مجازی ما طول محیط برابر 2/3 متر و سطر محیط برابر 2/3 متر میباشد. مقادیر محورها در گوشه پایین سمت راست برابر با 2/3 برای محور اول و 2/3 برای محور سوم و همچنین محور دوم نیز صفر میباشد.
|
const double *gps_values = wb_gps_get_values(gps); gps_x_d = gps_values[0] / 0.2; gps_z_d = gps_values[2] / 0.2; gps_x = (int)gps_x_d; gps_z = (int)gps_z_d; |
در دستورات بالا ابتدا اطلاعات سنسور موقعیت جهانی GPS را به کمک شی که به گره GPS ربط دادیم در برنامه دریافت میکنیم و این اطلاعات را در ثابتی با نام GPS_Values از نوع Double قرار میدهیم. مقادیر دریافتی براساس فاصله تا گوشه بالا سمت چپ محیط بر حسب متر میباشد و ما برای تعیین موقعیت ربات که در کدام خانه قرار دارد باید اطلاعات هر محور را بر 0/2 تقسیم کنیم و از مقدار صحیح آن استفاده کنیم. به دلیل اینکه اندازه هر کدام از خانه های محیط برابر 2/0 متر برای طول و 2/0 متر برای عرض میباشد. برای ما مهم نیست که ربات ما در کجای خانه قرار دارد و فقط برای ما مهم است که در کدام خانه قرار دارد از قسمت اعشاری آن صرفه نظر میکنیم. بنابر این خانه بالا سمت چپ در موقعیت صفر و صفر قرار دارد و خانه پایین سمت راست در موقعیت 15 و 15 قرار دارد.
5-8-15- تطبیق موقعیت خانهها با اندیسهای آرایه
برای اینکه مسیرهای رفته ربات را در حافظه آن نگهداریم که دفعه بعد از آنجا نرود باید از هر خانهای که رد میشویم مقدار خانه آرایه معادل با خانه که ربات در آن قرار دارد را یکی به آن اظافه کنیم. از آنجا که ما از یک آرایه با 18 سطر و ستون استفاده کردهایم و خانههای کناری این آرایه را برای بیرون از محیط شبیهسازی در نظر گرفتهایم باید عدد یک را به مقادیر آدرس سطر و ستونها اضافه کنیم. بنابر این موقعیت خانه بالا سمت چپ برابر یک و یک میباشد و خانه پایین سمت راست در موقعیت 16 و 16 قرار دارد.
|
gps_x = gps_x + 1; gps_z = gps_z + 1; |
در کد بالا مقدار GPS محورهای اول و سوم را یکی اضافه میکنیم تا معادل با مکان مورد نظر ما در آرایه دو بعدی باشد. ما می توانیم از دستور ((++ یا پلاس پلاس استفاده کنیم اما این دستور در بعضی مواقع در برنامه Webots ایجاد مشکل میکند و ممکن است یکی به مقدار آن متغییر اضافه نکند بنابر این بهتر است برای اضافه کردن یک واحد به مقدار یک متغییر مانند دستور بالا عمل کنیم.
5-8-16- ثبت مسیر حرکت ربات در آرایه
برای تعیین مسیرهایی که قبلا رفتهایم باید مقدار خانه آرایه دو بعدی معادل آن خانه را یکی واحد اضافه کنیم. برای این کار باید تعیین کنیم که هر بار وارد یک خانه شد فقط یک واحد اضافه کند و تا زمانی که به خانه دیگری نرفتهایم مقادیر آرایه را تغییر ندهد، زیرا در هر واحد زمان که در خانهای قرار دارد یک واحد به خانه معادل آرایه اضافه میکند.
|
if (gps_x_now != gps_x || gps_z_now != gps_z){ maze[gps_z][gps_x]++; gps_x_now = gps_x; gps_z_now = gps_z; } |
در کد بالا ابتدا خانه که در آن قرار داریم را با مختصات GPS مقایسه میکند اگر یکی از محورهای آن مقدارش تغییر کرده باشد یعنی ربات به خانه دیگری رفته باشد، مقدار خانه آرایه دوبعدی معادل خانه جدید را یک واحد اضافه میکنیم و خانه فعلی را برابر خانه جدیدی که در آن قرار داریم میدهیم. با این کار هر باری که به یک خانه جدیدی برود به مقدار آن خانه یکی واحد اضافه میکند.
5-8-17- بدست آوردن موقعیت ربات در هر خانه
برای اینکه تعیین کنیم ربات در کجای خانه تصمیم بگیرد که به کدام جهت برود که هنگام چرخش با دیوارهها برخورد نکند باید موقعیت ربات را در خانه بدست آورد. بهتر است وسط خانهها را برای تصمیم گیری انتخاب کنیم که به دیوارهها برخورد نکند. ما باید موقعیت ربات را در در هر خانه بدست آوریم تا بتوان وسط خانه را پیدا کرد.
|
mid_h_x_i = (gps_values[0] * 1000); mid_h_z_i = (gps_values[2] * 1000); mid_h_x = (mid_h_x_i % 200); mid_h_z = (mid_h_z_i % 200); |
در دستورا ت بالا ابتدا مقادیر GPS محورهای اول و سوم را در 1000 ضرب میکنیم تا رقمهای اعشاری آن به صحیح تبدیل شود، سپس این مقادیر را تقسیم بر 200 میکنیم که مقدار خانهها بین صفر تا 200 باشند. بدین صورت که بالای هر خانه مقدار متغییر وسط خانه محور X آن برابر با صفر و در پایین خانه مقدار متغییر وسط خانه محور X آن برابر با 200 میباشد و همچنین در سمت چپ مقدار متغییر وسط خانه محور Y برابر با صفر میباشد و در سمت راست مقدار متغییر وسط خانه محور Y برابر با 200 میباشد.
5-8-18- انجام یک عمل بیش از یک واحد زمانی
در بعضی موارد باید یک فعالیت رابیش از یک واحد زمانی انجام دهیم، مانند هنگامی که میخواهیم ربات 45 یا 180 درجه بچرخد باید تعیین کنیم که ربات به تعدادی خاصی از واحد زمانی بچرخد که به 45 یا 180 درجه برسد و پس از آن عملیات بعدی را انجام دهد.
|
if (step > 0){ step--; } |
در کد بالا ابتدا برسی میشود اگر در قسمتی از برنامه مقدار متغییر Step را که بیانگر تعداد واحد زمانی که باید یک فعالیت مورد نظر تکرار شود، را مقدار دهی کند در این مرحله سرعت چرخها همان مقدار قبلی میباشد تا گام زمانی تمام شود و یکی از مقدار متغییر واحد زمانی را کم میکنیم.
5-8-19- توقف نیروی گریز از مرکز ربات
در بعضی مواقع هنگامی که ربات کاری را به صورت پشت سر هم عمل میکند سرعت آن بیشتر میشود و برای عمل بعدی منحرف میشود، به عبارتی نیروی گریز از مرکز آن زیاد میشود. مانند هنگامی که ربات میخواهد 45 یا 180 درجه بچرخد سرعت چرخش ربات زیاد میشود و ربات اگر بخواهد بعد از چرخش به صورت مستقیم حرکت کند از مسیر خود منحرف میشود و به دیوارهها برخورد میکند. برای رفع این مشکل باید هنگامی که ربات چرخش خود را انجام داد متوقف شود تا نیروی چرخشی آن متوقف شود و سپس رو به جلو حرکت کند.
|
if (stop > 0){ stop--; left_speed = 0; right_speed = 0; } |
در کد بالا هنگامی که در برنامه مقدار متغییر Stop را بیشتر از صفر قرار دهیم سرعت چرخهای چپ و راست به اندازه یک واحد زمانی متوقف میشود و یک واحد از متغییر توقف کم میکند.
5-8-20- بدون حس عمل کردن
در بعضی مواقع نیاز است که که ربات بدون اینکه از حسگرهای محیط اطلاعاتی را دریافت کند عمل خواصی را انجام دهد. مانند زمانی که ربات چرخش را انجام داده و باید به صورت کاملا مستقیم حرکت کند.
|
if (stolid > 0){ stolid--; left_speed = SPEED; right_speed = SPEED; } |
در قطعه کد بالا اگر کاربر مقدار متغییر Stolid را بیشتر از صفر قرار دهد ربات به اندازه یک واحد زمانی به صورت مستقیم حرکت میکند و سپس مقدار متغییر بدون حس را یکی کم میکنیم.
5-8-21- معکوس کردن عمل
در برخی موارد نیاز میباشد که وقتی یک حرکت را انجام میدهیم بعد از گذشت چند واحد زمانی آن حرکت را عکس کنیم، مانند زمانی که ربات به دیوار نزدیک میشود و میخواهیم ربات از دیوار فاصله بگیرد، سرعت چرخ مخالف را کم میکنیم و هنگامی که ربات از دیوار دور شد، ربات به صورت مستقیم حرکت کند تا به دیواره طرف مقابل برخورد نکند.
|
if (revers > 0){ if (revers == 1){ left_speed = SPEED / 1.005; right_speed = SPEED; }else if (revers == 2){ left_speed = SPEED; right_speed = SPEED / 1.005; } |
در قطعه کد بالا اگر در برنامه مقدار متغییر Revers را بزرگتر از صفر قرار دهد تعیین میکند که نیاز به معگوس کردن یک عمل دارد. اگر مقدار این متغییر برابر یک باشد بدین معنی میباشد که ربات قبلا حرکتی را به راست انجام داده است و حال میخواهد حرکتی را به چپ انجام دهد که ربات در مسیر مستقیم خود قرار گیرد، بدین منظور سرعت چرخ سمت چپ را 1/0 کاهش میدهد، برای این کار سرعت چرخ را تقسیم بر 1/1 میکنیم. اگر مقدار این متغییر برابر 2 باشد بدین معنی میباشد که ربات قبلا حرکتی را به سمت چپ انجام داده است و حال می خواهد حرکتی را به راست انجام دهد که ربات در مسیر مستقیم خود قرار گیرد، بدین منظور سرعت چرخ سمت راست را 1/0 کاهش میدهد، برای این کار سرعت چرخ را تقسیم بر 1/1 میکنیم.
5-8-22- الویت انجام کارها
برای انجام کار بعدی ربات باید مشخص باشد که کدام کار نسبت به دیگری برتری دارد و ابتدا باید کدام کار را انجام دهد. در این اولویت اول با کاری میباشد که میخواهیم تکرار شود مانند تعداد بار چرخش، الویت دوم مربوط به توقف نیروی گریز از مرکز میباشد تا ربات ما از مسیر خود منحرف نشود، الویت سوم با بدون حس حرکت کردن ربات میباشد که فقط روبه جلو حرکت کند و به سنسورها توجهی نداشته باشد، الویت چهارم با معکوس کردن عمل میباشد که ربات به طرف دیگر دیوار نرود و در مسیر مستقیم قرار گیرد و الویت آخر با برسی سنسورها و انتخاب عمل بعدی میباشد.
|
if ((step > 0) || (stop > 0) || (stolid > 0) || (revers > 0)){ if (step > 0){ step--; }else if (stop > 0){ stop--; left_speed = 0; right_speed = 0; }else if (stolid > 0){ stolid--; left_speed = SPEED; right_speed = SPEED; }else if (revers > 0){ if (revers == 1){ left_speed = SPEED / 1.05; right_speed = SPEED; }else if (revers == 2){ left_speed = SPEED; right_speed = SPEED / 1.05; } } }else if ((step <= 0) && (stop <= 0) && (stolid <= 0) && (revers <= 0)){ //sensors check } |
در قطعه کد بالا ابتدا برسی میکند اگر مقادیر یکی از متغییر های Step ، Stop ، Stolid و یا Revers بزرگتر از یک باشد اولویت با این مقادیر میباشد، در غیر این صورت اولویت با چک کردن سنسورها میباشد. در بین متغییرهای اول نیز اولویت دارد، یعنی دو یا بیشتر از دوتای آنها هم زمان بزرگتر از یک باشد ابتدا آن که اولویتش بیشتر است اجرا می شود. اولویت اول با Step میباشد اگر بزرگتر از صفر نبود به ترتیب متغیییرهای Stop ، Stolid و Revers را چک میکند.
5-8-23- عملیات در مکانهای مختلف یک خانه
هنگامی که ربات در مکانهای مختلف یک خانه قرار دارد عملیات مختلفی را انجام می دهد، مثلا در وسط خانه عملیات تصمیم گیری را انجام میدهد که ربات به کدام طرف برود و وقتی که به سمت راست یا چپ میرود از دیوار فاصله میگیرد و همچنین اگر جلوتر یا عقب تر از وسط نیز باشد اگر به دیوار نزدیک شد از دیوار فاصله بگیرد.
|
if ((mid_h_x >= 60 && mid_h_x <= 140) && (mid_h_z >= 60 && mid_h_z <= 140)){ //robot in mid home }else if ((mid_h_x >= 60 && mid_h_x <= 140) && (mid_h_z = 170)){ //robot in O or E }else if ((mid_h_x < 60 || mid_h_x >= 170) && (mid_h_z >=60 && mid_h_z <= 140)){ //robot in N or S } |
در قطعه کد بالا ابتدا مشخص میکنیم زمانی که ربات در وسط خانه قرار دارد چه عملیاتی انجام دهیم که در این پروژه تصمیم گیری انجام میشود که ربات به کدام سمت حرکت کند. همانطور که قبلا نیز گفتیم مقدار متوسط خانهها در دو محور X و Z از صفر شروع میشود و به 200 خطم میشوند و وسط این دو برابر 100 میباشد و چون ربات ما بعد از هر گام زمانی اطلاعات سنسورها را دریافت میکند نمیتوان وسط را دقیقا مشخص کرد بنابر این باید وسط را برای دو محور از یک بازه استفاده کرد مثلا در این پروژه بازه ما برای دو محور از 60 تا 140 میباشد و زمانی که هردو محور در داخل این بازه باشند تصمیم گیری انجام میشود. قسمت دوم زمانی انجام میشود که ربات در داخل بازه محور اول و بیرون از بازه محور سوم باشد. در این حالت ربات ما یا به سمت راست حرکت کرده یا به سمت چپ بنابر این باید ربات را به وسط هدایت کنیم تا به دیواره ها برخورد نکند. در قسمت سوم ربات در بیرون از بازه محور اول و داخل بازه سوم میباشد، بنا بر این ربات در حالتی که به سمت شرق یا غرب حرکت میکند از راه خود منحرف شده و به سمت دیوارهها میرود بنابر این باید ربات را به وسط هدایت کنیم.
5-8-24- بررسی سنسورهای فاصلهیاب
برای تشخص اینکه کدام سمت ما دیوار قرار دارد و کدام سمت ما باز میباشد باید سنسورهای فاصله یاب را بررسی کنیم. از آنجا که ما 4 سنسور فاصله یاب داریم باید تمام شرایطی که ممکن است این سنسورها داشته باشند را بررسی کنیم و تعیین کنیم که کدام سمت ما باز میباشد و می توانیم از آنجا برویم.
|
if (ir0_value >= 400){ if (ir1_value >= 400){ if (ir2_value >= 400){ if (ir3_value >= 400){ }else if (ir3_value < 400){ } }else if (ir3_value >= 400){ }else if (ir3_value < 400){ } }else if (ir2_value >= 400){ if (ir3_value >= 400){ }else if (ir3_value < 400){ } }else if (ir2_value < 400){ if (ir3_value >= 400){ }else if (ir3_value < 400){ } }else if (ir1_value >= 200){ if (ir2_value >= 400){ if (ir3_value >= 400){ }else if (ir3_value < 400){ } }else if (ir3_value >= 400){ }else if (ir3_value < 400){ } }else if (ir1_value < 200){ if (ir2_value >=400){ if (ir3_value >= 400){ }else if (ir3_value < 400){ } }else if (ir3_value >= 400){ }else if (ir3_value < 400){ } } |
در قطعه کد بالا تمام شریطی که 4 سنسور فاصله یاب میتوانند داشته باشند را دارد. سنسور شماره صفر همان سنسور جلو سمت چپ، سنسور شماره یک همان سنسور جلو سمت راست، سنسور شماره 2 سنسور سمت چپ ربات و سنسور شماره 3 سنسور سمت راست ربات میباشد.
5-8-25- بررسی مقادیر سنسور قطبنما
در هر زمانی که میخواهیم تسمیم بگیریم که ربات ما باید به کدام سمت برود باید ابتدا تعیین کنیم که ربات ما رو به کدام جهت میباشد تا بتوان خانههای آرایه معادل را برسی کنیم که کدام الویت بیشتری دارد و برای رفتن به این خانه باید به سمت جلو ، چپ، راست و یا 180درجه بچرخد.
|
if (compass_x >= 0.98){ //N shomal }else if (compass_z <= -0.98){ //S jnob }else if (compass_x <= -0.98){ //E shargh }else if (compass_z >= 0.98){ //O gharb } |
قطعه کد بالا تعیین میکند که ربات رو به کدام جهت میباشد و بسته به این که رو به کدام جهت است چه عملیاتی را باید انجام دهد. جهت ربات همانطور که قبلا نیز گفتیم به کمک مقادیر محور اول و سوم قطبنما بدست میآید و مقادیر هر کدام از آنها از 1- تا 1 می باشد. از آنجایی که ما نمیتوانیم بطور دقیق تعیین که ربات ما رو به کدام جهت میباشد. بنابر این برای تعیین جهت ربات از یک بازه استفاده میکنیم. در این پروژه ما از محدوده 0/98- تا 1- و محدوده 0/98 تا 1 استفاده کردهایم. در قسمت اول عملیاتی را انجام میدهد که باید ربات رو به سمت شمال مجازی باشد که در پروژه ما در جهت مستقیم میباشد، در قسمت دوم عملیاتی را انجام میدهد که باید ربات رو به سمت جنوب باشد، قسمت سوم در شرایطی میباشد که ربات رو به سمت شرق باشد و در قسمت چهارم نیز در شرایطی میباشد که ربات رو به سمت غرب باشد و عملیات مورد نظر را در این قسمتها مینویسیم.
5-8-26- بررسی مقادیر خانههای آرایه
برای تعیین اینکه ربات از کدام مسیر برود که قبلا نرفته و یا حتی کمتر رفته را باید مقادیر خانههای آرایه دو بعدی را بررسی کند. خانههای که در چهار طرف خانه فعلی ربات میباشند را بررسی میکنیم و آن که کمتر است اگر مانعی جلوی آن قرار نداشنه باشد به آن خانه میرود.
|
gps_xp = gps_x + 1; gps_xm = gps_x - 1; gps_zp = gps_z + 1; gps_zm = gps_z - 1; |
از کد بالا برای بررسی مقادیر خانههای اطراف خانه ربات را به کمک محور اول و سوم سنسور GPS استفاده کردهایم. در قسمت اول مقدار محور X آن را یک واحد اضاافه کرده است که معادل با خانه سمت راست ربات میباشد، در قسمت دوم مقدار محور اول را یک واحد کم کرده است که معادل خانه سمت چپ ربات میباشد، در قسمت سوم مقدار محور Z را یک واحد اصافه کرده است که معدل خانه جلویی ربات میباشد و در قسمت آخر یا همان قسمت چهارم مقدار محور سوم را یک واحد کم کرده است، که معادل خانه عقبی ربات میباشد.
ممکن است با خود بگویید که ما میتوانستیم این کار را هنگام مقایسه مقادیر به کمک (++) یا همان پلاس پلاس انجام دهیم، مانند Maze[gps_x++][gps_y] اما این روش در برنامه Webots ایجاد مشکل میکند و اخطار میدهد، بنابر این مجبوریم از روش بالا برای برسی خانههای اطراف ربات استفاده کرد.
5-8-27- مقایسه خانههای آرایه
برای تشخیص اینکه اولویت کدام خانه بیشتر است باید مقادیر این خانهها را با هم مقایسه کنیم و اولویت هر کدام بیشتر است یا به عبارتی مقدار هر کدام از خانه ها کمتر است به آن خانه برویم و اگر اولویت خانهها با هم برابر باشد از اولویت پیش فرض استفاده میکنیم که در آن بیشترین اولویت با خانه جلوی میباشد به دلیل اینکه سرعت حرکت ربات را نمیگیرد و ربات سریعتر حرکت کند و بتواند خانههای بیشتری را بررسی کند، اولویت بعدی با خانه سمت راست، سپس خانه سمت چپ و الویت آخر نیز با خانه عقبی ربات میباشد به این دلیل که ربات زمان زیادی را برای بازگشت صرف میکند و بدترین حالت میباشد که ربات به خانه قبلی خود برود تا به مقصد برسد.
|
if ((maze[gps_zm][gps_x] <= maze[gps_zp][gps_x]) && (maze[gps_zm][gps_x] <= maze[gps_z][gps_xp]) && (maze[gps_zm][gps_x] <= maze[gps_z][gps_xm])){ //olaviat ba jlo }else if ((maze[gps_zp][gps_x] <= maze[gps_zm][gps_x]) && (maze[gps_zp][gps_x] <= maze[gps_z][gps_xp]) && (maze[gps_zp][gps_x] <= maze[gps_z][gps_xm])){ //olaviat ba rast }else if ((maze[gps_z][gps_xp] <= maze[gps_zm][gps_x]) && (maze[gps_z][gps_xp] <= maze[gps_zp][gps_x]) && (maze[gps_z][gps_xp] <= maze[gps_z][gps_xm])){ //olaviat ba chap }else if ((maze[gps_z][gps_xm] <= maze[gps_zm][gps_x]) && (maze[gps_z][gps_xm] <= maze[gps_zp][gps_x]) && (maze[gps_z][gps_xm] <= maze[gps_z][gps_xp])){ //olaviat ba aghab } |
در قطعه کد بالا مقادیر خانههای آرایه دو بعدی معادل با خانههای اطراف ربات را باهم مقایسه میکنند و آنکه اولویتش از همه بیشتر یا مقدارش از همه کمتر باشد اجرا میشود. قسمت اول زمانی اجرا میشود که مقدار خانه روبروی ربات از همه مقادیر دیگر کمتر یا حتی مساوی آنها باشد اجرا میشود یعنی در شرایطی که همه اولویتها نیز یکسان باشد اولویت جلو بیشتر است، قسمت دوم زمانی اجرا میشود که قسمت اول اجرا نشده باشد و مقدار خانه سمت راست ربات از همه کمتر یا مساوی باشد اجرا میشود، قسمت سوم زمانی اجرا میشود که قسمتهای قبلی اجرا نشده باشند و مقدار خانه سمت چپ ربات از همه کمتر یا مساوی باشد اجرا میشود و قسمت آخر یا همان قسمت چهارم زمانی اجرا میشود که هیچ کدام از شرایط قبل برقرار نشده باشند و مقدار خانه پشت سری ربات از همه کمتر یا مساوی باشد اجرا میشود.
به ازای هر بار که مقادیر سنسورهای فاصله یاب را بررسی میکنیم باید تعیین که ربات ما رو به کدام جهت میباشد و همچنین به ازای تمامی این حالات باید بررسی کنیم که الویت کدام یک از خانههای باز بیشتر از بقیه میباشد و آن را انتخاب کنیم و همچنین ربات باید چه عملی را انجام دهد تا بتوان به آن خانه برود. بنابر این تعداد حالتها بسیار زیاد به وجود میآید که باید همه آنها را برسی کرد.
5-8-28- حرکت ربات رو به جلو
برای این که بتوان ربات را به سمت جلو هدایت کرد باید سرعت چرخهای سمت راست و چپ را را برابر هم و مساوی حداکثر سرعت قرار دهیم.
|
left_speed = SPEED; right_speed = SPEED; |
در دستورا ت بالا سرعت چرخهای چپ و راست را برابر حداکثر سرعت چرخها که به صورت یک ثابت که در ابتدای برنامه آن را تعریف کردهایم و برابر سرعت 60 میباشد، قرار میدهیم.
5-8-29- چرخش 45 درجه ربات به راست
برای چرخیدن ربات باید سرعت چرخها سمت چپ و راست را نصف کنیم و و سرعت چرخ سمت راست را منفی کنیم تا ربات به سمت راست بچرخد.
|
left_speed = SPEED / 2; right_speed = -SPEED / 2; step = 10; stop = 3; |
در قطعه کد بالا برای چرخش 45 درجه ربات به سمت راست سرعت چرخ سمت چپ را نصف یعنی تقسیم بر 2 و همچنین سرعت چرخ سمت راست را نیز نصف میکنیم، فقط مقدار سرعت آن را منفی قرار میدهیم که به سمت عقب حرکت کند. با این کا ربات در جای خود به سمت راست میچرخد. برای این که ربات 45 درجه کامل را بچرخد باید تعداد واحد زمانی که ربات باید بچرخد که به 45 درجه برسد را برابر 10 قرار میدهیم، یا به عبارت دیگر مقدار متغییر Step را برابر عدد 10 قرار دهیم. هنگامی که ربات 45 درجه کامل را چرخید یک نیروی گریز از مرکزی در آن ایجاد شده است که هنگامی که میخواهد عملیات بعدی را انجام دهد از مکان خود منحرف میشود برای جلوگیری از این نیرو باید هنگامی که ربات 45 کامل را چرخید در جای خود متوقف شود و سپس کار بعدی خود را انجام دهد برای این کار مقدار متغییر Stop را برابر 3 قرار میدهیم که کاملا متوقف شود.
5-8-30- چرخش 45 درجه به چپ
برای چرخیدن ربات باید سرعت چرخها سمت چپ و راست را نصف کنیم و و سرعت چرخ سمت چپ را منفی کنیم تا ربات به سمت چپ بچرخد.
|
left_speed = -SPEED / 2; right_speed = SPEED / 2; step = 10; stop = 3; |
در قطعه کد بالا برای چرخش 45 درجه ربات به سمت چپ سرعت چرخ سمت راست را نصف میکنیم یعنی تقسیم بر 2 و همچنین سرعت چرخ سمت چپ را نیز نصف میکنیم، فقط مقدار سرعت آن را منفی قرار می دهیم که به سمت عقب حرکت کند. با این کار ربات در جای خود به سمت چپ میچرخد. برای این که ربات 45 درجه کامل را بچرخد باید تعداد واحد زمانی که ربات باید بچرخد که به 45 درجه برسد را برابر 10 قرار میدهیم یا به عبارت دیگر مقدار متغییر Step را برابر عدد 10 قرار دهیم. هنگامی که ربات 45 درجه کامل را چرخید یک نیروی گریز از مرکزی در آن ایجاد شده است، هنگامی که میخواهد عملیات بعدی را انجام دهد از مکان خود منحرف میشود برای جلوگیری از این نیرو با هنگامی که ربات 45 درجه کامل را چرخید در جای خود متوقف شود و سپس کار بعدی خود را انجام دهد برای این کار مقدار متغییر Stop را برابر 3 قرار میدهیم که کاملا متوقف شود.
5-8-31- چرخش 180 درجه به عقب
برای چرخیدن ربات هم میتوان از سمت راست چرخید هم از سمت چپ ولی ما در این پروژه برای چرخش از سمت راست میچرخد. برای چرخیدن ربات باید سرعت چرخ ها سمت چپ و راست را نصف کنیم و سرعت چرخ سمت راست را منفی کنیم تا ربات به سمت راست بچرخد.
|
left_speed = SPEED / 2; right_speed = -SPEED / 2; step = 17; stop = 3; |
در قطعه کد بالا برای چرخش 180 درجه ربات به عقب از سمت راست سرعت چرخ سمت چپ را نصف میکنیم یعنی تقسیم بر 2 و همچنین سرعت چرخ سمت راست را نیز نصف میکنیم فقط مقدار سرعت آن را منفی قرار می دهیم که به سمت عقب حرکت کند. با این کار ربات در جای خود به سمت راست میچرخد. برای این که ربات 180 درجه کامل را بچرخد باید تعداد واحد زمانی که ربات باید بچرخد که به 180 درجه برسد را برابر 17 قرار میدهیم یا به عبارت دیگر مقدار متغییر Step را برابر عدد 17 قرار میدهیم. هنگامی که ربات 180 درجه کامل را چرخید یک نیروی گریز از مرکزی در آن ایجاد شده است که هنگامی که میخواهد عملیات بعدی را انجام دهد از مکان خود منحرف میشود، برای جلوگیری از این نیرو باید هنگامی که ربات 180 کامل را چرخید در جای خود متوقف شود و سپس کار بعدی خود را انجام دهد برای این کار مقدار متغییر Stop را برابر 3 قرار میدهیم که کاملا متوقف شود.
5-8-32- فاصله گرفتن از دیوار
برای جلوگیری از برخرود ربات با دیوار هنگامی که ربات زیادی به دیوار نزدیک شود از آن فاصله بگیرد. این در شرایطی می باشد که در اطراف ربات دیواری وجود داشته باشد.
|
if ((ir3_value >= 200) || (ir2_value >= 200)){ //fasle az divar } |
در قطعه کد بالا بررسی میشود که آیا در سمت راست یا چپ ربات در فاصله 13 سانتی متری دیواری وجود داشته باشد که فاصله از دیوار فعال شود.
حال اگر دیوار وجود داشته باشد باید تعیین کنیم که در کدام طرف ربات دیوار وجود دارد. اگر در دو طرف ربات دیوار وجود داشته باشد هنگامی که به یکی از دیوارها نزدیک شود از آن فاصله بگیرد. اگر فقط در یک طرف دیوار وجود داشته باشد، هنگامی که زیاد به دیوار نزدیک میشود از آن دور شود و اگر زیادی از دیوار فاصله گرفت به دیوار نزدیکتر شود. این کاری که ما انجام میدهیم مانند زمانی است که یک کور میخواهد از بین دیوارها عبور کند دست خود را به دیوار میگیرد که با آن برخورد نکند و همچنین زیادی از آن دور نشود.
|
if((ir3_value >= 200) && (ir2_value < 200)){ //dar samt rast divar vjod dard }else if ((ir2_value >= 200) && (ir3_value < 200)){ //Dar samt chap divar vjod dard }else if ((ir3_value >= 200) && (ir2_value >= 200)){ //dar do taraf divar vjod dard } |
در قطعه کد بالا تعیین میکنیم که در کدام طرف ربات دیوار وجود دارد و اگر در سمت راست ربات دیوار وجود داشته باشد قسمت اول اجرا میشود و هنگام نزدیک شدن به دیوار از آن فاصله گیرد و هنگام دور شدن از دیوار به آن نزدیک شود. اگر در سمت چپ ربات دیوار وجود داشت قسمت دوم اجرا میشود و مانند قسمت قبل از دیوار برای عبور خود استفاده میکند. اگر در دو طرف ربات دیوار وجود داشته باشد از دیوار ها فاصله میگیرد که به دیوارها برخورد نکند.
برای فاصله گرفتن از دیوار سمت راست یا نزدیک شدن به دیوار سمت چپ باید سرعت چرخ سمت چپ را کم کنیم تا ربات به سمت چپ حرکت کند.
|
left_speed = SPEED / 1.005; right_speed = SPEED ; revers = 2; |
در قطعه کد بالا ربات مقداری کمی به سمت چپ میرود. برای رفتن ربات به سمت چپ سرعت چرخ چپ را کم میکنیم مثلا سرعت آن را تقسیم بر 0/005 میکنیم. هنگامی که ربات مقداری به سمت چپ رفت باید از رفتن ربات به سمت دیوار سمت راست جلوگیری کنیم بنابر این مقداری که سرعت چرخها را تغییر دادهایم عکس میکنیم که ربات به صورت مستقیم حرکت کند. این کار با کمک متغییر Revers که در ابتدای برنامه ایجاد کردهایم انجام می شود. هنگامی که مقدار این متغییر برابر 2 باشد حرکت ربات به سمت چپ را معکوس می کند که ربات به صورت مستقیم حرکت کند.
برای فاصله گرفتن از دیوار سمت چپ یا نزدیک شدن به دیوار سمت راست باید سرعت چرخ سمت راست را کم کنیم تا ربات به سمت راست حرکت کند.
|
left_speed = SPEED; right_speed = SPEED / 1.005; revers = 1; |
در قطعه کد بالا ربات مقداری کمی به سمت راست میرود. برای رفتن ربات به سمت راست سرعت چرخ راست را کم میکنیم مثلا سرعت آن را تقسیم بر 0/005 میکنیم. هنگامی که ربات مقداری به سمت راست رفت، باید از رفتن ربات به سمت دیوار سمت چپ جلوگیری کنیم بنابر این مقداری که سرعت چرخها را تغییر دادهایم عکس میکنیم که ربات به صورت مستقیم حرکت کند. این کار به کمک متغییر Revers انجام میشود و اگر برابر یک باشد حرکت ربات به سمت راست را معکوس میکند.
5-8-33- توقف در وسط خانهها
یکی از روشهایی که میتوان از آن استفاده کرد که ربات عملیات خود را به صورت دقیق تر انجام دهد توقف در وسط خانهها میباشد. این روش سرعت عملیاتی ربات را کاهش میدهد و در این پروژه از آن صرف نظر کردهایم ولی گفتین آن خالی از لطف نیست، چون باتوجه به کاهش سرعت ما ولی دقت ربات را زیاد میکند. برای توقف در وسط بستگی دارد که ربات ما رو به کدام جهت حرکت میکند زیرا وقتی میخواهیم ربات را متوقف کنیم در همانجا متوقف نمیشود بلکه کمی جلوتر میرود تا بتواند سرعت خود را به صفر برساند.
|
if (compass_x >= 0.98){ if ((mid_h_x >= 60 && mid_h_x <= 140) && (mid_h_z > 120 && mid_h_z <= 140)){ stop =10; mid_stop = 5; } }else if (compass_z <= -0.98){ if ((mid_h_x > 40 && mid_h_x <= 60) && (mid_h_z >= 60 && mid_h_z <= 140)){ stop =10; mid_stop = 5; } }else if (compass_x <= -0.98){ if ((mid_h_x >= 60 && mid_h_x <= 140) && (mid_h_z > 40 && mid_h_z <= 60)){ stop = 10; mid_stop = 5; } }else if (compass_z >= 0.98){ if ((mid_h_x > 120 && mid_h_x <= 140) && (mid_h_z >= 60 && mid_h_z <= 140)){ stop = 10; mid_stop = 5; } } if (mid_stop >0){ mid_stop--; } |
در قطعه کد بالا ابتدا برسی میکند ربات در کدام جهت حرکت میکند و بسته به اینکه ربات در کدام جهت میباشد، قبل از اینکه به وسط خانه برسد عملیات توقف را انجام میدهد تا زمانی که وسط خانه رسیدیم ربات کاملا متوقف شود. همچنین تعیین میکنیم زمانی که وسط خانه رسیدیم فقط یکبار توقف انجام دهیم.
5-8-34- انتقال سرعت به چرخها
سرعتهای که تاکنون ما برای چرخهای چپ و راست در شریط مختلف انجام دادهایم که از سرعتهای منفی و حداکثر سرعت 60 تشکیل شده است را باید به انتقال دهنده نیروی چرخ ها انتقال دهیم.
|
wb_differential_wheels_set_speed(left_speed, right_speed); |
در دستور بالا مقادیر متغییرهای عددی Left_Speed و Right_Speed را به عنوان سرعت چرخهای چپ و راست تعیین میکنیم.