kod:

#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 30

MPU6050 mpu;

// MPU kontrol ve durum değişkenleri
bool dmpReady = true; // DMP başarılıysa true yap
uint8_t mpuIntStatus; // MPU interrupt durumunu tutar
uint8_t devStatus; // işlem sonrası durum (0 = başarılı, !0 = hata)
uint16_t packetSize; // DMP paketi boyutu (varsayılan değer 42 byte)
uint16_t fifoCount; // FIFO anlık byte sayısı
uint8_t fifoBuffer[64]; // FIFO saklama

// yönlendirme ve hareket değişkenleri
Quaternion q; // [w, x, y, z] kuaternion (dördey) konteyneri
VectorFloat gravity; // [x, y, z] yerçekimi vektörü
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll konteyneri ve yerçekimi vektörü

//PID
double originalSetpoint = 178;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.08;
double input, output;

//bu değerleri kendi tasarımınıza uyacak şekilde ayarlayın
double Kp = 85;
double Kd = 2.5;
double Ki = 290;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

double motorSpeedFactorLeft = 0.8;
double motorSpeedFactorRight = 0.6;

//motor bağlantıları
int ENA = 5;
int IN1 = 7;
int IN2 = 6;
int IN3 = 8;
int IN4 = 9;
int ENB = 10;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

volatile bool mpuInterrupt = false; // MPU kesme pininin yüksek olup olmadığını gösterir.
void dmpDataReady()
{
mpuInterrupt = true;
}

void setup()
{
// I2C veri yolu (I2Cdev kütüphanesi bunu otomatik olarak yapmaz)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (eğer CPU 8MHz ise 200kHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif

mpu.initialize();

devStatus = mpu.dmpInitialize();

// kendi gyro değerlerinizi buradan girin
mpu.setXGyroOffset(200);
mpu.setYGyroOffset(85);
mpu.setZGyroOffset(-65);
mpu.setZAccelOffset(1750);

// Çalıştığından emin olun (eğer öyleyse 0 değerini döndürür)
if (devStatus == 0)
{
// DMP’yi aç
mpu.setDMPEnabled(true);

// Arduino interrupt etkinleştir
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

dmpReady = true;

// Daha sonra karşılaştırma için beklenen DMP paket boyutu
packetSize = mpu.dmpGetFIFOPacketSize();

//setup PID
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(10);
pid.SetOutputLimits(-255, 255);
}
else
{
// hata!
// 1 = başlangıç bellek yükü başarısız oldu
// 2 = DMP yapılandırma güncellemeleri başarısız oldu
// (durursa, genellikle kod 1 olacak)
Serial.print(F(“DMP başlatma başarısız oldu (kod”));
Serial.print(devStatus);
Serial.println(F(“)”));
}
}

void loop()
{
// programlama başarısız olursa, hiçbir şey yapma
if (!dmpReady) return;

// MPU interrupt veya ekstra paket için bekle
while (!mpuInterrupt && fifoCount < packetSize)
{
//Mpu verisi yok – PID hesapla ve motorları sür
pid.Compute();
motorController.move(output, MIN_ABS_SPEED);

}

// kesme bayrağını sıfırla ve INT_STATUS baytını al
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

// mevcut FIFO sayısını al
fifoCount = mpu.getFIFOCount();

// taşma olup olmadığını kontrol et
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
// resetle, böylece temiz bir şekilde devam edebiliriz
mpu.resetFIFO();
Serial.println(F(“FIFO taştı!”));

// DMP interrupt’ını kontrol edin (bu sık sık yapılmalıdır)
}
else if (mpuIntStatus & 0x02)
{
// Doğru kullanılabilir veri uzunluğunu bekle
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

// FIFO’dan paket oku
mpu.getFIFOBytes(fifoBuffer, packetSize);

// paketin > 1 olması halinde FIFO sayısını buradan takip edin
// (interrupt beklemeden hemen okuyabilmemizi sağlar.)
fifoCount -= packetSize;

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
input = ypr[1] * 180/M_PI + 180;
}
}