Arduino ile Dengede Duran Robot Yapımı (Selfbalancing Bot)

Merhabalar,

Bu projede yabancı bir sayfada gördüğüm ve iki teker üzerinde kendi kendine dengede durabilen ve küçük etkilere karşı dengesini koruyan bir robotu yapmaya çalıştım. En altta ilgili sayfanın linkini ekledim ve arduino kodunu bu sayfadan alarak uyguladım. Önceki projelerimdeki gibi kodu kendim yazmadım. Bu çalışmanın sağlayacağı katkılar bence MPU6050 ivmeölçer sensörünün kullanımını anlamak ve Elektronik Mühendisliğinin önemli konularından biri olan Kontrol Teorisinin temel seviyede anlaşılması olabilir.

Temel olarak kontrol teorisi bir sistemdeki girdi ve çıktı arasındaki farka bakarak sistemdeki hatayı hesaplar.

Girdi - Çıktı = Hata

 Arzu edilen sonuca ulaşmak için sürekli hatayı azaltacak şekilde girdiyi günceller. Bunun için bazı katsayılar kullanır ki bunlara PID diyoruz. Proportional-Derivative ve Integral.
P:Oransal olarak uygulanacak düzeltmenin büyüklüğüdür.
I:Bu düzeltmenin hangi şiddetle uygulanacağıdır.
D:Kaç düzeltme saykılı sonrasında amaca ulaşılmak istendiğidir.

Basit bir örnek vermek gerekirse araçlardaki cruise control tam bu işi yapar. Arzu edilen sürati muhafaza etmek için aracın hızı sürekli ölçülür, yavaşsa gaz verilir, hızlıysa gaz kesilir. İlk başta düzeltme komutları büyükken zamanla arzu edilen hıza yaklaşıldıkça daha küçük düzeltmeler verilir ve sonuçta istenen hız sabitlenir.

Ben burada çok yüzeysel olarak bahsettim ancak oldukça detaylı (derya deniz :) ) bir konu olup buradan detaylara ulaşabilirsiniz.

Metodoloji

Gelelim projemize; iki teker üzerinde otomatik duran araçlar günlük hayatta kaykaylar ve segway gibi bazı örneklerde karşımıza çıkıyor. Aslında bu sistem ters sarkaç mantığı ile çalışır. Yani eğer araç öne devrilecekse aracın ileri gitmesi düzeltici bir komut olur. Tam tersi şekilde geri devrilecekse de geri gitmesi. Yani devrileceği yöne hareket etmesi aksi yönde bir hareket sağlayacağından devrilmeyi sönümler.
Aslında bu konuya en basit örnek parmağımızla bir sopayı dengede tutma işidir. Farkında olmasak da bu işi yaparken sopa ileri devrilecekse elimiz ileri, sağa devrilecekse elimiz sağa doğru gider.
O nedenle biz de robotumuzda MPU6050 sensöründen ölçtüğümüz ileri geri eksenindeki (bu araç sadece öne veya arkaya devrilir sağ sola yıkılamaz) değişimi ve düz durma noktasını kullanarak bunu bir kontrol denklemi içerisindeki PID katsayıları ile işleme sokarak hatayı minimize edecek şekilde motorlara ileri veya geri komutu vereceğiz.

Burada dikkat edilecek noktaları vurgulayacak olursak öncelikle sistemin sağlıklı çalışabilmesi için hassas dönü verebilen motorlar gerekir. Bu iş için en iyi motorlar step motorlardır. DC motorlar için ise encoderli olan yani tur sayısı ölçülebilen motorlar gereklidir. Bu sayede büyük düzeltmelerde güçlü ve büyük, hassas düzeltmelerde ise minimum motor hareketi sağlanabilir. Biz bu projede kolay erişilebilir ve düşük maliyetli olması açısından en basit motorları kullanacağız. Çok hassas bir sonuç elde edemeyecek olsak da sistemin çalıştığını ve yapılabilirliğini kanıtlamış olacak ve kendimize bir rekabet yaratmış olacağız. Çünkü en iyi kaynaklarla zaten bu iş kolay yapılacaktır.

