ربات خودتعادل با آردوینو در چند قدم ساده

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

پس چطور متعادل می‌مونه؟ همون مثال جارو روی انگشت رو یادت بیار؛ ما انگشتمون رو دقیقا تو همون جهتی که جارو داره میفته حرکت می‌دیم تا زیر مرکز ثقلش قرار بگیره.

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

برای اینکه موتورها درست کار کنن، به اطلاعات دقیقی از وضعیت ربات نیاز داریم. باید بدونیم ربات به کدوم سمت داره میفته، چقدر کج شده و با چه سرعتی داره تعادلش رو از دست می‌ده. تمام این اطلاعات از طریق ماژول MPU6050 به دست میاد.

ما همه این داده‌ها رو با هم ترکیب می‌کنیم و یه سیگنال کنترلی می‌سازیم که موتورها رو هدایت می‌کنه و ربات رو سرپا نگه می‌داره.

+10

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

لایه وسطی جایگاه برد آردوینو، ماژول IMU و رگولاتور افزاینده ولتاژ 5 ولت هست. بالاترین لایه هم به باتری، کلید روشن/خاموش و سنسور فاصله اولتراسونیک اختصاص داره (البته سنسور رو همون آخر کار که تعادل ربات رو گرفتیم نصب می‌کنیم).

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

احتمالا متوجه شدی که برای تغذیه موتورها و برد کنترلر، از دو تا ماژول رگولاتور ولتاژ مجزا استفاده کردیم، حتی با اینکه هر دو به 5 ولت نیاز دارن. این نکته فوق‌العاده مهمیه! اگه از یه رگولاتور مشترک استفاده کنی، نویز شدیدی تو سیستم میفته.

نویز ناشی از کارکرد موتورها باعث می‌شه برنامه آردوینو مدام هنگ کنه. برای جلوگیری از این مشکل، علاوه بر جدا کردن منبع تغذیه‌ها، حتما یه خازن 10 میکروفاراد (10 میکروفاراد) هم به پایه‌های تغذیه موتور اضافه کن تا خیالت از بابت نویز راحت باشه.

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

atan2(y,z) زاویه بین محور مثبت Z و مختصات (z,y) رو برحسب رادیان محاسبه می‌کنه. اگه این زاویه پادساعت‌گرد باشه مثبت و اگه ساعت‌گرد باشه منفی در نظر گرفته می‌شه. ما از

کتابخونه MPU6050 برای خوندن این اطلاعات استفاده می‌کنیم. کد زیر رو آپلود کن تا ببینی زاویه شیب چطور تغییر می‌کنه: #include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" #include "math.h" MPU6050 mpu; int16_t accY, accZ; float accAngle; void setup() { mpu.initialize(); Serial.begin(9600); }

حالا سعی کن ربات رو تو یه زاویه ثابت نگه داری و همزمان به جلو و عقب حرکتش بدی. می‌بینی که زاویه نشون داده شده تو سریال مانیتور یهو تغییر می‌کنه! دلیلش اینه که شتاب افقی حرکت، روی محاسبات شتاب محورهای Y و Z تأثیر می‌ذاره و خطا ایجاد می‌کنه.

ژیروسکوپ 3 محوره MPU6050 سرعت زاویه‌ای (یعنی سرعت چرخش) رو اندازه می‌گیره. برای ربات خودتعادل ما، فقط دونستن سرعت زاویه‌ای تو محور X کافیه تا بفهمیم ربات با چه سرعتی داره به زمین نزدیک می‌شه.

تو کد پایین، ما مقدار ژیروسکوپ رو روی محور X می‌خونیم، به درجه بر ثانیه تبدیلش می‌کنیم و بعد تو زمان اجرای حلقه ضرب می‌کنیم تا میزان تغییر زاویه به دست بیاد. بعد این مقدار رو با زاویه قبلی جمع می‌کنیم تا زاویه فعلی مشخص بشه. #include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" MPU6050 mpu;

وقتی برنامه شروع به کار می‌کنه، موقعیت اولیه MPU6050 به عنوان نقطه صفر (بدون شیب) در نظر گرفته می‌شه و تمام زوایا نسبت به این نقطه سنجیده می‌شن.

اگه ربات رو تو یه زاویه ثابت و بی‌حرکت نگه داری، متوجه می‌شی که عدد زاویه کم‌کم بالا یا پایین می‌ره و ثابت نمی‌مونه. این مشکل به خاطر خطای ذاتی ژیروسکوپ یا همون دریفت (Drift) اتفاق میفته.

تو کدهای بالا، زمان هر حلقه با تابع millis() حساب می‌شد. اما تو قدم‌های بعدی، از وقفه‌های تایمر برای زمان‌بندی کاملا دقیق استفاده می‌کنیم تا بتونیم خروجی بی‌نقصی تو کنترل‌کننده PID داشته باشیم.

