Kendini Dengeleyen Robot

Kendini Dengeleyen Robot

Giriş

Bu projede iki teker üzerinde kendi dengesini sağlayan robot yapmayı hedefledik. Günümüzde hoverboard adlı cihazlardaki gibi kendi dengesini sağlayan robotların bir modelini ve çalışma mantığını anlamaya çalıştık. Dengesi için kullanılan PID algoritmasını anlattık. Gerekli malzemelerin ne olduğunu ve nereden temin edebileceğimiz aşağıdaki başlıklarda bulunmaktadır. Robotun elektrik şeması ve yazılımı aşağıda verilmiştir.

Gerekli Donanım Bileşenleri
1 adet Arduino Uno
2 adet Dc Motor ve Tekerlek
1 adet MPU6050 İvme ve Gyro Sensör Kartı
1 adet L298 Motor Sürücü Modülü
1 adet LM2596 Ayarlanabilir Voltaj Düşürücü Modülü
 1 adet Tattu 1800mAh 11.1V 75C 3S1P  Li-Po Pil

Gerekli Yazılım Bileşenleri
Arduino IDE (https://www.arduino.cc/en/Main/Software)
Fritzing (http://fritzing.org/download/)

Kullanılan Bileşenlerin Özellikleri
Arduino Uno

(https://www.direnc.net/orjinal-arduino-uno-r3)

Dc Motor
 (https://www.robolinkmarket.com/motor-ve-tekerlek-seti.html)

MPU6050

 (https://www.direnc.net/mpu6050-3-axis-gyro-ve-egim-sensoru)

L298
(https://www.direnc.net/l298-dc-step-motor-surucu)

LM2596

(https://www.direnc.net/lm2596-power-suplay-modul)

Tattu 1800mAh 11.1V 75C 3S1P
(https://www.malzemeyeri.com/Tattu-1800mAh-111v-75C-3S1P-XT60-Plug-LiPo-Batarya-Lipo-Pil,PR-1264.html)

  

Şematik Çizim

Şematik çizim fritzing programında yapıldı. Tattu Li-Po pil ve lm2596 yerine sadece 9V pil ile çizildi. Li-Po pile LM2596 bağlandıktan sonra sisteme yukarıdaki şematik çizimde 9V pilin bağlandğı gibi bağlanır.

Yapım Aşamaları
Projemiz için gerekli malzemeleri temin ettik. Şase için elimizde bulunan suntayı malzemelerimize göre kestik. İki katlı olmasını planladık. Alt katta ağırlığı daha fazla olan pili koyduk. Üst tarafa Arduino, MPU6050, L298 parçalarını koyduk. Elektronik malzemelerin bağlantı şeması yukarıda gösterilmektedir. Arduino kodu için araştırmalar sonucu PID algoritmasının kullanıldığını öğrendik. PID’nin ne olduğu aşağıdaki başlıkta anlattık.

PID

PID’nin açılımı P(Proportional-Oransal) - I(Integral-İntegral) - D(Derivative-Türevsel)’dır.

PID bir kontrol geri bildirim mekanizmasıdır. PID denetleyici ölçülü bir süreç içinde değişen ve istenilen ayar noktası ile arasındaki farkı alarak bir “hata” değerini hesaplar. Kontrolden giriş ayarları yapılarak hatanın en aza indirilmesi sağlanır.

PID için öncelikle hata tanımlaması yapılır. Referans (istenilen konum) değerden gelen konum arasındaki farka hata denir.

Hata = Ref - Gelen

P(Proportional): Hatayı bir katsayı(K_p) ile çarparak küçültmeyi hedefler.  Çıkışta salınım görme ihtimalinden dolayı K_p katsayısını küçük tutmayı hedefleriz.

P = K_p * Hata

I(Integral): İntegral ile hatanın alanını buluruz. Her bir dt çevrimde hata K_i katsayısıyla çarpılarak toplanır. Sürekli toplandığı için integral çok artar bunu engellemek için sınırlandırırız.

I = I + (K_i * Hata * dt)

dt = PID fonksiyonuna her girdiğindeki zaman

D(Derivative): İki örnek arasındaki zamanı hesaplar. Eğer hatada bir değişim olmadıysa türev sıfır olur.

EHata = Eski Hata (Bir önceki)

HD = Hata – EHata

D = (K_d *HD)/dt

PID Algoritması

Algoritma K_p, K_d ve K_i katsayılarından oluşur. Bu katsayıları deneme yanılma yöntemiyle buluruz.

Hata = Ref – Gelen

HD = Hata – EHata

P = K_p * Hata

I = I + (K_i * Hata * dt)

D = (K_d *HD)/dt

PID = P + I + D

EHata = Hata

Yazılım
Gerekli olan Arduino IDE yazılımını bilgisayarımıza indirip kuruyoruz.  Gerekli olan MPU6050, I2Cdev ve PID_v1 kütüphaneleri indirip kuruyoruz.

Arduino Kodu

#include "I2Cdev.h"
#include <PID_v1.h>
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
bool dmpReady = false;  
uint8_t mpuIntStatus;   
uint8_t devStatus;      
uint16_t packetSize;    
uint16_t fifoCount;     
uint8_t fifoBuffer[64];

Quaternion q;
VectorFloat gravity;
float ypr[3];          
double setpoint= 176;
double Kp = 20; //PID Oransal
double Kd = 0.8; //PID Türevsel
double Ki = 140; //PID İntegral
double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
volatile bool mpuInterrupt = false;
void dmpDataReady()
{
    mpuInterrupt = true;
}
void setup() {
  Serial.begin(115200);
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
    devStatus = mpu.dmpInitialize();
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1688);
    if (devStatus == 0)
    {
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(10);
        pid.SetOutputLimits(-255, 255);  
    }
    else
    {
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }
    pinMode (6, OUTPUT);
    pinMode (9, OUTPUT);
    pinMode (10, OUTPUT);
    pinMode (11, OUTPUT);
    analogWrite(6,LOW);
    analogWrite(9,LOW);
    analogWrite(10,LOW);
    analogWrite(11,LOW);
}
void loop() {
    if (!dmpReady) return;
    while (!mpuInterrupt && fifoCount < packetSize)
    {    
        pid.Compute();
        Serial.print(input);
        Serial.print(" =>");
        Serial.println(output);     
        if (input>150 && input<200)
        {
          if (output>0)
          Forward();
          else if (output<0)
          Reverse();
        }
        else
        Stop();  
    }
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    {
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));
    }
    else if (mpuIntStatus & 0x02)
    {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        fifoCount -= packetSize;
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        input = ypr[1] * 180/M_PI + 180;
   }
}
void Forward() 
{
    analogWrite(6,output);
    analogWrite(9,0);
    analogWrite(10,output);
    analogWrite(11,0);
    Serial.print("F");
}

void Reverse()
{
    analogWrite(6,0);
    analogWrite(9,output*-1);
    analogWrite(10,0);
    analogWrite(11,output*-1); 
    Serial.print("R");
}

void Stop()
{
    analogWrite(6,0);
    analogWrite(9,0);
    analogWrite(10,0);
    analogWrite(11,0); 
    Serial.print("S");
}

Proje Resmi