İkinci husus ise mekanik tasarım ve gövde. Öncelikle neden yapılırsa yapılsın gövdenin esnemeyen bir yapıda olması gerekir. Tam tekerlerin merkezinde olmalı ve tüm ekipmanlar gövde üzerinde ağırlık merkezi noktasına göre yerleştirilmelidir. Yoksa zaten doğal durumu dengesiz olan bir sistemi kararlılaştırmak oldukça zor olacak hatta mümkün olmayabilecektir. Ayrıca sistem ne kadar yüksek olursa ve ağırlık merkezi ne kadar yüksekte olursa yaratacağı atalet o kadar fazla olacağı için az ileri veya geri harekette daha fazla denge sağlayıcı kuvvet yaratır. Fakat kuvveti dengelemek için motorlarda da o kadar güç gerekir. O yüzden elimizdeki basit motorların yeterli olması için çok yüksek yapmamakta fayda var. Ayrıca mümkün olduğunca ağır elemanları (pil vb) en alta yerleştirdim ki stabil halde sistemin dengeye ulaşmasına yardımcı olsunlar. (hacıyatmaz gibi) Fakat aslında prensip olarak olması gereken üst kısımda yeterli ataletin yaratılabilmesi için tepede pil gibi ağır ekipmanların kullanılmasıdır.

Son bir husus ise yaptığım denemeler sonucunda gördüğüm kadarı ile IMU sensörlerin elektirksel parazite ve titreşime karşı çok hassas olduğu ve çalışmayı durdurduğu yönde. Eğer motorları ve Arduinoyu aynı kaynaktan beslerseniz motorların yaratacağı parazit IMU sensörün donmasına ve sistemin çalışmamasına neden oluyor. Bu yüzden 2 adet LM2596 ayarlı DC DC voltaj regülatörü kullanarak tek pilden motorları ve arduinoyu ayrı ayrı beslemek veya ikisi için de ayrı pil kullanmak. 2 pil daha çok ağırlık ve yer demek olduğundan ben iki adet voltaj regülatörü kullandım. Ayrıca titreşimi azaltmak için IMU sensör güzel sabitlemeli ve hareket etmemelidir.

Temel mantık olarak MPU6050 sensöründen ölçülen Y ekseni (ileri geri) verilerini sürekli okuyacak ve buna karşılık motorları süreceğiz.

Gerekli Malzemeler
1 adet Arduino Uno/Nano
1 adet motor sürücü devresi
2 adet DC redüksiyonlu motor
1 adet MPU6050 ivmeölçer sensör
2 adet LM2596 DC DC Voltaj Regülatörü
Robot Gövdesi için 12x6cm boyutunda en az iki adet levha (kontraplak, dakota, pleksi vb malzeme olabilir.)
En az 4 adet 4-5cmlik distans veya uygun aralık sağlayacak malzeme (çıta vb)
Vida, kablo vb.
Pil (Lipo olursa hemen bitmez)

Robot Gövdesinin Yapımı
Dakota levhadan 12x6cm boyutlarında iki adet parça keserek kullandım. 3 katlı da yapabilirsiniz ancak ne kadar alçak o kadar daha hafif ve motor gücüne uygun olacaktır. Eğer motorlarınız güçlü ise yüksek yapmak dengeleme açısından avantaj olabilir. Levhaların arasında kullanmak için 4-5 cmlik distans olarak adlandırılan yükseltme parçalarını kullanabilirsiniz. Eğer elinizde yoksa bir çıta veya kurşun kalemi bile kesip sıcak silikonla yapıştırarak kullanabilirsiniz.

İlk önce 3 katlı denedim ama devrilme açısı büyüdükçe verimsiz sarı motorların gücü önlemeye yetmedi. Ya da ben beceremedim. Sonra 2 kata indirip sistemi de Uno yerine nano kullanarak biraz hafifletince daha iyi olduğunu düşünüyorum. Ayrıca tekerlekler de önemli. Sürtünmesi güzel ve geniş tekerlekler daha iyi denge sağlayacaktır.

