کنترل کننده: پردازنده‌ی مرکزی در این ربات آردوینو UNO است. می‌توانید از هر نوع آردوینو که در دسترس دارید مانند Nano یا Mini نیز استفاده کنید.

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

درایور موتور: اگر موتورDC گیربکسی را انتخاب کرده‌اید باید از درایور موتور DC L293D استفاده کنید. در این آموزش نحوه‌ی راه اندازی و کنترل موتور DC با استفاده از درایور L293D بررسی شده است.

چرخ: اطمینان حاصل کنید که چرخ های مورد استفاده از چسبندگی خوبی دارند. میزان چسبندگی چرخ ها باید به گونه‌ای باشد که روی زمین نلغزند.

شتاب سنج و ژیروسکوپ: بهترین انتخاب شتاب سنج و ژیروسکوپ برای این ربات ماژول IMU شش محوره MPU6050 است. برای آشنایی با راه‌اندازی ماژول MPU6050 توسط آردوینو این آموزش را دنبال کنید.

باتری: باتری مورد استفاده باید تا حد امکان سبک باشد و ولتاژ کاری آن باید بیش از 5 ولت باشد تا بتواند آردوینو را مستقیماً و بدون تقویت کننده ، راه7اندازی کند. بنابراین یک باتری لیتیوم پلیمر 7.4 ولت انتخابی ایده‌آل خواهد بود.

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

بدنه و همچنین چهار قسمت مخصوص نصب موتور نیز باید مونتاژ شوند. برای اتصال موتور ها و بورد از پیچ و مهره 3 میلی‌متری استفاده شده است. بعد از مونتاژ باید به ساختاری که در تصویر نشان داده شده است دست پیدا کنید.

آردوینو و درایور موتور مستقیماً توسط باتری تغذیه می شوند. موتورهای DC می توانند از ولتاژ 5 تا 12 ولت استفاده کنند. تغذیه موتورها توسط درایور نیز تامین می‌شود. تغذیه ماژول IMU توسط آردوینو تامین می‌شود. در جدول و دیاگرام نحوه اتصالات نیز مشخص شده است. ماژول MPU6050 از طریق پروتکل I2C با آردوینو ارتباط برقرار می‌کند. بنابراین باید به پین های I2C آردوینو (A4,A5) متصل شود. موتورهای DC نیز به پین‌های PWM یعنی پین‌های D6 ، D9 ،D10 و D11 متصل می‌شوند. زیرا لازم است با تغییر Duty Cycle سیگنال‌های PWM سرعت موتور DC کنترل شود. 

حال باید برنامه لازم را جهت کنترل ربات تعادلی توسط آردوینو پیاده‌سازی کنیم. در روند برنامه توسط IMU میزان انحراف ربات به سمت جلو یا عقب بررسی می‌شود. سپس اگر به سمت جلو منحرف شده باشد، باید چرخ‌ها را در جهت عقب چرخانده شده و اگر به سمت عقب منحرف شده باشد، باید چرخ‌ها در جهت معکوس نیز بچرخند. در عین حال باید سرعت چرخش چرخ‌ها را نیز کنترل کنیم. اگر ربات از موقعیت مرکزی کمی منحرف شود ، چرخ‌ها به آرامی چرخانده شده و با دور شدن از موقعیت مرکزی ، سرعت آن افزایش می‌یابد. برای دستیابی به این منطق از کنترلر PID استفاده شده است. در این کنترلر موقعیت مرکز را به عنوان set-point و میزان انحراف به عنوان خروجی تعریف می‌شود. برای دانستن موقعیت فعلی ربات از MPU6050 استفاده شده که یک شتاب‌سنج و سرعت سنج شش محوره است. برای به دست آوردن یک موقعیت دقیق توسط این سنسور باید از مقادیر شتاب‌سنج و ژیروسکوپ به صورت همزمان استفاده شود. زیرا مقادیر شتاب نویزپذیر و مقادیر ژیروسکوپ با مشکل انباشتگی مواجه هستند. بنابراین با ترکیب هر دو مقدار به زاویای اویلر (رول ، پیچ و یاو) دست خواهیم یافت که در این ربات تنها مقدار یاو استفاده خواهد شد.

کتابخانه آماده‌ای برای کنترلر PID و محاسبه‌ی مقدار یاو سنسور MPU6050 در دو لینک زیر قرار داده شده و در این برنامه استفاده می‌شود. بنابراین در گام اول باید این دو کتابخانه را به کامپایلر آردوینو IDE اضافه کنید. سورس کد کامل این پروژه در انتهای پروژه قرار داده شده است. در ادامه قسمت های مهم این برنامه توضیح داده خواهد شد.

https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h

https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

