Dengeyi öğret

"""balance_env.py — 4. haftadaki denge modelinin gymnasium ortamı.

Fizik simülasyondakinin aynısı: arabalı ters sarkaç + back-EMF motor modeli,
200 Hz kontrol, RK4. Domain randomization her epizotta parametreleri oynatır.
  pip install gymnasium stable-baselines3 numpy
"""
import math

import gymnasium as gym
import numpy as np

G, DT = 9.81, 0.005


class BalanceEnv(gym.Env):
    metadata = {"render_modes": []}

    def __init__(self, randomize=True, latency_steps=1):
        self.randomize = randomize
        self.latency = latency_steps
        # gözlem: [x, xdot, theta, thetadot]; eylem: motor gerilim oranı [-1, 1]
        hi = np.array([2.0, 5.0, 0.8, 10.0], dtype=np.float32)
        self.observation_space = gym.spaces.Box(-hi, hi, dtype=np.float32)
        self.action_space = gym.spaces.Box(-1.0, 1.0, shape=(1,), dtype=np.float32)

    def _params(self):
        r = self.np_random
        k = (lambda lo, hi: r.uniform(lo, hi)) if self.randomize else (lambda lo, hi: (lo + hi) / 2)
        self.l = k(0.09, 0.15)          # gövde COM yüksekliği (m)
        self.mp = k(0.55, 0.85)         # gövde kütlesi
        self.mc = 0.25
        self.r = 0.0325
        self.tau_s = k(0.15, 0.21)      # iki motorun toplam durma torku (N·m)
        self.w0 = k(28.0, 34.0)         # boşta açısal hız (rad/s)
        self.noise = k(0.0, 0.01)

    def reset(self, seed=None, options=None):
        super().reset(seed=seed)
        self._params()
        self.s = np.array([0.0, 0.0, self.np_random.uniform(-0.15, 0.15), 0.0])
        self.queue = [0.0] * self.latency
        self.t = 0
        return self._obs(), {}

    def _obs(self):
        o = self.s + self.np_random.normal(0, self.noise, 4) * np.array([0, 0.1, 1, 5])
        return o.astype(np.float32)

    def _deriv(self, s, u):
        x, xd, th, thd = s
        w = xd / self.r
        torque = 2 * self.tau_s * (u - w / self.w0)       # τ = 2τs(u − ω/ω0)
        F = torque / self.r
        M, m, l = self.mc + self.mp, self.mp, self.l
        st, ct = math.sin(th), math.cos(th)
        den = M - m * ct * ct
        xdd = (F + m * l * thd * thd * st - m * G * st * ct) / den
        thdd = (G * st - xdd * ct) / l
        return np.array([xd, xdd, thd, thdd])

    def step(self, action):
        self.queue.append(float(np.clip(action[0], -1, 1)))
        u = self.queue.pop(0)
        s = self.s
        k1 = self._deriv(s, u)
        k2 = self._deriv(s + DT / 2 * k1, u)
        k3 = self._deriv(s + DT / 2 * k2, u)
        k4 = self._deriv(s + DT * k3, u)
        self.s = s + DT / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
        self.t += 1
        x, xd, th, thd = self.s
        fell = abs(th) > 0.6 or abs(x) > 1.5
        reward = 1.0 - 2 * th * th - 0.1 * x * x - 0.01 * xd * xd - 0.01 * u * u
        return self._obs(), (-10.0 if fell else reward), fell, self.t >= 2000, {}

"""train.py — PPO ile denge politikası; aynı ortamda LQR ile karşılaştır."""
import numpy as np
from stable_baselines3 import PPO
from stable_baselines3.common.env_util import make_vec_env

from balance_env import BalanceEnv

env = make_vec_env(lambda: BalanceEnv(randomize=True), n_envs=8)
model = PPO(
    "MlpPolicy", env,
    policy_kwargs=dict(net_arch=[32, 32]),     # ESP32'ye sığacak kadar küçük
    n_steps=2048, batch_size=256, gamma=0.995, learning_rate=3e-4, verbose=1,
)
model.learn(total_timesteps=1_500_000)
model.save("ppo_balance")

K = np.array([-0.84, -2.54, -5.08, -0.605])   # 4. haftadaki LQR kazancı


def evaluate(policy, n=50, randomize=True):
    ok, rms = 0, []
    for seed in range(n):
        e = BalanceEnv(randomize=randomize)
        o, _ = e.reset(seed=seed)
        ths = []
        for _ in range(2000):
            o, _, term, trunc, _ = e.step(policy(o))
            ths.append(o[2])
            if term or trunc:
                break
        ok += not term
        rms.append(float(np.sqrt(np.mean(np.square(ths)))))
    return ok / n, np.degrees(np.mean(rms))


ppo = lambda o: model.predict(o, deterministic=True)[0]
lqr = lambda o: np.clip([-K @ o], -1, 1)
for name, pol in [("PPO", ppo), ("LQR", lqr)]:
    s, r = evaluate(pol)
    print(f"{name}: ayakta kalma {s:.0%}, ortalama |θ| rms {r:.2f}°")

"""export_header.py — PPO aktörünü ESP32 için düz C dizilerine çevir."""
import numpy as np
from stable_baselines3 import PPO

model = PPO.load("ppo_balance")
layers = [m for m in model.policy.mlp_extractor.policy_net if hasattr(m, "weight")] + [model.policy.action_net]

with open("policy.h", "w") as f:
    f.write("#pragma once\n// otomatik üretildi: PPO aktörü, tanh gizli katmanlar\n")
    for i, L in enumerate(layers):
        W = L.weight.detach().numpy()
        b = L.bias.detach().numpy()
        f.write(f"constexpr int L{i}_IN = {W.shape[1]}, L{i}_OUT = {W.shape[0]};\n")
        f.write(f"const float L{i}_W[] = {{{', '.join(f'{v:.6f}f' for v in W.flatten())}}};\n")
        f.write(f"const float L{i}_B[] = {{{', '.join(f'{v:.6f}f' for v in b)}}};\n")
print("policy.h yazıldı")