Tek Rotorlu PID Kontrollü Denge Sistemi

KULLANILAN MALZEMELER

  • Çeşitli ölçülerde MDF tahta parçaları.
  • Tij.
  • 2 Adet rulman.
  • 4 Adet cıvata.
  • 2 Adet somun.
  • Arduino Nano.
  • Orta boy breadboard.
  • L298N motor sürücü.
  • MPU6050 Jiroskop ve ivme sensörü.
  • 3V 4200RPM DC motor.
  • Çeşitli jumper ve zil teli kablo.

DEVRE ELEMANLARI

MPU6050 JİROSKOP

  • MPU6050 ivme sensörleri3 eksen açısal ivme ve 3 eksen gyro olmak üzere 6 eksende ölçüm yapabilme özelliğiyle drone gibi hava araçlarında ve denge robotlarında sıkça kullanılan sensörlerdir.
  • Sadece ev ve okul projelerinde değil, endüstride de kullanılan bir modüldür. I2C protokolü ile haberleşmektedir. Her eksen için 16 bit register ayrılmıştır. (+) ve (-) yönde değer okunabilmektedir.

L298N

MINI N40-2330 BRUSHED DC MOTOR

RULMAN

YAPIM AŞAMALARI

FİNAL

YAZILIM VE PID KONTROL

#include
////////////////DEĞİŞKEN TANIMLARI///////////////
int in1 = 3;
int in2 = 4;
int enA = 5;

int mspeed = 10;
int mmspeed;
int turnspeed = 50;
int16_t Acc_rawX, Acc_rawY, Acc_rawZ, Gyr_rawX, Gyr_rawY, Gyr_rawZ;
float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2];
float elapsedTime, time, timePrev;
int i;
float rad_to_deg = 180 / 3.141592654;
float PID, pwmLeft, pwmRight, error, previous_error;
float pid_p = 0;
float pid_i = 0;
float pid_d = 0;
////////////////////////PID VERİLERİ/////////////////////
float kp = 10; // positional çarpan
float ki = 0.02; // integral çarpanı
float kd = 1 ; // türev çarpanı
float desired_angle = 0; // hedeflenen açı değeri
void setup()
{
Wire.begin(); /////////////I2C HABERLEŞME///////////////
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
////////////////GİRİŞ ÇIKIŞ TANIMLAMALARI//////////////////////
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(enA, OUTPUT);

Serial.begin(9600);
time = millis(); ///////////////MİLİSANİYE CİNSİNDEN ZAMAN SAYMAYA BAŞLIYOR/////////////
}
void loop()
{
/*////////////////////////UYARI//////////////////////
DÖNGÜ İÇERİSİNDE HİÇBİR DELAY KULLANMAYIN.
AKSİ HALDE DÖNGÜ YAVAŞLAYACAK VE PID DÜZGÜN ÇALIŞMAYACAKTIR.
AYNI ZAMANDA HERHANGİ BİR SERİ YAZICI DA KULLANMAYIN.
*/
timePrev = time;
time = millis();
elapsedTime = (time – timePrev) / 1000;
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(0x68, 6, true);
////////////////////ACC DATAYI IMUDAN RAW OLARAK ALMA/////////////////
Acc_rawX = Wire.read() << 8 | Wire.read();
Acc_rawY = Wire.read() << 8 | Wire.read();
Acc_rawZ = Wire.read() << 8 | Wire.read();
/////////////////////RAW DATAYI AÇIYA ÇEVİRME/////////////////////
Acceleration_angle[0] = atan((Acc_rawY / 16384.0) / sqrt(pow((Acc_rawX / 16384.0), 2) + pow((Acc_rawZ / 16384.0), 2))) * rad_to_deg;
Acceleration_angle[1] = atan(-1 * (Acc_rawX / 16384.0) / sqrt(pow((Acc_rawY / 16384.0), 2) + pow((Acc_rawZ / 16384.0), 2))) * rad_to_deg;
Wire.beginTransmission(0x68);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(0x68, 4, true);
//////////////////GYRO DATAYI IMUDAN RAW OLARAK ALMA/////////////////////////
Gyr_rawX = Wire.read() << 8 | Wire.read();
Gyr_rawY = Wire.read() << 8 | Wire.read();
////////////////////RAW DATAYI AÇIYA ÇEVİRME///////////////////////
Gyro_angle[0] = Gyr_rawX / 131.0;
Gyro_angle[1] = Gyr_rawY / 131.0;
//////////////////////////////ALINAN İKİ AÇIYI KULLAN////////////////////////
Total_angle[0] = 0.98 * (Total_angle[0] + Gyro_angle[0] * elapsedTime) + 0.02 * Acceleration_angle[0];
Total_angle[1] = 0.98 * (Total_angle[1] + Gyro_angle[1] * elapsedTime) + 0.02 * Acceleration_angle[1];
////TOTAL_ANGLE[0] BİZİM İHTİYACIMIZ OLAN PITCH DEĞERİ////////////
error = Total_angle[0] – desired_angle; /////////////////HATA HESAPLAMASI////////////////////
///////////////////////POZİSYONAL HATA//////////////
pid_p = kp * error;
///////////////////////İNTEGRAL HATA/////////////////
if (-4 < error < 4) // integral hatasını son kademede yükle.
{
pid_i = pid_i + (ki * error);
}
///////////////////////TÜREVSEL HATA//////////////
pid_d = kd * ((error – previous_error) / elapsedTime);
///////////////////////TOPLAM PID DEĞERİ/////////////////
PID = pid_p + pid_d + pid_i; //bütün hataları topla
///////////////////////HATA DEĞERİNİ GÜNCELLEME////////
previous_error = error;
//Serial.println(PID); //////////HATA AYIKLAMA//////////////
//delay(60); //////////HATA AYIKLAMA//////////////
//Serial.println(Total_angle[0]);
//delay(100);
//////////HATA AYIKLAMA//////////////
/////////////////PID VERİSİNİ KESİN SONUÇLARA AKTARMA//////////////////////////////////
if (PID < -220) // PID değerini -255, +255 arasında sınırla. { PID = -255; } if (PID > 220)
{
PID = 255;
}
mspeed = abs(PID);

//mmspeed = map(mspeed, -1024, 1024, -255, 255); ////////////DENEME YAZILIMI////////
//Serial.println(mspeed); //////////HATA AYIKLAMA//////////////
///////////////PID’NİN ÖZÜ, PROJEYE ÖZEL KULLANIM///////////////

if (Total_angle[0] < 0) // alınan açı değerine göre motoru saat yönü veya tersinde çalıştır. { halt(); } if (Total_angle[0] > 0)
{
clockw();
}
if (Total_angle[0] > 45)
halt();
if (Total_angle[0] < -45)
halt();

}
//////////////HAREKET FONKSİYONLARI///////////////////
void clockw()
{
analogWrite(enA, mspeed);
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);

}
void anti()
{
delay(40); // ani ve hızlı motor yönü değişimi için koruma denemesi, kaldırılacak.
analogWrite(enA, mspeed);
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);

}
void halt()
{

analogWrite(enA, 0);
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);

}