ابتدا کتابخانه‌های I2C ، PID و  MPU6050  فراخوانی می‌شوند.

#include "I2Cdev.h"
#include <PID_v1.h> //From https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h
#include "MPU6050_6Axis_MotionApps20.h" //https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

سپس متغیرهای لازم را برای بدست آوردن داده‌ها از سنسور MPU6050 تعریف می‌شوند. مقادیر بردار گرانش و کواترنیون‌ها را خوانده و سپس مقدار پیچ و رول را محاسبه می کنیم. مقادیر نهایی از نوع float در آرایه [ypr[3 ذخیره می‌شوند.

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
 
// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

مهم‌ترین بخش برنامه همین قسمت است که ضرایب کنترلر مشخص می‌شوند. اگردر ربات ساخته شده قطعات به صورت متقارن قرار گرفته باشند و مرکز ثقل به درستی تنظیم شده باشد (که در بیشتر موارد چنین نیست) مقدار set-point  ، 180 خواهد بود. در غیر این صورت در حالیکه ربات را کج کرده اید، مقادیر خروجی را در سریال مانیتور آردوینو مشاهده کنید تا مقدار مطلوب  set-point را پیدا کرده و برا ساس آن پارامترها را تنظیم کنید. مقادیر Kp ، Kd و Ki وابسته به ساختار ربات بوده و برای هر ربات متفاوت است. در پایان این آموزش نحوه تنظیم این مقادیر در یک ویدیو نشان داده شده است.

/*********Tune these 4 values for your BOT*********/
double setpoint= 176; //set the value when the bot is perpendicular to ground using serial monitor.
//Read the project documentation on circuitdigest.com to learn how to set these values
double Kp = 21; //Set this first
double Kd = 0.8; //Set this secound
double Ki = 140; //Finally set this
/******End of values setting*********/

در خط بعدی با وارد کردن متغیرهای ورودی، خروجی،set point، kp ،ki وkd الگوریتم کنترلی PID را راه‌اندازی اولیه می‌کنیم. قبلاً  توسط کد بالا مقادیر set-point، Kp ، Ki و Kd را  تنظیم شده‌اند. مقدار ورودی مقدار فعلی yaw است که از سنسور MPU6050 خوانده شده  و مقدار خروجی مقداری خواهد بود که توسط الگوریتم PID محاسبه می شود. بنابراین اساساً الگوریتم PID مقدار خروجی به ما می دهد که باید برای تصحیح مقدار ورودی، جهت نزدیک شدن مقدار به set point استفاده شود.

PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

در داخل تنظیمات برنامه ماژول MPU6050 را توسط DMP ماژول راه‌اندازی اولیه می‌کنیم. این کار به ما در تلفیق داده‌های شتاب‌سنج و ژیروسکوپ کمک کرده و یک مقدار قابل اعتماد از زوایای رول ، پیچ و یاو را می‌دهد. یك بخشی از كدی كه باید در setup function برنامه تغییر دهید، مقادیر آفست ژیروسکوپ است. هر سنسور MPU6050 مقادیر آفست خاص خود را دارد. می توانید از این دستور استفاده کرده و خط زیر را نیز مطابق با مقدار آفست سنسور خود در برنامه به روز کنید.

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1688);

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

//Initialise the Motor outpu pins
    pinMode (6, OUTPUT);
    pinMode (9, OUTPUT);
    pinMode (10, OUTPUT);
    pinMode (11, OUTPUT);
 
//By default turn off both the motors
    analogWrite(6,LOW);
    analogWrite(9,LOW);
    analogWrite(10,LOW);
    analogWrite(11,LOW)

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

  while (!mpuInterrupt && fifoCount < packetSize)
    {
        //no mpu data - performing PID calculations and output to motors    
        pid.Compute();  
       
        //Print the value of Input and Output on serial monitor to check how it is working.
        Serial.print(input); Serial.print(" =>"); Serial.println(output);
              
        if (input>150 && input<200){//If the Bot is falling
         
        if (output>0) //Falling towards front
        Forward(); //Rotate the wheels forward
        else if (output<0) //Falling towards back
        Reverse(); //Rotate the wheels backward
        }
        else //If Bot not falling
        Stop(); //Hold the wheels still
    }

متغیر خروجی PID نیز سرعت چرخش موتور را مشخص می‌کند. اگر ربات در حالت افتادن قرار بگیرد با چرخش آرام چرخ‌ها اصلاح حرکت انجام می‌شود. اگر این میزان چرخش کم نتواند حرکت را اصلاح کند، سرعت موتور را افزایش می‌دهیم. مقدار چرخش سریع چرخ‌ها توسط ضرایب تناسبی و انتگرالگیر کنترلر (PI) انجام می‌شود. توجه داشته باشید که برای عملکرد معکوس مقدار خروجی  در 1- ضرب شده تا به مقداری مثبت تبدیل شود.

void Forward() //Code to rotate the wheel forward
{
    analogWrite(6,output);
    analogWrite(9,0);
    analogWrite(10,output);
    analogWrite(11,0);
    Serial.print("F"); //Debugging information
}
 
void Reverse() //Code to rotate the wheel Backward 
{
    analogWrite(6,0);
    analogWrite(9,output*-1);
    analogWrite(10,0);
    analogWrite(11,output*-1);
   Serial.print("R");
}
 
void Stop() //Code to stop both the wheels
{
    analogWrite(6,0);
    analogWrite(9,0);
    analogWrite(10,0);
    analogWrite(11,0);
    Serial.print("S");
}

شیوه عملکرد ربات تعادلی

پس از آماده شدن سخت افزار می‌توانید برنامه را بر روی آردوینو آپلود کنید. سریال مانیتور آردوینو را برای باز کرده و مقادیر MPU6050 را بخوانید. اگر همه چیز مطابق آنچه انتظار می‌رفت عمل کند باید تصویر را که در این بخش قرار داده شده مشاهده کنید. در اینجا مقادیر ورودی و خروجی کنترلر PID را در input => output باید مشاهده کنید. اگر ربات در حالت تعادل باشد، مقدار خروجی 0 خواهد بود. مقدار ورودی مقدار فعلی سنسور MPU6050 است. حرف  "F" بیانگر حرکت رو به جلو ربات و "R" بیانگر حرکت معکوس آن است. ویدئوی زیر کارکرد کامل ربات را نشان می‌دهد وهمچنین نحوه اصلاح مقادیر PID را نشان می‌دهد.

کد کامل برنامه:

#include "I2Cdev.h"
#include <PID_v1.h> //From https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h
#include "MPU6050_6Axis_MotionApps20.h" //https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

MPU6050 mpu;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

 

/*********Tune these 4 values for your BOT*********/
double setpoint= 176; //set the value when the bot is perpendicular to ground using serial monitor. 
//Read the project documentation on circuitdigest.com to learn how to set these values
double Kp = 21; //Set this first
double Kd = 0.8; //Set this secound
double Ki = 140; //Finally set this 
/******End of values setting*********/

double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

 

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
    mpuInterrupt = true;
}