Temel şekil bu. Sanırım detaya girmeye gerek yok. Elinizde bulunan materyal ile yapabilirsiniz. Ama dediğim gibi gövde rijit olmalı esnek olmamalı. Ayrıca tekerleklerin arasından geçtiğini düşündüğümüz hayali mil de tam ortada olmalı.

 Yukarıdaki resimde tüm elektronikçilerde bulabileceğiniz LM2596 DC DC ayarlı voltaj regüle devresi görülüyor. Mavi komponent üzerinden vida ile çıkış voltajını ayarlayabilir, 1 tanesi ile motorları, diğeri ile 5v ayarlayarak Arduinoyu besleyebilirsiniz. Böylece iki bölümü birbirinden izole etmiş olursunuz.

Ben elimde 1 adet olduğu için LM2596 yı motorları sürmede kullandım. Arduinoyu beslemek için 5v çıkış veren yukarıdaki model uçak BEC devresini kullandım. Önemli olan Arduinoyu 5V pinden besleyecekseniz güvenilir ve stabil 5v veren bir kaynak ile beslemek veya VIN pininden 7-12V arası bir kaynakla beslemek.

Devre Şeması 

MPU6050 sensörü arduinoya bağlamak kolay ancak dikkat edilecek nokta VCC çıkışını arduinoda 3.3V'a bağlamak. Bazı kartların 5v'u da desteklediği yazsa da garanti olması için siz 5v'a bağlamayın zira bağlayıp yakan çok. !!! Yukarıdaki resimde sıra ile VCC--> 3.3v'a, Ground-->GND'ye, SCL>A5'e, SDA--->A4'e ve INT pini ise dijital 2'ye bağlanmalıdır. 

MPU6050 Sensörünüzün Offset Değerlerini Bulma

Her bir cihazın akselerometre ve gyroları için farklı offset değerleri bulunur. Bunları bilip uygulamalarda kullanırsanız daha hassas sonuçlar elde edersiniz. Bunun için linkteki kütüphaneyi indirip arduino klasörü içinde bulunan kütphaneleri eklemeli ve sonra aşağıdaki kodu arduinoya yükleyip seri monitörü açarak klavyeden herhangi bir tuşa basıp sensör kalibrasyonunuzu gerçekleştirebilir ve offset değerlerinizi serial monitörden öğrenerek robot kodundaki yere yazabilirsiniz. Bu kalibrasyon işlemini yaparken robotunuz sabit ve ayakta düz pozisyonda durmalıdır. 

Kalibrasyonu Kodu

// Arduino sketch that returns calibration offsets for MPU6050 //   Version 1.1  (31th January 2014)

// Done by Luis Ródenas <luisrodenaslorda@gmail.com>

// Based on the I2Cdev library and previous work by Jeff Rowberg <jeff@rowberg.net>

// Updates (of the library) should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib

// These offsets were meant to calibrate MPU6050's internal DMP, but can be also useful for reading sensors. 

// The effect of temperature has not been taken into account so I can't promise that it will work if you 

// calibrate indoors and then use it outdoors. Best is to calibrate and use at the same room temperature.

/* ==========  LICENSE  ==================================

 I2Cdev device library code is placed under the MIT license

 Copyright (c) 2011 Jeff Rowberg

 Permission is hereby granted, free of charge, to any person obtaining a copy

 of this software and associated documentation files (the "Software"), to deal

 in the Software without restriction, including without limitation the rights

 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

 copies of the Software, and to permit persons to whom the Software is

 furnished to do so, subject to the following conditions:

 The above copyright notice and this permission notice shall be included in

 all copies or substantial portions of the Software.

 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN

 THE SOFTWARE.

 =========================================================

 */

// I2Cdev and MPU6050 must be installed as libraries

#include "I2Cdev.h"

#include "MPU6050.h"

#include "Wire.h"

///////////////////////////////////   CONFIGURATION   /////////////////////////////

//Change this 3 variables if you want to fine tune the skecth to your needs.

int buffersize=1000;     //Amount of readings used to average, make it higher to get more precision but sketch will be slower  (default:1000)

int acel_deadzone=8;     //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge  (default:8)

int giro_deadzone=1;     //Giro error allowed, make it lower to get more precision, but sketch may not converge  (default:1)

// default I2C address is 0x68

// specific I2C addresses may be passed as a parameter here

// AD0 low = 0x68 (default for InvenSense evaluation board)

// AD0 high = 0x69

//MPU6050 accelgyro;

