Denge modu

// include/mpu6050.h — kütüphanesiz, register seviyesinde MPU6050
#pragma once
#include <Arduino.h>
#include <Wire.h>

struct ImuSample {
  float ax, ay, az;   // m/s²
  float gx, gy, gz;   // rad/s (bias çıkarılmış)
};

class Mpu6050 {
 public:
  bool begin(int sda, int scl, uint8_t addr = 0x68) {
    addr_ = addr;
    Wire.begin(sda, scl, 400000);           // 400 kHz: 14 bayt ~0.4 ms
    if (read8(0x75) != 0x68) return false;  // WHO_AM_I
    write8(0x6B, 0x01);   // uyandır, saat kaynağı: gyro X PLL (daha kararlı)
    write8(0x1A, 0x03);   // DLPF ~44 Hz: motor titreşimini keser, ~5 ms gecikme ekler
    write8(0x19, 0x04);   // örnekleme 1 kHz / (1+4) = 200 Hz
    write8(0x1B, 0x08);   // gyro ±500 °/s → 65.5 LSB/(°/s)
    write8(0x1C, 0x00);   // ivme ±2 g → 16384 LSB/g
    return true;
  }

  // Robot hareketsizken çağır: gyro sıfır kaymasını ölç.
  void calibrateGyro(int n = 500) {
    float sx = 0, sy = 0, sz = 0;
    for (int i = 0; i < n; i++) {
      ImuSample s= readRaw();
      sx = s.gx; sy = s.gy; sz = s.gz;
      delay(2);
    }
    bias_[0]= sx / n; bias_[1]= sy / n; bias_[2]= sz / n;
  }

  ImuSample read() {
    ImuSample s= readRaw();
    s.gx -= bias_[0]; s.gy -= bias_[1]; s.gz -= bias_[2];
    return s;
  }

 private:
  ImuSample readRaw() {
    uint8_t b[14];
    Wire.beginTransmission(addr_);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(addr_, (uint8_t)14);
    for (int i= 0; i < 14 && Wire.available(); i++) b[i]= Wire.read();
    auto w= [&](int i) { return static_cast<int16_t>((b[i] << 8) | b[i + 1]); };
    const float A= 9.80665f / 16384.0f;
    const float G= (PI / 180.0f) / 65.5f;
    return {w(0) * A, w(2) * A, w(4) * A, w(8) * G, w(10) * G, w(12) * G};
  }
  uint8_t read8(uint8_t reg) {
    Wire.beginTransmission(addr_);
    Wire.write(reg);
    Wire.endTransmission(false);
    Wire.requestFrom(addr_, (uint8_t)1);
    return Wire.available() ? Wire.read() : 0xFF;
  }
  void write8(uint8_t reg, uint8_t v) {
    Wire.beginTransmission(addr_);
    Wire.write(reg);
    Wire.write(v);
    Wire.endTransmission();
  }
  uint8_t addr_= 0x68;
  float bias_[3]= {0, 0, 0};
};

// include/tilt.h — eğim tahmini: complementary ve Kalman, yan yana
#pragma once
#include <math.h>

// Hangi eksen "öne eğilme"? IMU'yu nasıl monte ettiğine bağlı.
// Robotu elle öne eğ: değeri pozitif artan eksenleri seç.
//   pitch_acc = atan2(ax, az)   (x ileri, z yukarı montajda)
//   pitch_rate = -gy            (sağ el kuralı: y sola bakar)

class Complementary {
 public:
  explicit Complementary(float alpha = 0.98f) : a_(alpha) {}
  float update(float accAngle, float gyroRate, float dt) {
    if (!init_) { angle_ = accAngle; init_ = true; }
    angle_ = a_ * (angle_ + gyroRate * dt) + (1 - a_) * accAngle;
    return angle_;
  }
  void alpha(float a) { a_ = a; }
 private:
  float a_, angle_ = 0;
  bool init_ = false;
};