YAZILIMIN MANTIĞINI BASİT BİR ŞEKİLDE AÇIKLAYACAK OLURSAK;

Giriş çıkış pinlerinin yerleri arduinoya tanıtılır.

Positional, kp, integral, ki ve türev, d, katsayıları arduinoya söylenir.

PID nin temelini oluşturan ve bizim isteğimizi yerine getirmek üzere temel alarak işlem yapacak sayı, yani açı değeri arduinoya söylenir.

İstenilen değer ile mevcut değer karşılaştırılır. Eğer bu değerler birbirine eşit değil ise PID bu noktada devreye girer. PID kontrolün temelinde matematiksel işlem olan türev ve integral yatar. İlk adımda birbirine eşit olmayan iki veriyi karşılaştırdıktan sonra aradaki farkı belirleyip positional hata katsayısı ile çarpar ve daha sonra kullanılmak üzere bu değeri pid_p değişkenine yükler.

Şu an ölçülen açı değerinden belirlenen açı değerini çıkartır, farkını alır ve bu farka hata, «error» der.

Daha sonra tanımlanan bu hatayı positional hata katsayısı ile çarpıp hesaplar.

Tekrar bu hatanın integralini alır ve pid’nin integral değişkenine yükler.

Hatanın türevini alır ve aynı şekilde pid’nin türev değişkenine bu değeri yükler.

Toplamda 3 değişken, yani pozisyonal, türev ve integral değerleri PID’ye yüklenir.

Bu PID değeri ise başka bir değişkene tanımlanarak motora PWM sinyali göndermeye yardımcı olur ve motorun hızını bu PID değişkeni belirler.

Son olarak istenilen açı değerinden büyük, küçük veya eşit olmak üzere motor yönü belirlenir ve motor çalıştırılır.

Kısaca PID nin bizim projemizde çalışma mantığı budur.

ÇALIŞMA MANTIĞI

Devreyi kontrol eden arduino, gerekli verileri bize sağlayan jiroskop ve bu değerlere göre yazdığımız kod ile birlikte motorun hızını ve çalışma yönünü ayarlayan motor sürücüsü devremizin başlıca elamanlarıdır.

Dışarıdan bir müdahale geldiğinde, jiroskoptan alınan ve ayarlanan açı değerine göre, motoru saat yönünde veya tersinde çalıştırıp denge tahtasının konumunu gerekli seviyede tutmayı hedefler.

Tahtanın konumunu ve istenilen açı değerini karşılaştırarak motorun çalışma yönüyle birlikte motorun hız kontrollü çalışmasını sağlar.

DEVRE ŞEMASI

VİDEO

Bir cevap yazın

E-posta hesabınız yayımlanmayacak. Gerekli alanlar * ile işaretlenmişlerdir