void setup() {
  Serial.begin(115200);

  // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

     // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // load and configure the DMP
    devStatus = mpu.dmpInitialize();

    
    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1688); 

      // make sure it worked (returns 0 if so)
    if (devStatus == 0)
    {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
        
        //setup PID
        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(10);
        pid.SetOutputLimits(-255, 255);  
    }
    else
    {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

//Initialise the Motor outpu pins
    pinMode (6, OUTPUT);
    pinMode (9, OUTPUT);
    pinMode (10, OUTPUT);
    pinMode (11, OUTPUT);

//By default turn off both the motors
    analogWrite(6,LOW);
    analogWrite(9,LOW);
    analogWrite(10,LOW);
    analogWrite(11,LOW);
}

 

void loop() {
 
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize)
    {
        //no mpu data - performing PID calculations and output to motors     
        pid.Compute();   
        
        //Print the value of Input and Output on serial monitor to check how it is working.
        Serial.print(input); Serial.print(" =>"); Serial.println(output);
               
        if (input>150 && input<200){//If the Bot is falling 
          
        if (output>0) //Falling towards front 
        Forward(); //Rotate the wheels forward 
        else if (output<0) //Falling towards back
        Reverse(); //Rotate the wheels backward 
        }
        else //If Bot not falling
        Stop(); //Hold the wheels still
        
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    }
    else if (mpuIntStatus & 0x02)
    {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        mpu.dmpGetQuaternion(&q, fifoBuffer); //get value for q
        mpu.dmpGetGravity(&gravity, &q); //get value for gravity
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //get value for ypr

        input = ypr[1] * 180/M_PI + 180;

   }
}

void Forward() //Code to rotate the wheel forward 
{
    analogWrite(6,output);
    analogWrite(9,0);
    analogWrite(10,output);
    analogWrite(11,0);
    Serial.print("F"); //Debugging information 
}

void Reverse() //Code to rotate the wheel Backward  
{
    analogWrite(6,0);
    analogWrite(9,output*-1);
    analogWrite(10,0);
    analogWrite(11,output*-1); 
    Serial.print("R");
}

void Stop() //Code to stop both the wheels
{
    analogWrite(6,0);
    analogWrite(9,0);
    analogWrite(10,0);
    analogWrite(11,0); 
    Serial.print("S");
}