test_demo/MX-PD-盘古 - new/PanGu.DieBonderApp/MainShell/Motion/AxisSpeedManager.cs

using MainShell.Common;
using MainShell.Hardware;
using MainShell.Log;
using MainShell.Parameter;
using MaxwellFramework.Core.Attributes;
using System;
using System.Collections.Generic;
using System.Linq;
using static MainShell.ParaSetting.Model.SpeedSetting;

namespace MainShell.Motion
{
    public enum AxisSpeedProfile
    {
        Low,
        Medium,
        High,
        Custom
    }

    public sealed class AxisMotionProfile
    {
        public AxisMotionProfile(double velocity, double acceleration, double deceleration, double jerk)
        {
            Velocity = velocity;
            Acceleration = acceleration;
            Deceleration = deceleration;
            Jerk = jerk;
        }

        public double Velocity { get; }
        public double Acceleration { get; }
        public double Deceleration { get; }
        public double Jerk { get; }
    }

    [Singleton]
    public class AxisSpeedManager
    {
        private readonly GlobalParameterContext _globalParam;
        private readonly HardwareManager _hardware;

        public AxisSpeedManager(GlobalParameterContext globalParam, HardwareManager hardware)
        {
            _globalParam = globalParam ?? throw new ArgumentNullException(nameof(globalParam));
            _hardware = hardware ?? throw new ArgumentNullException(nameof(hardware));
        }

        public void SetAxisSpeed(string axisName, double speed)
        {
            var speedItem = GetRequiredSpeedTypeItem(axisName);
            var profile = CreateCustomProfile(speedItem, speed);
            ApplyAxisMotionProfile(axisName, profile, AxisSpeedProfile.Custom, null);
        }

        public void SetAxisSpeed(string axisName, AxisSpeedProfile profile)
        {
            var speedItem = GetRequiredSpeedTypeItem(axisName);
            var motionProfile = CreateProfile(speedItem, profile);
            ApplyAxisMotionProfile(axisName, motionProfile, profile, speedItem);
        }

        public void ApplySpeedSettings()
        {
            var speedItems = _globalParam?.SpeedParaSysSetting?.SpeedTypeItemCollection?.SpeedTypeItemList;
            if (speedItems == null || speedItems.Count == 0)
            {
                "Axis speed settings skipped because no speed configuration is available.".LogInfo();
                return;
            }

            var currentProfile = MapCurrentSpeedTypeText(_globalParam.SpeedParaSysSetting.CurrentSpeedType.ToString());
            foreach (var speedItem in speedItems)
            {
                if (speedItem == null || string.IsNullOrWhiteSpace(speedItem.AxisName))
                {
                    continue;
                }

                var profile = CreateProfile(speedItem, currentProfile);
                ApplyAxisMotionProfile(speedItem.AxisName, profile, currentProfile, speedItem);
            }
        }

        private void ApplyAxisMotionProfile(string axisName, AxisMotionProfile profile, AxisSpeedProfile profileType, SpeedTypeItem speedItem)
        {
            if (string.IsNullOrWhiteSpace(axisName))
            {
                throw new ArgumentNullException(nameof(axisName));
            }

            var axis = _hardware.GetAxisByName(axisName);
            if (axis == null)
            {
                throw new ArgumentException(string.Format("Axis with name {0} not found.", axisName), nameof(axisName));
            }

            ValidateMotionProfile(profile, axisName);

            axis.Param.Velocity = profile.Velocity;
            axis.Param.Accelerate = profile.Acceleration;
            axis.Param.Decelerate = profile.Deceleration;
            axis.Param.Jerk = profile.Jerk;

            try
            {
                axis.IssueParam();
                LogProfileApplied(axisName, profile, profileType, speedItem);
            }
            catch (Exception ex)
            {
                string.Format("Axis:{0} speed profile apply failed. Profile:{1} Error:{2}", axisName, profileType, ex.Message).LogSysError();
                throw;
            }
        }

        private SpeedTypeItem GetRequiredSpeedTypeItem(string axisName)
        {
            if (string.IsNullOrWhiteSpace(axisName))
            {
                throw new ArgumentNullException(nameof(axisName));
            }

            var speedItem = _globalParam?.SpeedParaSysSetting?.SpeedTypeItemCollection?.SpeedTypeItemList?
                .FirstOrDefault(x => x != null && string.Equals(x.AxisName, axisName, StringComparison.OrdinalIgnoreCase));

            if (speedItem == null)
            {
                throw new InvalidOperationException(string.Format("No speed configuration found for axis {0}.", axisName));
            }

            return speedItem;
        }

