Kütüphanesiz Roll, Pitch ve Yaw Hesabı

Programlama Kodları

//AÇIKLAMA
//Bu uygulamada jiroskop ve ivmeölçerden alınan verilerle Roll, Pitch ve Yaw hesabı yapılır.
//İlk başta "IMU_olcum_hatalari()" fonksiyonu ile jiroskop ve ivmeölçerden alınan 200'er adet
//verinin ortalaması alınarak Ofset(hata payı) değerleri bulunur ve okunan değerlerlerden
//çıkarılarak gerçek değerler bulunur.

#include <Wire.h>

const int modul_adres = 0x68;
float GyroX, GyroY, GyroZ;  //jiroskop değerleri için değişkenler
float AccX, AccY, AccZ;     //ivmeölçer değerleri için değişkenler
float gyroAngleX, gyroAngleY, gyroAngleZ; //jiroskop açı değişkenleri
float accAngleX, accAngleY; //ivmeölçer açı değişkenleri
float GyroErrorX, GyroErrorY, GyroErrorZ; //jiroskop Ofset(hata payı) değişkenleri
float AccErrorX, AccErrorY;  //ivmeölçer Ofset(hata payı) değişkenleri
float roll, pitch, yaw;
float simdiki_zaman, gecmis_zaman, gecen_zaman;
int i = 0;

void setup()
{
  Serial.begin(9600);
  Wire.begin(); //I2C seri iletişimini başlat

  //Modül resetleniyor
  Wire.beginTransmission(modul_adres);  //0x68 adresli modül ile iletişime geç
  Wire.write(0x6B); //0x6B adresli register'a git.
  Wire.write(0x00); //Register içeriğini sıfırla
  Wire.endTransmission();

  //Jiroskop ölçek ayarının varsayılan değeri olan ±250deg/s'yi kullanacağız
  //İvmeölçer ölçek ayarının varsayılan değeri olan ±2g'yi kullanacağız.

  //Dijital Alçak Geçiş Filtresinin (DLPF) Band Genişiliği belirleniyor
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x1A);   //0x1A adresli register’a git.
  Wire.write(0x02);   //Register içeriğini 00000010 yap. İvmeölçer band genişliği 94 Hz ve jiroskop bant genişliği 98 Hz olsun.
  Wire.endTransmission();

  //Ölçüm hataları hesaplanıyor
  IMU_olcum_hatalari();

  delay(3000);
}

