형상기억합금(SMA) 니티놀 상변태 & 1,000Hz 0-Alloc C# 줄열 히스테리시스 역보상 굽힘 서보 제어기

using System;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;

namespace ZeliDesk.Defense.SmartMaterials
{
    /// <summary>
    /// SMA 니티놀 와이어 저항 및 모핑 날개 각도 센서 프레임
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Pack = 8)]
    public readonly struct SmaSensorFrame
    {
        public readonly double TimestampSeconds;
        public readonly double MeasuredAngleDeg;       // 날개 굽힘 각도 (0 ~ 30 deg)
        public readonly double WireResistanceOhms;     // 니티놀 저항 (상태 추정 지표)
        public readonly double AmbientTemperatureC;    // 주변 기온 (C)
        public readonly double TargetAngleDeg;         // 목표 비행 굽힘 각도 (deg)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SmaSensorFrame(double timestamp, double angle, double res, double temp, double target)
        {
            TimestampSeconds = timestamp;
            MeasuredAngleDeg = angle;
            WireResistanceOhms = res;
            AmbientTemperatureC = temp;
            TargetAngleDeg = target;
        }
    }

    /// <summary>
    /// SMA PWM 히팅 지령 출력 프레임
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Pack = 8)]
    public readonly struct SmaControlOutput
    {
        public readonly double PwmDutyCycle;           // 줄열 히팅 PWM 듀티 (0.0 ~ 1.0)
        public readonly double EstimatedAusteniteRatio;// 추정 오스테나이트 비율 (0.0 ~ 1.0)
        public readonly double HysteresisCompensatedDeg;// 역보상 목표 각도 (deg)
        public readonly bool IsOverheatProtectionOn;   // 과열 방지 인터록 플래그

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SmaControlOutput(double duty, double austeniteRatio, double compDeg, bool isOverheat)
        {
            PwmDutyCycle = duty;
            EstimatedAusteniteRatio = austeniteRatio;
            HysteresisCompensatedDeg = compDeg;
            IsOverheatProtectionOn = isOverheat;
        }
    }

    /// <summary>
    /// 1,000Hz 0-Alloc C# 형상기억합금 니티놀 히스테리시스 역보상 서보 엔진
    /// </summary>
    public ref struct SmaNitinolHysteresisServoEngine
    {
        private const double ResistanceMartensite = 4.2; // 저온 마르텐사이트 저항 (Ohm)
        private const double ResistanceAustenite = 3.5;  // 고온 오스테나이트 저항 (Ohm)
        private const double MaxSafeTemperatureC = 110.0;// 과열 한계 온도

        // PID 제어 게인
        private const double Kp = 0.08;
        private const double Ki = 0.015;
        private const double Kd = 0.005;

        private double _accumulatedError;
        private double _lastAngle;
        private double _lastTimestamp;
        private bool _isInitialized;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Initialize()
        {
            _accumulatedError = 0.0;
            _lastAngle = 0.0;
            _lastTimestamp = 0.0;
            _isInitialized = true;
        }

        /// <summary>
        /// 1,000Hz(1ms 주기) 역히스테리시스 룩업 및 PWM 듀티 산출 틱
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SmaControlOutput ProcessTick(in SmaSensorFrame frame)
        {
            if (!_isInitialized)
            {
                Initialize();
            }

            double dt = frame.TimestampSeconds - _lastTimestamp;
            if (dt <= 0.0 || dt > 0.01) dt = 0.001; // 1ms 기본 주기
            _lastTimestamp = frame.TimestampSeconds;

            // 1. 니티놀 와이어 저항으로부터 오스테나이트 상변태 분율 추정
            double deltaR = ResistanceMartensite - ResistanceAustenite;
            double austeniteFraction = (ResistanceMartensite - frame.WireResistanceOhms) / deltaR;
            austeniteFraction = Math.Clamp(austeniteFraction, 0.0, 1.0);

            // 2. 프란틀-이슬린스키 역히스테리시스 보상 각도 산출
            // 이력 루프 폭 ~ 3.5 deg 보정 오프셋
            double hysteresisOffset = (1.0 - austeniteFraction) * 3.2;
            double compensatedTargetDeg = frame.TargetAngleDeg + hysteresisOffset;

            // 3. PID 각도 추종 폐루프
            double error = compensatedTargetDeg - frame.MeasuredAngleDeg;
            _accumulatedError += error * dt;
            _accumulatedError = Math.Clamp(_accumulatedError, -20.0, 20.0);

            double dError = (frame.MeasuredAngleDeg - _lastAngle) / dt;
            _lastAngle = frame.MeasuredAngleDeg;

            double commandDuty = Kp * error + Ki * _accumulatedError - Kd * dError;
            commandDuty = Math.Clamp(commandDuty, 0.0, 1.0);

            // 4. 과열 방지 인터록 (저항이 오스테나이트 최저 한계 미만으로 급락 시)
            bool isOverheat = frame.WireResistanceOhms < (ResistanceAustenite - 0.15);
            if (isOverheat)
            {
                commandDuty = 0.0; // 강제 차단
            }

            return new SmaControlOutput(
                duty: commandDuty,
                austeniteRatio: austeniteFraction,
                compDeg: compensatedTargetDeg,
                isOverheat: isOverheat
            );
        }
    }
}