// 2 durumlu Kalman: [açı, gyro bias]. Bias'ı kendi kendine öğrenir.
class TiltKalman {
 public:
  float qAngle = 0.001f, qBias = 0.003f, rMeasure = 0.03f;
  float update(float accAngle, float gyroRate, float dt) {
    rate_ = gyroRate - bias_;
    angle_ += dt * rate_;
    P_[0][0] += dt * (dt * P_[1][1] - P_[0][1] - P_[1][0] + qAngle);
    P_[0][1] -= dt * P_[1][1];
    P_[1][0] -= dt * P_[1][1];
    P_[1][1] += qBias * dt;
    const float S = P_[0][0] + rMeasure;
    const float K0 = P_[0][0] / S, K1 = P_[1][0] / S;
    const float y = accAngle - angle_;
    angle_ += K0 * y;
    bias_ += K1 * y;
    const float p00 = P_[0][0], p01 = P_[0][1];
    P_[0][0] -= K0 * p00;
    P_[0][1] -= K0 * p01;
    P_[1][0] -= K1 * p00;
    P_[1][1] -= K1 * p01;
    return angle_;
  }
  float rate() const { return rate_; }   // bias'ı düşülmüş açısal hız
  float bias() const { return bias_; }
 private:
  float angle_ = 0, bias_ = 0, rate_ = 0;
  float P_[2][2] = {{0, 0}, {0, 0}};
};

// src/balance.cpp — adım 4.4: kaskad denge kontrolcüsü (dış: hız PI, iç: açı PD)
// 200 Hz, 1. çekirdekte. Robot dik tutulunca devreye girer, 40°'yi geçerse keser.
#include <Arduino.h>
#include "drive.h"     // Motor, QuadEncoder (3. hafta)
#include "mpu6050.h"
#include "pins.h"
#include "tilt.h"

struct Gains {
  float kp = 5.0f, kd = 0.5f;        // PWM / rad, PWM / (rad/s)  — simülasyondaki varsayılanlar
  float kv = 0.5f, kiv = 0.2f;       // rad / (m/s), rad / m
  float trim = 0.0f;                 // rad: ağırlık merkezi tam üstte değilse dengedeki açı
};

Gains gains;
volatile float g_vRef = 0, g_wRef = 0;   // m/s, rad/s — /cmd_vel'den
constexpr float WHEEL_R = 0.0325f, CPR = 1400.0f, TRACK = 0.175f;
constexpr float CUTOFF = 40.0f * DEG_TO_RAD;
constexpr int HZ = 200;

Motor left(PIN_PWMA, PIN_AIN1, PIN_AIN2), right(PIN_PWMB, PIN_BIN1, PIN_BIN2);
QuadEncoder encL(PIN_ENC_L_A, PIN_ENC_L_B), encR(PIN_ENC_R_A, PIN_ENC_R_B);
Mpu6050 imu;
Complementary tilt(0.98f);

enum class Mode { Disarmed, Arming, Balancing, Fallen };
volatile Mode mode = Mode::Disarmed;

void balanceTask(void*) {
  TickType_t wake = xTaskGetTickCount();
  const float dt = 1.0f / HZ;
  int32_t lastL = encL.count(), lastR = encR.count();
  float v = 0, integ = 0, thRef = 0;
  uint32_t uprightSince = 0;
  for (;;) {
    vTaskDelayUntil(&wake, pdMS_TO_TICKS(1000 / HZ));
    ImuSample s = imu.read();
    const float rate = -s.gy;
    const float th = tilt.update(atan2f(s.ax, s.az), rate, dt) - gains.trim;

    int32_t l = encL.count(), r = encR.count();
    const float vL = (l - lastL) / CPR * 2 * PI * WHEEL_R / dt;
    const float vR = (r - lastR) / CPR * 2 * PI * WHEEL_R / dt;
    lastL = l;
    lastR = r;
    v += 0.2f * ((vL + vR) / 2 - v);   // tekerlek hızı gürültülü: hafif filtre

    switch (mode) {
      case Mode::Disarmed:
      case Mode::Fallen:
        left.coast(); right.coast();
        if (fabsf(th) < 5 * DEG_TO_RAD) {
          if (!uprightSince) uprightSince= millis();
          if (millis() - uprightSince > 500) { mode = Mode::Arming; integ = 0; }
        } else uprightSince = 0;
        continue;
      case Mode::Arming:
        mode = Mode::Balancing;
        [[fallthrough]];
      case Mode::Balancing:
        if (fabsf(th) > CUTOFF) { mode = Mode::Fallen; uprightSince = 0; continue; }
        break;
    }

    // dış döngü: hız hatası → istenen eğim (öne gitmek için öne eğil)
    const float e = g_vRef - v;
    integ = constrain(integ + e * dt, -2.0f, 2.0f);
    thRef = constrain(gains.kv * e + gains.kiv * integ, -0.25f, 0.25f);
    // iç döngü: eğim hatası + açısal hız → PWM
    const float u = gains.kp * (th - thRef) + gains.kd * rate;
    // dönüş: iki tekere zıt ofset, dengeyi bozmadan
    const float turn = constrain(g_wRef * 0.08f, -0.3f, 0.3f);
    left.set(constrain(u - turn, -1.0f, 1.0f));
    right.set(constrain(u + turn, -1.0f, 1.0f));
  }
}