void loop()
{
  //_______________İvmeölçer Hesaplamaları______________
  //İvmeölçer değerleri okunuyor
  Wire.beginTransmission(modul_adres);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(modul_adres, 6, true);
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0;
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0;

  //İvmeölçerden okunan değerlerle açılar hesaplanıyor ve Ofsetler(hata payları) çıkarılıyor
  accAngleX = (atan2(AccY, sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - (-1.02);
  accAngleY = (atan2(-1 * AccX, sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) - (-6.98);

  //__________________Jiroskop Hesaplamaları________________
  //zaman hesapları yapılıyor. jiroskop açı hesabı için kullanılacak
  gecmis_zaman = simdiki_zaman;
  simdiki_zaman = millis(); //Arduino çalışmaya başladığından itibaren ms olarak geçen süre
  gecen_zaman = (simdiki_zaman - gecmis_zaman) / 1000; //geçen süre hesaplanıp sn'ye çevrildi

  //Jiroskop değerleri okunuyor
  Wire.beginTransmission(modul_adres);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(modul_adres, 6, true);
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

  //Jiroskoptan okunan değerlerden Ofsetler(hata payları) çıkarılıyor
  GyroX = GyroX - (-0.18);
  GyroY = GyroY - (-0.60);
  GyroZ = GyroZ - (-0.32);

  //Jiroskop açıları hesaplanıyor
  gyroAngleX = gyroAngleX + GyroX * gecen_zaman;
  gyroAngleY = gyroAngleY + GyroY * gecen_zaman;
  gyroAngleZ = gyroAngleZ + GyroZ * gecen_zaman;

  //_________________Tamamlayıcı Filtre (Complementary Filter___________
  //Tamamlayıcı filtre ile Roll, Pitch, Yaw hesaplanıyor
  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
  yaw = gyroAngleZ;

  //________________________Seri Port Ekranına Yazdırma___________________
  Serial.print("Roll = ");  Serial.print(roll);   Serial.print(" | ");
  Serial.print("Pitch = "); Serial.print(pitch);  Serial.print(" | ");
  Serial.print("Yaw = ");   Serial.println(yaw);

  delay(200);
}

void IMU_olcum_hatalari()
{
  //__________________İvmeölçer Ölçüm Hatası_______________
  //İvmeölçerden ard arda 200 okuma yapılıp ortalaması alınıyor
  while (i < 200)
  {
    Wire.beginTransmission(modul_adres);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(modul_adres, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;

    AccErrorX  = AccErrorX + (atan2(AccY, sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI);
    AccErrorY  = AccErrorY + (atan2(-1 * AccX, sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI);

    i++;
  }
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  i = 0;

  //__________________Jiroskop Ölçüm Hatası_______________
  //Jiroskoptan ard arda 200 okuma yapılıp ortalaması alınıyor
  while (i < 200)
  {
    Wire.beginTransmission(modul_adres);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(modul_adres, 6, true);
    GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
    GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
    GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

    GyroErrorX = GyroErrorX + GyroX;
    GyroErrorY = GyroErrorY + GyroY;
    GyroErrorZ = GyroErrorZ + GyroZ;

    i++;
  }

  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
  GyroErrorZ = GyroErrorZ / 200;

  //________________________Seri Port Ekranına Yazdırma___________________
  Serial.print("AccErrorX = ");   Serial.println(AccErrorX);
  Serial.print("AccErrorY = ");   Serial.println(AccErrorY);
  Serial.print("GyroErrorX = ");  Serial.println(GyroErrorX);
  Serial.print("GyroErrorY = ");  Serial.println(GyroErrorY);
  Serial.print("GyroErrorZ = ");  Serial.println(GyroErrorZ);
}

Devre Şeması

Devamını Oku

Kütüphanesiz Tüm Ham Değerlerin Okunması ve Hesaplanması

Programlama Kodları

#include <Wire.h>

const int modul_adres = 0x68;
float GyroX, GyroY, GyroZ;
float AccX, AccY, AccZ;
float Temp;

void setup()
{
  Serial.begin(9600);
  Wire.begin(); //I2C seri iletişimini başlat

  //Modül resetleniyor
  Wire.beginTransmission(modul_adres);  //0x68 adresli modül ile iletişime geç
  Wire.write(0x6B); //0x6B adresli register'a git.
  Wire.write(0x00); //Register içeriğini sıfırla
  Wire.endTransmission();

  //Jiroskop ölçek ayarı yapılıyor
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x1B);    //0x1B adresli register’a git.
  Wire.write(0x10);    //Register içeriğini 00010000 yap. Ölçek ayarı ±1000deg/s olsun.
  Wire.endTransmission();

  //İvmeölçer ölçek ayarı yapılıyor
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x1C);    //0x1C adresli register’a git.
  Wire.write(0x10);    //Register içeriğini 00010000 yap. Ölçek ayarı ±8g olsun.
  Wire.endTransmission();

  //Dijital Alçak Geçiş Filtresinin (DLPF) Band Genişiliği belirleniyor
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x1A);   //0x1A adresli register’a git.
  Wire.write(0x02);   //Register içeriğini 00000010 yap. İvmeölçer band genişliği 94 Hz ve jiroskop bant genişliği 98 Hz olsun.
  Wire.endTransmission();

  delay(20);
}

void loop()
{
  //Modülün 0x3B adresinden itibaren 0x48 e kadar sıralı olarak:  6 bayt ivmeölçer, 2 bayt sıcaklık, 6 bayt jiroskop ham değerleri vardır.
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x3B); //0x3B adresli register’a git.
  Wire.endTransmission(false);  //Modül ile olan iletişimi durdurma, iletişime devam et ve Wire.write() ile 0x3B adresli register’a gidilmesini onayla.
  Wire.requestFrom(modul_adres, 14, true);  //0x68 slave adresli modülün 0x3B adresinden itibaren sıralı olarak 14 baytlık veri iste. (true)İstekten sonra veri yolunu serbest bırak.

  //İvmeölçer ham değerleri modülden okunup hassasiyet ayarına bölünerek g birimine dönüştürülüyor
  AccX = (Wire.read() << 8 | Wire.read()) / 4096.0;
  AccY = (Wire.read() << 8 | Wire.read()) / 4096.0;
  AccZ = (Wire.read() << 8 | Wire.read()) / 4096.0;

  //Çipin ham sıcaklık değerleri modülden okunarak derece birimine dönüştürülüyor
  Temp = (Wire.read() << 8 | Wire.read()) / 340 + 36.53;


  //Jiroskop ham değerleri modülden okunup hassasiyet ayarına bölünerek derece/sn birimine dönüştürülüyor
  GyroX = (Wire.read() << 8 | Wire.read()) / 32.8;
  GyroY = (Wire.read() << 8 | Wire.read()) / 32.8;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 32.8;

  //İvmeölçer değerleri ekrana yazdırılıyor
  Serial.print("AccX = ");   Serial.print(AccX);  Serial.print(" derece/sn");  Serial.print(" | ");
  Serial.print("AccY = ");   Serial.print(AccY);  Serial.print(" derece/sn");  Serial.print(" | ");
  Serial.print("AccZ = ");   Serial.print(AccZ);  Serial.print(" derece/sn");  Serial.print(" ___ ");

  //Çipin sıcaklık değeri ekrana yazdırılıyor
  Serial.print("Temp = ");   Serial.print(Temp);  Serial.print(" derece");  Serial.print(" ___ ");

  //Jiroskop değerleri ekrana yazdırılıyor
  Serial.print("GyroX = ");   Serial.print(GyroX);  Serial.print(" g"); Serial.print(" | ");
  Serial.print("GyroY = ");   Serial.print(GyroY);  Serial.print(" g"); Serial.print(" | ");
  Serial.print("GyroZ = ");   Serial.print(GyroZ);  Serial.println(" g");

  delay(100);
}

Devre Şeması

Seri Port Ekran Görüntüsü

Devamını Oku

Kütüphanesiz Tüm Ham Değerleri Okuma

Programlama Kodları

#include <Wire.h>

const int modul_adres = 0x68;
float GyroX, GyroY, GyroZ;
float AccX, AccY, AccZ;
float Temp;

void setup()
{
  Serial.begin(9600);
  Wire.begin(); //I2C seri iletişimini başlat

  //Modül resetleniyor
  Wire.beginTransmission(modul_adres);  //0x68 adresli modül ile iletişime geç
  Wire.write(0x6B); //0x6B adresli register'a git.
  Wire.write(0x00); //Register içeriğini sıfırla
  Wire.endTransmission();

  //Jiroskop ölçek ayarı yapılıyor
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x1B);    //0x1B adresli register’a git.
  Wire.write(0x10);    //Register içeriğini 00010000 yap. Ölçek ayarı ±1000deg/s olsun.
  Wire.endTransmission();

  //İvmeölçer ölçek ayarı yapılıyor
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x1C);    //0x1C adresli register’a git.
  Wire.write(0x10);    //Register içeriğini 00010000 yap. Ölçek ayarı ±8g olsun.
  Wire.endTransmission();

  //Dijital Alçak Geçiş Filtresinin (DLPF) Band Genişiliği belirleniyor
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x1A);   //0x1A adresli register’a git.
  Wire.write(0x02);   //Register içeriğini 00000010 yap. İvmeölçer band genişliği 94 Hz ve jiroskop bant genişliği 98 Hz olsun.
  Wire.endTransmission();

  delay(20);
}

