Kendi Kendine Dengelenen Robot Yapımı — PID Kontrolü ile İki Tekerli Denge Robotu

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

// ─── PID KATSAYILARI (kalibre edilmesi gerekiyor) ───
double Kp = 25.0;
double Ki = 140.0;
double Kd = 0.8;

double hedefAci = 0.0;  // Dik durma hedefi (derece)
double mevcutAci = 0.0;
double onceki_hata = 0.0;
double integral_toplam = 0.0;

// Motor pinleri
#define MOTOR_SOL_ILERI 5
#define MOTOR_SOL_GERI 6
#define MOTOR_SAG_ILERI 9
#define MOTOR_SAG_GERI 10

unsigned long onceki_zaman;

void setup() {
    Serial.begin(115200);
    Wire.begin();

    mpu.initialize();
    if (!mpu.testConnection()) {
        Serial.println("MPU6050 baglanti hatasi!");
        while (1);
    }

    pinMode(MOTOR_SOL_ILERI, OUTPUT);
    pinMode(MOTOR_SOL_GERI, OUTPUT);
    pinMode(MOTOR_SAG_ILERI, OUTPUT);
    pinMode(MOTOR_SAG_GERI, OUTPUT);

    onceki_zaman = millis();

    Serial.println("Denge robotu baslatiliyor...");
    delay(1000);
}

double aci_hesapla() {
    int16_t ax, ay, az, gx, gy, gz;
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // İvmeölçerden basit açı hesaplama (gerçek projede
    // Kalman filtre veya complementary filter kullanılmalı —
    // bu basitleştirilmiş versiyon eğitim amaçlıdır)
    double ivme_aci = atan2(ay, az) * 180.0 / PI;

    return ivme_aci;
}

double pid_hesapla(double hata, double deltaZaman) {
    integral_toplam += hata * deltaZaman;

    // Anti-windup — integral terimin sınırsız büyümesini önle
    integral_toplam = constrain(integral_toplam, -50, 50);

    double turev = (hata - onceki_hata) / deltaZaman;

    double cikti = (Kp * hata) + (Ki * integral_toplam) + (Kd * turev);

    onceki_hata = hata;

    return cikti;
}

void motorlari_kontrol_et(double guc) {
    // Pozitif güç = ileri, negatif güç = geri
    int pwm_deger = constrain(abs(guc), 0, 255);

    if (guc > 0) {
        analogWrite(MOTOR_SOL_ILERI, pwm_deger);
        digitalWrite(MOTOR_SOL_GERI, LOW);
        analogWrite(MOTOR_SAG_ILERI, pwm_deger);
        digitalWrite(MOTOR_SAG_GERI, LOW);
    } else {
        digitalWrite(MOTOR_SOL_ILERI, LOW);
        analogWrite(MOTOR_SOL_GERI, pwm_deger);
        digitalWrite(MOTOR_SAG_ILERI, LOW);
        analogWrite(MOTOR_SAG_GERI, pwm_deger);
    }
}

void loop() {
    unsigned long simdiki_zaman = millis();
    double deltaZaman = (simdiki_zaman - onceki_zaman) / 1000.0;
    onceki_zaman = simdiki_zaman;

    mevcutAci = aci_hesapla();
    double hata = hedefAci - mevcutAci;

    // Robot çok fazla eğildiyse (düştüyse) motorları durdur — güvenlik
    if (abs(mevcutAci) > 45) {
        motorlari_kontrol_et(0);
        integral_toplam = 0;  // Integral'i sıfırla, tekrar denge kurunca fresh başlasın
        return;
    }

    double pid_ciktisi = pid_hesapla(hata, deltaZaman);
    motorlari_kontrol_et(pid_ciktisi);

    // Debug için seri port çıktısı
    Serial.print("Aci: ");
    Serial.print(mevcutAci);
    Serial.print(" | PID Cikti: ");
    Serial.println(pid_ciktisi);

    delay(10);  // ~100Hz kontrol döngüsü
}