MPU6050 accelgyro(0x68); // <-- use for AD0 high

int16_t ax, ay, az,gx, gy, gz;

int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;

int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;

///////////////////////////////////   SETUP   ////////////////////////////////////

void setup() {

  // join I2C bus (I2Cdev library doesn't do this automatically)

  Wire.begin();

  // COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE

  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.

  // initialize serial communication

  Serial.begin(115200);

  // initialize device

  accelgyro.initialize();

  // wait for ready

  while (Serial.available() && Serial.read()); // empty buffer

  while (!Serial.available()){

    Serial.println(F("Send any character to start sketch.\n"));

    delay(1500);

  }                

  while (Serial.available() && Serial.read()); // empty buffer again

  // start message

  Serial.println("\nMPU6050 Calibration Sketch");

  delay(2000);

  Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");

  delay(3000);

  // verify connection

  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  delay(1000);

  // reset offsets

  accelgyro.setXAccelOffset(0);

  accelgyro.setYAccelOffset(0);

  accelgyro.setZAccelOffset(0);

  accelgyro.setXGyroOffset(0);

  accelgyro.setYGyroOffset(0);

  accelgyro.setZGyroOffset(0);

}

///////////////////////////////////   LOOP   ////////////////////////////////////

void loop() {

  if (state==0){

    Serial.println("\nReading sensors for first time...");

    meansensors();

    state++;

    delay(1000);

  }

  if (state==1) {

    Serial.println("\nCalculating offsets...");

    calibration();

    state++;

    delay(1000);

  }

  if (state==2) {

    meansensors();

    Serial.println("\nFINISHED!");

    Serial.print("\nSensor readings with offsets:\t");

    Serial.print(mean_ax); 

    Serial.print("\t");

    Serial.print(mean_ay); 

    Serial.print("\t");

    Serial.print(mean_az); 

    Serial.print("\t");

    Serial.print(mean_gx); 

    Serial.print("\t");

    Serial.print(mean_gy); 

    Serial.print("\t");

    Serial.println(mean_gz);

    Serial.print("Your offsets:\t");

    Serial.print(ax_offset); 

    Serial.print("\t");

    Serial.print(ay_offset); 

    Serial.print("\t");

    Serial.print(az_offset); 

    Serial.print("\t");

    Serial.print(gx_offset); 

    Serial.print("\t");

    Serial.print(gy_offset); 

    Serial.print("\t");

    Serial.println(gz_offset); 

    Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");

    Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");

    Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");

    while (1);

  }

}

///////////////////////////////////   FUNCTIONS   ////////////////////////////////////

void meansensors(){

  long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;

  while (i<(buffersize+101)){

    // read raw accel/gyro measurements from device

    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    

    if (i>100 && i<=(buffersize+100)){ //First 100 measures are discarded

      buff_ax=buff_ax+ax;

      buff_ay=buff_ay+ay;

      buff_az=buff_az+az;

      buff_gx=buff_gx+gx;

      buff_gy=buff_gy+gy;

      buff_gz=buff_gz+gz;

    }

    if (i==(buffersize+100)){

      mean_ax=buff_ax/buffersize;

      mean_ay=buff_ay/buffersize;

      mean_az=buff_az/buffersize;

      mean_gx=buff_gx/buffersize;

      mean_gy=buff_gy/buffersize;

      mean_gz=buff_gz/buffersize;

    }

    i++;

    delay(2); //Needed so we don't get repeated measures

  }

}

void calibration(){

  ax_offset=-mean_ax/8;

  ay_offset=-mean_ay/8;

  az_offset=(16384-mean_az)/8;

  gx_offset=-mean_gx/4;

  gy_offset=-mean_gy/4;

  gz_offset=-mean_gz/4;

  while (1){

    int ready=0;

    accelgyro.setXAccelOffset(ax_offset);

    accelgyro.setYAccelOffset(ay_offset);

    accelgyro.setZAccelOffset(az_offset);

    accelgyro.setXGyroOffset(gx_offset);

    accelgyro.setYGyroOffset(gy_offset);

    accelgyro.setZGyroOffset(gz_offset);

    meansensors();

    Serial.println("...");

    if (abs(mean_ax)<=acel_deadzone) ready++;

    else ax_offset=ax_offset-mean_ax/acel_deadzone;

    if (abs(mean_ay)<=acel_deadzone) ready++;

    else ay_offset=ay_offset-mean_ay/acel_deadzone;

    if (abs(16384-mean_az)<=acel_deadzone) ready++;

    else az_offset=az_offset+(16384-mean_az)/acel_deadzone;

    if (abs(mean_gx)<=giro_deadzone) ready++;

    else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);

    if (abs(mean_gy)<=giro_deadzone) ready++;

    else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);

    if (abs(mean_gz)<=giro_deadzone) ready++;

    else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);

    if (ready==6) break;

  }

}