void loop()
{
  //Modülün 0x3B adresinden itibaren 0x48 e kadar sıralı olarak:  6 bayt ivmeölçer, 2 bayt sıcaklık, 6 bayt jiroskop ham değerleri vardır.
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x3B); //0x3B adresli register’a git.
  Wire.endTransmission(false);  //Modül ile olan iletişimi durdurma, iletişime devam et ve Wire.write() ile 0x3B adresli register’a gidilmesini onayla.
  Wire.requestFrom(modul_adres, 14, true);  //0x68 slave adresli modülün 0x3B adresinden itibaren sıralı olarak 14 baytlık veri iste. (true)İstekten sonra veri yolunu serbest bırak.

  //İvmeölçer ham değerleri modülden okunuyor
  AccX = Wire.read() << 8 | Wire.read();  //1 bayt veri oku. 8 bit sola kaydır. Sonra 1 bayt veri daha oku. İlk okunan 1 bayt veri ile “veya” işlemine tabi tut ve sonucu AccX değişkenine ata.
  AccY = Wire.read() << 8 | Wire.read();  //1 bayt veri oku. 8 bit sola kaydır. Sonra 1 bayt veri daha oku. İlk okunan 1 bayt veri ile “veya” işlemine tabi tut ve sonucu AccY değişkenine ata.
  AccZ = Wire.read() << 8 | Wire.read();  //1 bayt veri oku. 8 bit sola kaydır. Sonra 1 bayt veri daha oku. İlk okunan 1 bayt veri ile “veya” işlemine tabi tut ve sonucu AccZ değişkenine ata.

  //Çipin ham sıcaklık değerleri modülden okunuyor
  Temp = Wire.read() << 8 | Wire.read();  //1 bayt veri oku. 8 bit sola kaydır. Sonra 1 bayt veri daha oku. İlk okunan 1 bayt veri ile “veya” işlemine tabi tut ve sonucu Temp değişkenine ata.

  //Jiroskop ham değerleri modülden okunuyor
  GyroX = Wire.read() << 8 | Wire.read(); //1 bayt veri oku. 8 bit sola kaydır. Sonra 1 bayt veri daha oku. İlk okunan 1 bayt veri ile “veya” işlemine tabi tut ve sonucu GyroX değişkenine ata.
  GyroY = Wire.read() << 8 | Wire.read(); //1 bayt veri oku. 8 bit sola kaydır. Sonra 1 bayt veri daha oku. İlk okunan 1 bayt veri ile “veya” işlemine tabi tut ve sonucu GyroY değişkenine ata.
  GyroZ = Wire.read() << 8 | Wire.read(); //1 bayt veri oku. 8 bit sola kaydır. Sonra 1 bayt veri daha oku. İlk okunan 1 bayt veri ile “veya” işlemine tabi tut ve sonucu GyroZ değişkenine ata

  //İvmeölçerden okunan ham değerler ekrana yazdırılıyor
  Serial.print("AccX = ");   Serial.print(AccX);  Serial.print(" | ");
  Serial.print("AccY = ");   Serial.print(AccY);  Serial.print(" | ");
  Serial.print("AccZ = ");   Serial.print(AccZ);  Serial.print(" ___ ");

  //Çipin ham sıcaklık değerleri ekrana yazdırılıyor
  Serial.print("Temp = ");   Serial.print(Temp);  Serial.print(" ___ ");

  //Jiroskoptan okunan ham değerler ekrana yazdırılıyor
  Serial.print("GyroX = ");   Serial.print(GyroX);  Serial.print(" | ");
  Serial.print("GyroY = ");   Serial.print(GyroY);  Serial.print(" | ");
  Serial.print("GyroZ = ");   Serial.println(GyroZ);  

  delay(100);
}

