orac_hardware.py

"""
ORAC-NT + MPU-6050 – ХАРДУЕРНА ВЕРСИЯ v2
Фикс: baseline се изчислява ПООТДЕЛНО за всяка ос
"""

import serial
import time
import numpy as np
from collections import deque


class Watchdog_v5:
    def __init__(self, h_limit=0.012, persistence_req=1, warmup_steps=100):
        self.h_limit = h_limit
        self.persistence_req = persistence_req
        self.warmup = 0
        self.warmup_steps = warmup_steps
        self.consecutive = 0
        self.cus_pos = [0., 0., 0.]

        # Per-axis baseline
        self.baseline = [deque(maxlen=warmup_steps) for _ in range(3)]
        self.avg = [None, None, None]
        self.k_drift = [None, None, None]

    def compute(self, sensors):
        self.warmup += 1

        # Фаза 1: калибриране per-axis
        if self.warmup <= self.warmup_steps:
            for i, s in enumerate(sensors):
                self.baseline[i].append(abs(s))
            if self.warmup > 10:
                for i in range(3):
                    self.avg[i] = np.mean(self.baseline[i])
                    self.k_drift[i] = max(0.005, np.std(self.baseline[i]) * 0.5)
            return 0.0, 'CALIBRATING'

        # Фаза 2: magnitude deviation detector
        magnitude = np.sqrt(sum(s**2 for s in sensors))
        baseline_mag = np.sqrt(sum(self.avg[i]**2 for i in range(3)))
        deviation = abs(magnitude - baseline_mag)

       

        if deviation > self.h_limit:
            self.consecutive += 1
            if self.consecutive >= self.persistence_req:
                self.consecutive = 0
                return 0.9, 'ANOMALY'
        else:
            self.consecutive = max(0, self.consecutive - 1)

        return 0.0, 'NONE'


class OracController:
    MODES = {'NORMAL': 0.0, 'SURVIVAL': 70.0}

    def __init__(self):
        self.current_mode = 'NORMAL'
        self.temp = 25.0

    def decide(self, score, fault, G):
        if self.current_mode == 'SURVIVAL':
            self.temp += 0.02 * (25 - self.temp)
        else:
            self.temp += 0.01 * (30 - self.temp)

        Q = 1.0 - score
        D = 1.0 - (self.MODES[self.current_mode] / 100.0)
        T = np.clip((self.temp - 25) / 30, 0, 1)
        W = Q * D - T

        if fault == 'ANOMALY':
            self.current_mode = 'SURVIVAL'
            status = "⚠️  АНОМАЛИЯ"
        elif G > 1.5 or G < 0.5:
            self.current_mode = 'SURVIVAL'
            status = "💥 УДАР"
        elif W < 0.08:
            self.current_mode = 'SURVIVAL'
            status = "🔥 КРИТИЧНО"
        else:
            self.current_mode = 'NORMAL'
            status = "✅ НОРМАЛНО"

        return self.current_mode, W, status


def main():
    print("=" * 70)
    print("🚀 ORAC-NT v5.4 + MPU-6050 — ХАРДУЕРЕН ТЕСТ v2")
    print("=" * 70)

    try:
        ser = serial.Serial('COM4', 115200, timeout=1)
        time.sleep(2)
        print("✅ Свързан с Arduino на COM4\n")
    except Exception as e:
        print(f"❌ Грешка: {e}")
        return

    watchdog = Watchdog_v5(warmup_steps=100)
    orac = OracController()

    print("⏳ Калибриране (100 стъпки)...\n")
    print(f"{'Стъпка':>6} | {'G(X)':>7} | {'G(Y)':>7} | {'G(Z)':>7} | "
          f"{'W':>7} | {'Режим':<10} | Статус")
    print("-" * 78)

    step = 0
    try:
        while True:
            line = ser.readline().decode(errors='ignore').strip()
            if line:
                parts = line.split(',')
                if len(parts) == 3:
                    try:
                        ax, ay, az = map(int, parts)
                        gx, gy, gz = ax/16384., ay/16384., az/16384.

                        score, fault = watchdog.compute([gx, gy, gz])
                        mode, W, status = orac.decide(score, fault, gz)

                        if fault == 'CALIBRATING':
                            if step % 10 == 0 and watchdog.avg[2]:
                                print(f"{step:6d} | {gx:7.3f} | {gy:7.3f} | {gz:7.3f} | "
                                      f"{'---':>7} | {'CALIBR.':<10} | "
                                      f"Z_avg={watchdog.avg[2]:.4f} "
                                      f"k={watchdog.k_drift[2]:.4f}")
                        else:
                            print(f"{step:6d} | {gx:7.3f} | {gy:7.3f} | {gz:7.3f} | "
                                  f"{W:7.3f} | {mode:<10} | {status}")
                        step += 1
                    except Exception:
                        pass
            time.sleep(0.01)
    except KeyboardInterrupt:
        print("\n\n👋 Спиране.")
        if watchdog.avg[2]:
            print(f"\n📊 Per-axis baseline:")
            axes = ['X', 'Y', 'Z']
            for i in range(3):
                print(f"   G({axes[i]}): avg={watchdog.avg[i]:.4f}  "
                      f"k_drift={watchdog.k_drift[i]:.4f}")
        ser.close()
        print("✅ Затворено.")

if __name__ == "__main__":
    main()