void setupBalance() {
  imu.begin(PIN_SDA, PIN_SCL);
  imu.calibrateGyro();      // açılışta robotu yatır ya da sabit tut
  xTaskCreatePinnedToCore(balanceTask, "bal", 4096, nullptr, 6, nullptr, 1);
}

#!/usr/bin/env python3
"""lqr_gain.py — kendi robotunun ölçülerinden LQR kazancı.

  pip install numpy scipy control
Değerleri ölç: gövde kütlesi (tekerlek ve motorlar hariç), ağırlık merkezinin
aks üstündeki yüksekliği (mekanik sayfası verir ya da robotu bir kenarda
dengeleyerek bul), motorun stall torku ve boşta hızı (1. haftadaki step testinden).
Model simülasyondakiyle aynı: τ = 2·τs·(u − ω/ω0), u = PWM (−1..1).
"""
import numpy as np
from scipy.linalg import solve_discrete_are

g = 9.81
r = 0.0325          # tekerlek yarıçapı (m)
mp = 0.70           # gövde (kg)
mc = 0.25           # tekerlek + motor (+ tekerlek ataleti / r²)
l = 0.12            # ağırlık merkezi, aksın üstünde (m)
taus = 0.18         # motor başına stall torku (N·m) — 300 RPM JGA25-370 için tahmin
w0 = 31.4           # boşta açısal hız (rad/s) = 300 RPM
fc = 200            # kontrol frekansı (Hz)

M, ml = mc + mp, mp * l
J = mp * l**2 + mp * (2 * l) ** 2 / 12
a, b = 2 * taus, 2 * taus / (r * w0)
det = M * J - ml**2

# durum: [x, ẋ, θ, θ̇], giriş: u (PWM)
A = np.array([
    [0, 1, 0, 0],
    [0, -b * (J / r + ml) / det, -ml * mp * g * l / det, 0],
    [0, 0, 0, 1],
    [0, b * (ml / r + M) / det, M * mp * g * l / det, 0],
])
B = np.array([[0], [a * (J / r + ml) / det], [0], [-a * (ml / r + M) / det]])

dt = 1 / fc
Ad, Bd = np.eye(4) + A * dt, B * dt          # Euler ayrıklaştırma (dt küçük)
Q = np.diag([1.0, 1.0, 10.0, 0.1])           # konum, hız, eğim, açısal hız
R = np.array([[1.0]])                        # PWM kullanmanın bedeli

P = solve_discrete_are(Ad, Bd, Q, R)
K = np.linalg.solve(R + Bd.T @ P @ Bd, Bd.T @ P @ Ad)
print("açık döngü kutupları:", np.round(np.linalg.eigvals(A), 2))
print("kapalı döngü kutupları:", np.round(np.abs(np.linalg.eigvals(Ad - Bd @ K)), 4), "(hepsi < 1 olmalı)")
print("K =", np.round(K.ravel(), 3), "  →  u = -K · [x - x_ref, v - v_ref, θ, θ̇]")