Arduino Kodu ve PID ayarı

Bu kodu linkteki örnekten alarak uyguladım. Öncelikle kod için gerekli olan kütüphaneleri buradan indirip kurabilirsiniz. Koddaki PID katsayıları ayarı kısmı her robot için farklı olacaktır. Birebir ölçülerde ve malzemelerden yapılan roborlarda benzer değerler olabilir fakat tasarımı farklı olan robotlarda değerler mutlaka değişik olur.

Kendi PID'nizi ayarlamak için önceüç değeri de (P, I, D) sıfır yapın. P değerini ta ki robotunuz düzenli salınımlar halinde bir ileri bir geri saykıla girene kadar arttırın. P çok yüksek olursa alet sürekli salınıma girer. Az olursa robotu ayakta tutmaya yetmez. Robotu ayakta tuttuğu ancak çok çılgınca salınım yaptırmadığı tatlı bir salınım olan yerde bırakın. Sonra bu saykılı duruduracak kadar I değerini arttırın. I çok arttırılırsa bu seferde aşırı titreşimli çalışır. En sonda da D değerini minimum salınım tekrarı ile arzu edilen denge durumunu sağlayacak kadar arttırın.

Bu tamamen bir deneme yanılma süreci. Uzun süreli bir test ile en uygun değerleri bulabilirsiniz. Ayrıca bu değerler zemine göre bile değişebilir. Örneğin parke gibi yüzeylerle halı farkeder. Halıda sürtünme daha fazla olacağı için alet daha kolay dengeye gelecektir.

Aşağıdaki kod için linkteki kütüphaneyi indirip arduinonuzun ilgili klasörüne atmalısınız.

Programın Kodu 

//kütüphaneleri çağırıyoruz.

#include <PID_v1.h>

#include <LMotorController.h>

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

#include "Wire.h"

#endif

#define MIN_ABS_SPEED 20 //motorları sürerken kullanılacak hız değişkeni

MPU6050 mpu; //bir MPU6050 nesnesi yaratıyoruz.

// MPU kontrolü için değişkenleri tanımlıyoruz

bool dmpReady = false; // DMP başlatma başarılı ise TRUE olacak

uint8_t mpuIntStatus; // interrupt status byte bilgilerini tutacak

uint8_t devStatus; // cihazın çalışma durumu bilgisi (0 = başarılı, !0 = HATA)

uint16_t packetSize; // DMP paket boyutu (standardı 42 byte)

uint16_t fifoCount; // FIFO stackteki tüm byteları sayıyor

uint8_t fifoBuffer[64]; // FIFO hafıza kapasitesi

// pozisyon/hareket değişkenleri

Quaternion q; // [w, x, y, z] Dördeyleri saklayacak

VectorFloat gravity; // [x, y, z] yerçekimi vektörü

float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll eksen bilgisi

//PID değişkenleri

// setpoint kaç derecede düz durduğu. Sizin ki 1-2 derece farklı olabilir.

double originalSetpoint = 175;

double setpoint = originalSetpoint;

double movingAngleOffset = 0.1;

double input, output;

// bu sayılar her farklı robot için değişir siz de kendi katsayılarınızı deneme yanılma ile belirleyin

double Kp = 40;   

double Kd = 1.3;

double Ki = 160;

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

double motorSpeedFactorLeft = 0.60; //sağ ve sol motorun hangi hızla döneceği katsayıları aralarında

//eşitsizlik olabilir.

double motorSpeedFactorRight = 0.63;

//MOTOR ÇIKIŞLARI

int ENA = 5;