همون‌طور که می‌دونی، مکمل بودن یعنی ترکیب کردن دو چیز به شکلی که نقاط قوت همدیگه رو تقویت کنن و ضعف‌ها رو بپوشونن. ما الان دو تا مقدار زاویه از دو منبع مختلف داریم. عدد شتاب‌سنج با حرکت‌های ناگهانی افقی خراب می‌شه و عدد ژیروسکوپ تو گذر زمان دچار خطا می‌شه. به زبون ساده، شتاب‌سنج تو سیگنال‌های کوتاه‌مدت و ژیروسکوپ تو سیگنال‌های طولانی‌مدت آسیب‌پذیر هستن.

این دو تا سنسور دقیقا مکمل هم هستن! پس باید داده‌های این دو تا رو با استفاده از یه فیلتر هوشمندانه ترکیب کنیم. با استفاده از فیلتر مکمل، می‌تونیم یه زاویه کاملا پایدار و دقیق به دست بیاریم. این فیلتر در واقع یه فیلتر بالاگذر برای ژیروسکوپ و یه فیلتر پایین‌گذر برای شتاب‌سنج هست تا نویز و خطاها رو حذف کنه. فرمولش اینه: currentAngle = 0.9934 * (previousAngle + gyroAngle) + 0.0066 * (accAngle)

اعداد 0.9934 و 0.0066 ضرایب این فیلتر برای یه ثابت زمانی 0.75 ثانیه‌ای هستن. فیلتر پایین‌گذر اجازه می‌ده سیگنال‌های طولانی‌تر از این زمان عبور کنن و فیلتر بالاگذر هم سیگنال‌های کوتاه‌تر رو رد می‌کنه. با تغییر این ثابت زمانی، می‌تونی رفتار سیستم رو نرم‌تر یا تیزتر کنی.

اگه این ثابت زمانی رو کمتر کنی، تأثیر شتاب افقی تو محاسبات بیشتر می‌شه و ممکنه رباتت واکنش‌های ناخواسته‌ای نشون بده. برای حذف خطاهای آفست، حتما ماژول MPU6050 رو کالیبره کن. هر خطایی که مربوط به آفست باشه رو می‌تونی تو بخش setup() برنامه‌ات با این دستورات برطرف کنی: mpu.setYAccelOffset(1593); mpu.setZAccelOffset(963); mpu.setXGyroOffset(40);

کلمه PID مخفف سه عبارت تناسبی (Proportional)، انتگرال (Integral) و مشتق (Derivative) هست. هر کدوم از این سه بخش، رفتار خاصی رو برای حفظ تعادل رباتمون ایجاد می‌کنن.

بخش تناسبی یا همون P، یه واکنش متناسب با خطای سیستم تولید می‌کنه. تو پروژه ما، خطای سیستم همون زاویه کج شدن ربات نسبت به حالت عموده.

بخش انتگرال یا I، روی خطاهای جمع‌شده در طول زمان تمرکز داره. این مقدار در واقع مجموع کل خطاها ضرب در زمان نمونه‌برداری هست. این یعنی سیستم به گذشته خودش نگاه می‌کنه تا خطاهای ماندگار رو برطرف کنه.

بخش مشتق یا D، بر اساس سرعت تغییرات خطا عمل می‌کنه. این مقدار از تفاضل خطای فعلی و خطای قبلی تقسیم بر زمان نمونه‌برداری به دست میاد. این قسمت مثل یه پیش‌گو عمل می‌کنه و حدس می‌زنه ربات تو لحظه بعدی چه رفتاری نشون می‌ده.

حالا اگه هر کدوم از این مقادیر رو تو ضریب‌های مخصوص به خودشون (یعنی Kp, Ki و Kd) ضرب کنیم و با هم جمع بزنیم، یه خروجی نهایی به دست میاد. این خروجی همون فرمانیه که به موتورها ارسال می‌شه تا ربات رو سرپا نگه داره.

1. اول از همه مقدار Ki و Kd رو صفر کن. حالا کم‌کم مقدار Kp رو بالا ببر تا جایی که ربات شروع کنه حول نقطه صفر به جلو و عقب نوسان کردن. 2.

حالا نوبت Ki هست. این ضریب رو اون‌قدر زیاد کن تا وقتی ربات از تعادل خارج می‌شه، سریع‌تر واکنش نشون بده. مقدار Ki باید به اندازه‌ای باشه که ربات بتونه سریعا به حالت عمود برگرده و زاویه‌اش بیشتر نشه.

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

4. این مراحل رو چندین بار تکرار کن و هر بار ضرایب رو خیلی ریز و با دقت تغییر بده تا در نهایت رباتت مثل یه مجسمه، پایدار و بی‌نقص سر جاش بایسته.

سنسور اولتراسوینک که من اینجا استفاده کردم مدل US-020 هست. این سنسور چهار تا پین به اسم‌های Vcc, Trig, Echo و Gnd داره و با ولتاژ 5 ولت کار می‌کنه.

