حال باید برنامه لازم را جهت کنترل ربات تعادلی توسط آردوینو پیاده‌سازی کنیم. در روند برنامه توسط 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");
}