        private static AxisMotionProfile CreateCustomProfile(SpeedTypeItem speedItem, double speed)
        {
            ValidatePositiveFinite(speed, nameof(speed), speedItem.AxisName);
            ValidatePositiveFinite(speedItem.Acc, nameof(speedItem.Acc), speedItem.AxisName);
            ValidatePositiveFinite(speedItem.Dec, nameof(speedItem.Dec), speedItem.AxisName);
            ValidatePositiveFinite(speedItem.Jerk, nameof(speedItem.Jerk), speedItem.AxisName);
            ValidatePositiveFinite(speedItem.Speed, nameof(speedItem.Speed), speedItem.AxisName);

            var scale = speed / speedItem.Speed;
            return new AxisMotionProfile(
                speed,
                speedItem.Acc * scale,
                speedItem.Dec * scale,
                speedItem.Jerk * scale);
        }

        private static AxisMotionProfile CreateProfile(SpeedTypeItem speedItem, AxisSpeedProfile profile)
        {
            if (speedItem == null)
            {
                throw new ArgumentNullException(nameof(speedItem));
            }

            ValidatePositiveFinite(speedItem.Speed, nameof(speedItem.Speed), speedItem.AxisName);
            ValidatePositiveFinite(speedItem.Acc, nameof(speedItem.Acc), speedItem.AxisName);
            ValidatePositiveFinite(speedItem.Dec, nameof(speedItem.Dec), speedItem.AxisName);
            ValidatePositiveFinite(speedItem.Jerk, nameof(speedItem.Jerk), speedItem.AxisName);

            var percent = ResolveProfilePercent(speedItem, profile);
            var scale = percent / 100.0d;
            return new AxisMotionProfile(
                speedItem.Speed * scale,
                speedItem.Acc * scale,
                speedItem.Dec * scale,
                speedItem.Jerk * scale);
        }

        private static double ResolveProfilePercent(SpeedTypeItem speedItem, AxisSpeedProfile profile)
        {
            switch (profile)
            {
                case AxisSpeedProfile.Low:
                    return ValidatePercent(speedItem.LowPercent, nameof(speedItem.LowPercent), speedItem.AxisName);
                case AxisSpeedProfile.Medium:
                    return ValidatePercent(speedItem.MedianPercent, nameof(speedItem.MedianPercent), speedItem.AxisName);
                case AxisSpeedProfile.High:
                    return ValidatePercent(speedItem.HighPercent, nameof(speedItem.HighPercent), speedItem.AxisName);
                default:
                    return 100.0d;
            }
        }

        private static AxisSpeedProfile MapCurrentSpeedTypeText(string currentSpeedTypeText)
        {
            if (string.Equals(currentSpeedTypeText, "High", StringComparison.OrdinalIgnoreCase))
            {
                return AxisSpeedProfile.High;
            }

            if (string.Equals(currentSpeedTypeText, "Median", StringComparison.OrdinalIgnoreCase)
                || string.Equals(currentSpeedTypeText, "Medium", StringComparison.OrdinalIgnoreCase))
            {
                return AxisSpeedProfile.Medium;
            }

            return AxisSpeedProfile.Low;
        }

        private static void ValidateMotionProfile(AxisMotionProfile profile, string axisName)
        {
            if (profile == null)
            {
                throw new ArgumentNullException(nameof(profile));
            }

            ValidatePositiveFinite(profile.Velocity, nameof(profile.Velocity), axisName);
            ValidatePositiveFinite(profile.Acceleration, nameof(profile.Acceleration), axisName);
            ValidatePositiveFinite(profile.Deceleration, nameof(profile.Deceleration), axisName);
            ValidatePositiveFinite(profile.Jerk, nameof(profile.Jerk), axisName);
        }

        private static void ValidatePositiveFinite(double value, string propertyName, string axisName)
        {
            if (double.IsNaN(value) || double.IsInfinity(value) || value <= 0d)
            {
                throw new ArgumentOutOfRangeException(propertyName, string.Format("Axis {0} has invalid motion parameter {1}: {2}.", axisName, propertyName, value));
            }
        }

        private static double ValidatePercent(double value, string propertyName, string axisName)
        {
            if (double.IsNaN(value) || double.IsInfinity(value) || value <= 0d || value > 100d)
            {
                throw new ArgumentOutOfRangeException(propertyName, string.Format("Axis {0} has invalid speed profile percent {1}: {2}.", axisName, propertyName, value));
            }

            return value;
        }

        private static void LogProfileApplied(string axisName, AxisMotionProfile profile, AxisSpeedProfile profileType, SpeedTypeItem speedItem)
        {
            var source = speedItem != null ? "ConfiguredProfile" : "CustomScaledProfile";
            string.Format(
                "Axis:{0} speed profile applied. Source:{1} Profile:{2} Velocity:{3:F3} Acc:{4:F3} Dec:{5:F3} Jerk:{6:F3}",
                axisName,
                source,
                profileType,
                profile.Velocity,
                profile.Acceleration,
                profile.Deceleration,
                profile.Jerk).LogInfo();
        }
    }
}