int IN1 = 6;

int IN2 = 7;

int IN3 = 8;

int IN4 = 9;

int ENB = 10;

LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

volatile bool mpuInterrupt = false; // MPU interrrupt pini HIGH olmuş mu ona bakıyor

void dmpDataReady()

{

mpuInterrupt = true;

}

void setup()

{

  Serial.begin(9600);

// I2C Bus'a bağalanıyoruz (I2Cdev kütüphanesi bunu otomatik yapmaz)

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

Wire.begin();

TWBR = 24; // 400kHz I2C saat hızı 

#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE

Fastwire::setup(400, true);

#endif

Serial.println("IMU başlatılıyor");

mpu.initialize();

devStatus = mpu.dmpInitialize();

// hassasiyet için buraya yukarıdaki anlatılan yöntemle aldığınız kendi gyro offset değerlerinizi girebilirsiniz.

mpu.setXGyroOffset(220);

mpu.setYGyroOffset(76);

mpu.setZGyroOffset(-85);

mpu.setZAccelOffset(1788); 

// cihaz çalıştı mı kontrol ediyoruz(başarılı ise 0 döndürecek)

if (devStatus == 0)

{

// DMP açılıyor

mpu.setDMPEnabled(true);

// Arduino interrupt tespiti etkinleştiriliyor

attachInterrupt(0, dmpDataReady, RISING);

mpuIntStatus = mpu.getIntStatus();

// main loop kullansın diye DMP Hazır değişkenine true atıyoruz

dmpReady = true;

packetSize = mpu.dmpGetFIFOPacketSize();

//PID ayarlanıyor

pid.SetMode(AUTOMATIC);

pid.SetSampleTime(10); //örnekleme zamanı

pid.SetOutputLimits(-255, 255); //çünkü motorların limiti bu

}

else

{

// HATA!

// 1 = Ön Hafıza Yükleme Başarısız!

// 2 = DMP konfigürasyon güncellemesi hatalı!

// (Genelde 1 numaralı hata oluşabilir)

Serial.print(F("DMP Başlatma Hatalı (Hata Kodu "));

Serial.print(devStatus);

Serial.println(F(")"));

}

}

void loop()

{

// Eğer hata varsa boşuna uğraşma çık

if (!dmpReady) return;

// MPU interrupt için veya ilave paketler için bekle

while (!mpuInterrupt && fifoCount < packetSize)

{

//PID hesapları yapılıyor ve motorlar sürülüyor 

pid.Compute();

motorController.move(output, MIN_ABS_SPEED);

}

// Interrupt bayrağı resetleniyor ve INT_STATUS_BYTE verisi alınıyor

mpuInterrupt = false;

mpuIntStatus = mpu.getIntStatus();

// O anki FIFO sayısını alıyor

fifoCount = mpu.getFIFOCount();

// FIFO stack'de overflow hatası var mı ona bakıyor.

if ((mpuIntStatus & 0x10) || fifoCount == 1024)

{

// FIFO resetleniyor ki overflow temizlensin

mpu.resetFIFO();

Serial.println(F("FIFO overflow!"));

// aksi taktirde DMP interrupt var mı ona bakıyor (sıklıkla olan bir durum)

}

else if (mpuIntStatus & 0x02)

{

// doğru veri uzunluğu için kısa bir bekleme

while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

// FIFO stack'ten veri okunuyor.

mpu.getFIFOBytes(fifoBuffer, packetSize);

// 1 ve daha fazla paket varsa burada FIFO içeriği sayılıyor böylece interrupt olmadan hemen okuma sağlanıyor

fifoCount -= packetSize;

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

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

}

}

Son Hali ve Test Videoları

Test Videosu-1

Test Videosu-2

Sonuç olarak tam hassas ve yumuşak bir şekilde dengeyi bulan bir robot olmadı ki bu donanımla olması da çok beklenemez. Ancak iyileştirmeler yapılabilir. Yine de başlangıçtaki amacımıza ulaşarak kendi dengede duran bir robot yapmayı başarmış olduk. Umarım bu örnek sizlere de yardımcı olur ve daha hassas ve iyi şekilde dengede duran versiyonlar yapabilirsiniz.
Teşekkürler,

Onur.