Devre Şeması

Seri Port Ekran Görüntüsü

Devamını Oku

Kütüphanesiz Ham Sıcaklık Değerlerini Okuma

Programlama Kodları

#include <Wire.h>

const int modul_adres = 0x68;
float Temp;

void setup() 
{
  Serial.begin(9600);
  Wire.begin(); //I2C seri iletişimini başlat

  //Modül resetleniyor
  Wire.beginTransmission(modul_adres);  //0x68 adresli modül ile iletişime geç
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();

  //Dijital Alçak Geçiş Filtresinin (DLPF) Band Genişiliği belirleniyor
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x1A);   //0x1A adresli register’a git.
  Wire.write(0x02);   //Register içeriğini 00000010 yap. İvmeölçer band genişliği 94 Hz ve jiroskop bant genişliği 98 Hz olsun.
  Wire.endTransmission();

  delay(20);
}

void loop() 
{
  //Çipin ham sıcaklık değerleri modülden okunuyor
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x41); //0x41 adresli register’a git.
  Wire.endTransmission(false);  //Modül ile olan iletişimi durdurma, iletişime devam et ve Wire.write() ile 0x41 adresli register’a gidilmesini onayla.
  Wire.requestFrom(modul_adres, 2, true);  //0x68 slave adresli modülün 0x41 adresinden itibaren sıralı olarak 2 baytlık veri iste. (true)İstekten sonra veri yolunu serbest bırak.
  Temp = Wire.read() << 8 | Wire.read();  //1 bayt veri oku. 8 bit sola kaydır. Sonra 1 bayt veri daha oku. İlk okunan 1 bayt veri ile “veya” işlemine tabi tut ve sonucu Temp değişkenine ata.

  //Çipin ham sıcaklık değerleri ekrana yazdırılıyor
  Serial.print("Temp = ");   Serial.println(Temp);

  delay(100);
  
}

