HC_APTBS/Services/Impl/BenchPidController.cs

using System;
using System.Diagnostics;
using System.Threading;
using System.Threading.Tasks;

namespace HC_APTBS.Services.Impl
{
    /// <summary>
    /// PID controller for smooth RPM ramping on the test bench motor.
    /// Runs a background loop that reads actual RPM, computes the PID output,
    /// and writes the corresponding motor voltage via a callback.
    ///
    /// <para>
    /// All values are normalized to 0–1.0 internally. The output is scaled back
    /// to the configured voltage range before calling the write delegate.
    /// </para>
    /// </summary>
    public sealed class BenchPidController : IDisposable
    {
        // ── Gains ────────────────────────────────────────────────────────────────

        private readonly double _kp;
        private readonly double _ki;
        private readonly double _kd;

        // ── Ranges ───────────────────────────────────────────────────────────────

        /// <summary>Maximum process variable (RPM).</summary>
        private readonly double _pvMax;

        /// <summary>Maximum output variable (voltage).</summary>
        private readonly double _outMax;

        // ── Delegates ────────────────────────────────────────────────────────────

        private readonly Func<double> _readActualRpm;
        private readonly Func<double> _readTargetRpm;
        private readonly Action<double> _sendVoltage;

        // ── Loop state ───────────────────────────────────────────────────────────

        private readonly int _intervalMs;
        private CancellationTokenSource? _cts;
        private Task? _loopTask;

        // ── PID state ────────────────────────────────────────────────────────────

        private double _errSum;
        private double _lastPv;
        private Stopwatch _sw = new();

        /// <summary>True when the PID loop is actively running.</summary>
        public bool IsRunning { get; private set; }

        // ── Constructor ──────────────────────────────────────────────────────────

        /// <summary>
        /// Creates a new PID controller for bench RPM control.
        /// </summary>
        /// <param name="kp">Proportional gain.</param>
        /// <param name="ki">Integral gain.</param>
        /// <param name="kd">Derivative gain.</param>
        /// <param name="intervalMs">Loop interval in milliseconds.</param>
        /// <param name="pvMax">Maximum process variable (RPM). Minimum is always 0.</param>
        /// <param name="outMax">Maximum output (voltage). Minimum is always 0.</param>
        /// <param name="readActualRpm">Delegate to read the current bench RPM.</param>
        /// <param name="readTargetRpm">Delegate to read the desired RPM setpoint.</param>
        /// <param name="sendVoltage">Delegate to write the output voltage to the CAN bus.</param>
        public BenchPidController(
            double kp, double ki, double kd, int intervalMs,
            double pvMax, double outMax,
            Func<double> readActualRpm,
            Func<double> readTargetRpm,
            Action<double> sendVoltage)
        {
            _kp = kp;
            _ki = ki;
            _kd = kd;
            _intervalMs = Math.Max(1, intervalMs);
            _pvMax = pvMax;
            _outMax = outMax;
            _readActualRpm = readActualRpm;
            _readTargetRpm = readTargetRpm;
            _sendVoltage = sendVoltage;
        }

        // ── Public API ───────────────────────────────────────────────────────────

        /// <summary>
        /// Starts the PID loop with bumpless transfer from the given initial conditions.
        /// If already running, stops first before restarting.
        /// </summary>
        /// <param name="initialVoltage">Current motor voltage (for bumpless startup).</param>
        /// <param name="initialRpm">Current actual RPM (for bumpless startup).</param>
        public void Start(double initialVoltage, double initialRpm)
        {
            if (IsRunning) Stop();

            // Bumpless startup: pre-initialize integral sum so the first output
            // matches the current voltage, avoiding a step response.
            double normalizedOutput = Scale(initialVoltage, 0, _outMax, 0, 1.0);
            _errSum = _ki != 0 ? normalizedOutput / _ki : 0;
            _lastPv = Scale(Clamp(initialRpm, 0, _pvMax), 0, _pvMax, 0, 1.0);
            _sw = Stopwatch.StartNew();

            IsRunning = true;
            _cts = new CancellationTokenSource();
            var ct = _cts.Token;

            _loopTask = Task.Run(async () =>
            {
                using var timer = new PeriodicTimer(TimeSpan.FromMilliseconds(_intervalMs));
                try
                {
                    while (IsRunning && await timer.WaitForNextTickAsync(ct))
                    {
                        Compute();
                    }
                }
                catch (OperationCanceledException) { }
            }, ct);
        }

        /// <summary>
        /// Stops the PID loop and sends 0 V to the motor.
        /// </summary>
        public void Stop()
        {
            if (!IsRunning) return;
            IsRunning = false;
            _cts?.Cancel();
            _cts?.Dispose();
            _cts = null;
            _sendVoltage(0);
        }

        // ── PID core ─────────────────────────────────────────────────────────────

        private void Compute()
        {
            double pv = _readActualRpm();
            double sp = _readTargetRpm();

            // Clamp and normalize to 0–1.0
            pv = Scale(Clamp(pv, 0, _pvMax), 0, _pvMax, 0, 1.0);
            sp = Scale(Clamp(sp, 0, _pvMax), 0, _pvMax, 0, 1.0);

            double err = sp - pv;

            // Time delta in seconds
            double dt = _sw.Elapsed.TotalSeconds;
            _sw.Restart();
            if (dt <= 0) dt = _intervalMs / 1000.0;

            // P term
            double pTerm = _kp * err;

            // I term (with anti-windup: only integrate when PV is in-range)
            double partialSum = _errSum;
            double iTerm = 0;
            if (pv >= 0 && pv <= 1.0)
            {
                partialSum = _errSum + dt * err;
                iTerm = _ki * partialSum;
            }

            // D term (derivative-on-measurement, not on error)
            double dTerm = 0;
            if (dt > 0)
                dTerm = _kd * (pv - _lastPv) / dt;

            // Combine, clamp, scale to output voltage range
            double output = Clamp(pTerm + iTerm + dTerm, 0, 1.0);
            double voltage = Scale(output, 0, 1.0, 0, _outMax);

            _sendVoltage(voltage);

            // Update state
            _errSum = partialSum;
            _lastPv = pv;
        }

        // ── Helpers ──────────────────────────────────────────────────────────────

        private static double Clamp(double value, double min, double max)
            => value > max ? max : value < min ? min : value;

        private static double Scale(double value, double fromMin, double fromMax, double toMin, double toMax)
        {
            if (Math.Abs(fromMax - fromMin) < 1e-12) return toMin;
            double pct = (value - fromMin) / (fromMax - fromMin);
            return toMin + pct * (toMax - toMin);
        }

        /// <inheritdoc/>
        public void Dispose()
        {
            IsRunning = false;
            _cts?.Cancel();
            _cts?.Dispose();
            _cts = null;
        }
    }
}