feat: execute EM gear-switch boundaries

2026-08-04 13:01:59 +08:00
parent 55119de1c7
commit de402e61ee
6 changed files with 394 additions and 1 deletions
@@ -0,0 +1,144 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Pure caller-clocked state machine for a zero-speed direction change.</summary>
+public sealed class GearSwitchStateMachine
+{
+    private readonly double stopSpeedToleranceMetersPerSecond;
+    private readonly TimeSpan zeroSpeedHoldDuration;
+    private DateTimeOffset? zeroSpeedSince;
+
+    public GearSwitchStateMachine(double stopSpeedToleranceMetersPerSecond, double zeroSpeedHoldSeconds)
+    {
+        if (!IsNonNegativeFinite(stopSpeedToleranceMetersPerSecond))
+            throw new ArgumentOutOfRangeException(nameof(stopSpeedToleranceMetersPerSecond));
+        if (!IsNonNegativeFinite(zeroSpeedHoldSeconds))
+            throw new ArgumentOutOfRangeException(nameof(zeroSpeedHoldSeconds));
+
+        this.stopSpeedToleranceMetersPerSecond = stopSpeedToleranceMetersPerSecond;
+        zeroSpeedHoldDuration = TimeSpan.FromSeconds(zeroSpeedHoldSeconds);
+    }
+
+    public GearSwitchState State { get; private set; } = GearSwitchState.Following;
+
+    public GearSwitchStateUpdate Update(DateTimeOffset now, double measuredSignedSpeed, TravelDirection desiredDirection,
+        TravelDirection currentDirection, bool directionConfirmed, bool atGearSwitchBoundary, bool atTerminal)
+    {
+        if (double.IsNaN(measuredSignedSpeed) || double.IsInfinity(measuredSignedSpeed))
+            throw new ArgumentOutOfRangeException(nameof(measuredSignedSpeed));
+        if (!Enum.IsDefined(typeof(TravelDirection), desiredDirection) ||
+            !Enum.IsDefined(typeof(TravelDirection), currentDirection))
+        {
+            throw new ArgumentOutOfRangeException(nameof(desiredDirection));
+        }
+
+        if (atTerminal)
+        {
+            State = GearSwitchState.Completed;
+            zeroSpeedSince = null;
+            return Held("terminal boundary reached", false, true);
+        }
+
+        switch (State)
+        {
+            case GearSwitchState.Following:
+                if (desiredDirection != currentDirection)
+                {
+                    State = GearSwitchState.ApproachingGearSwitch;
+                    return Moving("approaching gear-switch boundary");
+                }
+                return Moving("following current direction segment");
+
+            case GearSwitchState.ApproachingGearSwitch:
+                if (!atGearSwitchBoundary)
+                    return Moving("approaching gear-switch boundary");
+                State = GearSwitchState.HoldingZero;
+                zeroSpeedSince = IsStopped(measuredSignedSpeed) ? now : null;
+                return Held("holding at gear-switch boundary", false, false);
+
+            case GearSwitchState.HoldingZero:
+                if (!IsStopped(measuredSignedSpeed))
+                {
+                    zeroSpeedSince = null;
+                    return Held("measured speed remains above stop tolerance", false, false);
+                }
+                if (!zeroSpeedSince.HasValue)
+                {
+                    zeroSpeedSince = now;
+                    return Held("zero-speed dwell started", false, false);
+                }
+                if (now - zeroSpeedSince.Value < zeroSpeedHoldDuration)
+                    return Held("zero-speed dwell in progress", false, false);
+
+                State = GearSwitchState.RequestingDirectionChange;
+                return Held("requesting direction change", true, false);
+
+            case GearSwitchState.RequestingDirectionChange:
+                State = GearSwitchState.AwaitingDirectionConfirmation;
+                return Held("awaiting direction confirmation", false, false);
+
+            case GearSwitchState.AwaitingDirectionConfirmation:
+                if (directionConfirmed && desiredDirection == currentDirection)
+                {
+                    State = GearSwitchState.Following;
+                    zeroSpeedSince = null;
+                    return Moving("direction change confirmed");
+                }
+                return Held("awaiting direction confirmation", false, false);
+
+            case GearSwitchState.Completed:
+                return Held("trajectory already completed", false, true);
+
+            default:
+                throw new InvalidOperationException("Unsupported gear-switch state.");
+        }
+    }
+
+    private GearSwitchStateUpdate Moving(string reason)
+    {
+        return new GearSwitchStateUpdate(State, false, false, true, false, reason);
+    }
+
+    private GearSwitchStateUpdate Held(string reason, bool requestDirectionChange, bool isTrajectoryComplete)
+    {
+        return new GearSwitchStateUpdate(State, true, requestDirectionChange, false, isTrajectoryComplete, reason);
+    }
+
+    private bool IsStopped(double measuredSignedSpeed)
+    {
+        return Math.Abs(measuredSignedSpeed) < stopSpeedToleranceMetersPerSecond;
+    }
+
+    private static bool IsNonNegativeFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value) && value >= 0d;
+    }
+}
+
+public sealed class GearSwitchStateUpdate
+{
+    internal GearSwitchStateUpdate(GearSwitchState state, bool holdZero, bool requestDirectionChange,
+        bool allowsTrajectoryMotion, bool isTrajectoryComplete, string reason)
+    {
+        State = state;
+        HoldZero = holdZero;
+        RequestDirectionChange = requestDirectionChange;
+        AllowsTrajectoryMotion = allowsTrajectoryMotion;
+        IsTrajectoryComplete = isTrajectoryComplete;
+        Reason = reason ?? string.Empty;
+    }
+
+    public GearSwitchState State { get; }
+
+    public bool HoldZero { get; }
+
+    public bool RequestDirectionChange { get; }
+
+    public bool AllowsTrajectoryMotion { get; }
+
+    public bool IsTrajectoryComplete { get; }
+
+    public string Reason { get; }
+}