Devre Şeması

Seri Port Ekran Görüntüsü

Devamını Oku

Kütüphanesiz Ham İvmeölçer Değerlerini Okuma

 

Programlama Kodları

#include <Wire.h>
const int modul_adres = 0x68;
float AccX, AccY, AccZ;
void setup()
{
  Serial.begin(9600);
  Wire.begin(); //I2C seri iletişimini başlat

  //İvmeölçer ölçek ayarı yapılıyor
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x1C);    //0x1C adresli register’a git.
  Wire.write(0x10);    //Register içeriğini 00010000 yap. Ölçek ayarı ±8g olsun.
  Wire.endTransmission();

  //Dijital Alçak Geçiş Filtresinin (DLPF) Band Genişiliği belirleniyor
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x1A);   //0x1A adresli register’a git.
  Wire.write(0x02);   //Register içeriğini 00000010 yap. İvmeölçer band genişliği 94 Hz ve jiroskop bant genişliği 98 Hz olsun.
  Wire.endTransmission();

  delay(20);
}

void loop()
{
  //İvmeölçer ham değerleri modülden okunuyor
  Wire.beginTransmission(modul_adres); //0x68 slave adresli modül ile iletişime başla.
  Wire.write(0x3B); //0x3B adresli register’a git.
  Wire.endTransmission(false);  //Modül ile olan iletişimi durdurma, iletişime devam et ve Wire.write() ile 0x3B adresli register’a gidilmesini onayla.
  Wire.requestFrom(modul_adres, 6, true);  //0x68 slave adresli modülün 0x3B adresinden itibaren sıralı olarak 6 baytlık veri iste. (true)İstekten sonra veri yolunu serbest bırak.
  AccX = Wire.read() << 8 | Wire.read();  //1 bayt veri oku. 8 bit sola kaydır. Sonra 1 bayt veri daha oku. İlk okunan 1 bayt veri ile “veya” işlemine tabi tut ve sonucu AccX değişkenine ata.
  AccY = Wire.read() << 8 | Wire.read();  //1 bayt veri oku. 8 bit sola kaydır. Sonra 1 bayt veri daha oku. İlk okunan 1 bayt veri ile “veya” işlemine tabi tut ve sonucu AccY değişkenine ata.
  AccZ = Wire.read() << 8 | Wire.read();  //1 bayt veri oku. 8 bit sola kaydır. Sonra 1 bayt veri daha oku. İlk okunan 1 bayt veri ile “veya” işlemine tabi tut ve sonucu AccZ değişkenine ata.

  //İvmeölçerden okunan ham değerler ekrana yazdırılıyor
  Serial.print("AccX = ");   Serial.print(AccX);  Serial.print(" | ");
  Serial.print("AccY = ");   Serial.print(AccY);  Serial.print(" | ");
  Serial.print("AccZ = ");   Serial.println(AccZ);

  delay(100);
}

Devre Şeması

Seri Port Ekran Görüntüsü

Devamını Oku