پین‌های Trig و Echo رو به ترتیب به پین‌های دیجیتال 9 و 8 آردوینو وصل کن. ما قراره برای راحت‌تر خوندن فاصله‌ها از یه کتابخونه سبک استفاده کنیم.

با کمک کتابخونه NewPing، برنامه رو طوری می‌نویسیم که هر 100 میلی‌ثانیه یه بار فاصله رو بسنجه. اگه فاصله تا مانع روبرو بین 0 تا 20 سانتی‌متر بود، به ربات دستور می‌دیم یه چرخش انجام بده تا مسیرش عوض بشه.

کد Arduino: #include "I2Cdev.h" #include "MPU6050.h" #include "math.h" #include <NewPing.h> #define leftMotorPWMPin 6 #define leftMotorDirPin 7 #define rightMotorPWMPin 5 #define rightMotorDirPin 4 #define TRIGGER_PIN 9 #define ECHO_PIN 8 #define MAX_DISTANCE 75 #define Kp 40 #define Kd 0.05 #define Ki 40 #define sampleTime 0.005 #define targetAngle -2.5

کد Arduino: MPU6050 mpu; NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); int16_t accY, accZ, gyroX; volatile int motorPower, gyroRate; volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0; volatile byte count=0; int distanceCm;

کد Arduino: void setMotors(int leftMotorSpeed, int rightMotorSpeed) { if(leftMotorSpeed >= 0) { analogWrite(leftMotorPWMPin, leftMotorSpeed); digitalWrite(leftMotorDirPin, LOW); } else { analogWrite(leftMotorPWMPin, 255 + leftMotorSpeed); digitalWrite(leftMotorDirPin, HIGH); } if(rightMotorSpeed >= 0) { analogWrite(rightMotorPWMPin, rightMotorSpeed); digitalWrite(rightMotorDirPin, LOW); } else { analogWrite(rightMotorPWMPin, 255 + rightMotorSpeed); digitalWrite(rightMotorDirPin, HIGH); } }

کد Arduino: void init_PID() { // initialize Timer1 cli(); // disable global interrupts TCCR1A = 0; // set entire TCCR1A register to 0 TCCR1B = 0; // same for TCCR1B // set compare match register to set sample time 5ms OCR1A = 9999; // turn on CTC mode TCCR1B |= (1 << WGM12); // Set CS11 bit for prescaling by 8 TCCR1B |= (1 << CS11); // enable timer compare interrupt TIMSK1 |= (1 << OCIE1A); sei(); // enable global interrupts }

کد Arduino: void setup() { // set the motor control and PWM pins to output mode pinMode(leftMotorPWMPin, OUTPUT); pinMode(leftMotorDirPin, OUTPUT); pinMode(rightMotorPWMPin, OUTPUT); pinMode(rightMotorDirPin, OUTPUT); // set the status ال‌ای‌دی to output mode pinMode(13, OUTPUT); // initialize the MPU6050 and set offset values mpu.initialize(); mpu.setYAccelOffset(1593); mpu.setZAccelOffset(963); mpu.setXGyroOffset(40); // initialize PID sampling loop init_PID(); }

کد Arduino: void loop() { // read acceleration and gyroscope values accY = mpu.getAccelerationY(); accZ = mpu.getAccelerationZ(); gyroX = mpu.getRotationX(); // set motor power after constraining it motorPower = constrain(motorPower, -255, 255); setMotors(motorPower, motorPower); // measure distance every 100 milliseconds if((count%20) == 0){ distanceCm = sonar.ping_cm(); } if((distanceCm < 20) && (distanceCm != 0)) { setMotors(-motorPower, motorPower); } } // The ISR will be called every 5 milliseconds ISR(TIMER1_COMPA_vect) { // calculate the angle of inclination accAngle = atan2(accY, accZ)*RAD_TO_DEG; gyroRate = map(gyroX, -32768, 32767, -250, 250); gyroAngle = (float)gyroRate*sampleTime; currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle); error = currentAngle - targetAngle; errorSum = errorSum + error; errorSum = constrain(errorSum, -300, 300); //calculate output from P, I and D values motorPower = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime; prevAngle = currentAngle; // toggle the ال‌ای‌دی on pin13 every second count++; if(count == 200) { count = 0; digitalWrite(13, !digitalRead(13)); } }

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

با این حال، همین ربات کوچیک هم می‌تونه روی سطوح مختلف تعادل خودش رو خیلی خوب حفظ کنه و یه خروجی جذاب بهت بده. امیدوارم از این آموزش لذت برده باشی! حتما این پروژه رو بساز و تجربه‌ات رو با دوستات به اشتراک بذار. تیم آیسی‌مدار همیشه از دیدن پروژه‌های خلاقانه شما انرژی می‌گیره؛ موفق باشی!