Display_D_HP/TeamAAS-VM/Core/Robots/XYZU_Robot.cs

using MahApps.Metro.Controls;
using Opc.Ua;
using Prism;
using Prism.Ioc;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Diagnostics;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using TeamAAS_VP.Core.PLCs;
using TeamAAS_VP.Enums;
using TeamAAS_VP.Interfaces;
using TeamAAS_VP.Models;
using TouchSocket.Core;
using TouchSocket.Sockets;
using static System.Windows.Forms.AxHost;

namespace TeamAAS_VP.Core.Robots
{
    /// <summary>
    /// Generic multi-axis robot that composes axis objects and uses PLC nodes configured in RobotInfo.PlcRobotParameter
    /// to perform coordinated moves (Go/Move/CalibMotion) for 3 or 4 axes.
    /// This implementation maps X/Y/Z/(U) axes by name and writes their position nodes then triggers ExecuteMove node.
    /// </summary>
    public class XYZU_Robot : IRobot, INotifyPropertyChanged
    {
        public event Action<Guid, object, ConnectedEventArgs> ConnectedEvent;
        public event Action<Guid, object, ClosedEventArgs> DisconnectedEvent;
        public event Action<Guid, object, ReceivedDataEventArgs> ReceivedEvent;
        public event Action<Guid, object, string> SendEvent;

        public XYZU_Robot(RobotInfo robot, OpcUaClientPLC pLC)
        {
            RobotInfo = robot;
            Name = robot.RobotName;
            Id = robot.Id;
            RobotNo = robot.RobotNo;
            RobotIp = robot.IP;
            RobotPort = robot.Port;
            ConnectType = robot.ConnectType;
            Terminator = robot.Terminator;
            DataEncoding = robot.DataEncoding;
            Brand = RobotBrand.XYZ_Platform;
            Plc = pLC;
            Plc.ConnectChangedEvent += Plc_ConnectChangedEvent;

            // build axis list from known RobotInfo.PlcRobotParameter nodes (best-effort)
            Axes = new List<Axis>();
            // X
            var ax = new Axis();
            ax.Name = "X";
            ax.Index = 1;
            ax.Command = robot.PlcRobotParameter.AxixList[0].Command;
            ax.Parameter = robot.PlcRobotParameter.AxixList[0].Parameter;
            ax.State = robot.PlcRobotParameter.AxixList[0].State;
            Axes.Add(ax);

            // Y
            var ay = new Axis();
            ay.Name = "Y";
            ay.Index = 2;
            ay.Command = robot.PlcRobotParameter.AxixList[1].Command;
            ay.Parameter = robot.PlcRobotParameter.AxixList[1].Parameter;
            ay.State = robot.PlcRobotParameter.AxixList[1].State;
            Axes.Add(ay);
            // Z
            var az = new Axis();
            az.Name = "Z";
            az.Index = 3;
            az.Command = robot.PlcRobotParameter.AxixList[2].Command;
            az.Parameter = robot.PlcRobotParameter.AxixList[2].Parameter;
            az.State = robot.PlcRobotParameter.AxixList[2].State;
            Axes.Add(az);
            // U optional
            if (robot.PlcRobotParameter.AxixList.Count >= 4)
            {
                var au = new Axis();
                au.Name = "U";
                au.Index = 4;
                au.Command = robot.PlcRobotParameter.AxixList[3].Command;
                au.Parameter = robot.PlcRobotParameter.AxixList[3].Parameter;
                au.State = robot.PlcRobotParameter.AxixList[3].State;
                Axes.Add(au);
                Brand = RobotBrand.XYZU_Platform;
            }

            //所有轴的位置节点
            
            var nodeIds = Axes.Where(a => !string.IsNullOrWhiteSpace(a.State.ActPositionNode)).Select(a => a.State.ActPositionNode).ToArray();
            List<string> positionNodes = new List<string>(nodeIds);
            pLC.SubscribeNodes("AxisPosition",positionNodes, (res) =>
            {
                if (res.key!= "AxisPosition") return;

                for (int i = 0; i < Axes.Count; i++)
                {
                    var axis = Axes[i];
                    if (axis.State.ActPositionNode.Contains(res.nodeId))
                    {
                        float v = Convert.ToSingle(res.value);
                        switch (axis.Name)
                        {
                            case "X": CurrentPosition.X = v; break;
                            case "Y": CurrentPosition.Y = v; break;
                            case "Z": CurrentPosition.Z = v; break;
                            case "U": CurrentPosition.U = v; break;
                        }
                    }
                }
            });
        }

        private void Plc_ConnectChangedEvent(object arg1, bool arg2)
        {
            CanExecute = arg2;
            if (arg2)
                ConnectedEvent?.Invoke(Id, this, null);
            else
                DisconnectedEvent?.Invoke(Id, this, null);
        }

        #region 属性
        public TcpClient TcpClient { get; private set; }

        public TcpService TcpService { get; private set; }

        public Guid Id { get; set; }

        public string Name { get; set; }

        /// <summary>
        /// 机器人编号
        /// </summary>
        public int RobotNo { get; set; }

        public int RobotPort { get; private set; }

        public string RobotIp { get; private set; }

        public TCPConnectType ConnectType { get; private set; }

        public Terminator Terminator { get; private set; }

        public DataEncoding DataEncoding { get; private set; }

        public bool IsConnected
        {
            get
            {
                if (Plc != null)
                {
                    return Plc.IsConnected;
                }
                else
                {
                    return false;
                }
            }
        }

        public int Timeout { get; set; } = 120000;

        private bool _CanExecute = true;

        public bool CanExecute
        {
            get { return _CanExecute; }
            set { SetProperty(ref _CanExecute, value); }
        }

        public int SelectedTool { get; private set; } = 0;

        public RobotBrand Brand { get; private set; }

        public OpcUaClientPLC Plc { get; private set; }

        public RobotInfo RobotInfo { get; private set; }

        public List<Axis> Axes { get; private set; }

        /// <summary>
        /// 进入调试模式
        /// </summary>
        /// <returns></returns>
        public bool EnterDebugMode { get; set; }

        private RPoint _CurrentPosition = new RPoint();
        /// <summary>
        /// 当前位置
        /// </summary>
        public RPoint CurrentPosition
        {
            get { return _CurrentPosition; }
            set { SetProperty(ref _CurrentPosition, value); }
        }

        #endregion

        #region 连接
        public void Connect()
        {
            if (Plc.IsConnected)
            {
                ConnectedEvent?.Invoke(Id, this, null);
            }
        }

        public Task ConnectAsync()
        {
            if (Plc.IsConnected)
            {
                ConnectedEvent?.Invoke(Id, this, null);
            }
            return Task.CompletedTask;
        }

        public void Disconnect()
        {

        }

        public void Dispose()
        {

        }
        #endregion

        #region 控制
        public bool Reset() => true;
        public Task<bool> ResetAsync() { return Task.Run(() => Reset()); }
        public bool Motor(bool state)
        {
            //遍历所有轴，设置电机状态
            //获取所有轴的Poer节点，组成一个集合，一次性写入
            Dictionary<string, object> nodesToWrite = new Dictionary<string, object>();
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Command.PowerNode))
                {
                    nodesToWrite[axis.Command.PowerNode] = state;
                }
            }
            if (Plc == null || !Plc.IsConnected) return false;
            return Plc.WriteNodes(nodesToWrite);
        }
        public async Task<bool> MotorAsync(bool state)
        {
            //遍历所有轴，设置电机状态
            //获取所有轴的Poer节点，组成一个集合，一次性写入
            Dictionary<string, object> nodesToWrite = new Dictionary<string, object>();
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Command.PowerNode))
                {
                    nodesToWrite[axis.Command.PowerNode] = state;
                }
            }
            if (Plc == null || !Plc.IsConnected) return false;
            return await Plc.WriteNodesAsync(nodesToWrite);
        }
        public bool Power(bool state) => true;
        public Task<bool> PowerAsync(bool state) => Task.FromResult(true);
        public bool Speed(int value)
        {
            return true;
        }
        public Task<bool> SpeedAsync(int value) => Task.FromResult(true);
        public bool Speedfactor(int value) => true;
        public Task<bool> SpeedfactorAsync(int value) => Task.FromResult(true);
        public bool Speeds(double value)
        {
            //遍历所有轴，设置电机状态
            //获取所有轴的Poer节点，组成一个集合，一次性写入
            Dictionary<string, object> nodesToWrite = new Dictionary<string, object>();
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Parameter.ManuVelocityNode))
                {
                    nodesToWrite[axis.Command.PowerNode] = (float)value;
                }
            }
            if (Plc == null || !Plc.IsConnected) return false;
            return Plc.WriteNodes(nodesToWrite);
        }
        public async Task<bool> SpeedsAsync(double value)
        {
            //遍历所有轴，设置电机状态
            //获取所有轴的Poer节点，组成一个集合，一次性写入
            Dictionary<string, object> nodesToWrite = new Dictionary<string, object>();
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Parameter.ManuVelocityNode))
                {
                    nodesToWrite[axis.Command.PowerNode] = (float)value;
                }
            }
            if (Plc == null || !Plc.IsConnected) return false;
            return await Plc.WriteNodesAsync(nodesToWrite);
        }
        public bool Accel(int value) => true;
        public Task<bool> AccelAsync(int value) => Task.FromResult(true);
        public bool Accels(double value) => true;
        public Task<bool> AccelsAsync(double value) => Task.FromResult(true);

        public RPoint GetRobotPos()
        {
            if (Plc == null || !Plc.IsConnected) return null;
            try
            {
                var nodeIds = Axes.Where(a => !string.IsNullOrWhiteSpace(a.State.ActPositionNode)).Select(a => a.State.ActPositionNode).ToArray();
                var data = Plc.ReadNodes(nodeIds);
                if (data == null || data.Count == 0) return null;

                //获取所有的值
                var values = data.Values.ToArray();

                RPoint point = new RPoint();
                for (int i = 0; i < Axes.Count && i < values.Length; i++)
                {
                    var val = values[i];
                    float v = 0;
                    if (val != null)
                    {
                        try { v = Convert.ToSingle(val); } catch { }
                    }
                    switch (Axes[i].Name)
                    {
                        case "X": point.X = v; break;
                        case "Y": point.Y = v; break;
                        case "Z": point.Z = v; break;
                        case "U": point.U = v; break;
                        default: break;
                    }
                }
                return point;
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("获取机器人位置出错", ex);
                throw;
            }
        }

        public Task<RPoint> GetRobotPosAsync()
        {
            return Task.Run(() => GetRobotPos());
        }

        public bool Go(RPoint position)
        {
            return WaitMoveFinished(position);
        }

        public Task<bool> GoAsync(RPoint position)
        {
            return Task.Run(() => Go(position));
        }

        public bool Move(RPoint position) => Go(position);
        public Task<bool> MoveAsync(RPoint position) => GoAsync(position);
        public bool Jump(RPoint position, double? LimZ) => Go(position);
        public Task<bool> JumpAsync(RPoint position, double? LimZ) => GoAsync(position);
        public bool Jog(string axis, double distance)
        {
            //获取当前机器人坐标
            var currentPos = GetRobotPos();
            if (currentPos == null) return false;
            switch (axis.ToUpper())
            {
                case "X":
                    currentPos.X += (float)distance;
                    break;
                case "Y":
                    currentPos.Y += (float)distance;
                    break;
                case "Z":
                    currentPos.Z += (float)distance;
                    break;
                case "U":
                    currentPos.U += (float)distance;
                    break;
                default:
                    return false;
            }
            return Go(currentPos);

        }
        public Task<bool> JogAsync(string axis, double distance)
        {
            return Task.Run(() => Jog(axis, distance));
        }
        public bool Joint(int joint, double distance)
        {
            return false;
        }
        public Task<bool> JointAsync(int joint, double distance) => Task.FromResult(false);
        public bool SFree()
        {
            return Motor(false);
        }
        public Task<bool> SFreeAsync() => Task.Run(() => SFree());
        public bool SLock() => Motor(true);
        public Task<bool> SLockAsync() => Task.Run(() => SLock());
        public bool CalibMotion(RPoint position, double? LimZ)
        {
            //先Z轴到安全高度
            var currentPos = GetRobotPos();
            if (currentPos == null) return false;
            if (LimZ.HasValue)
            {
                currentPos.Z = (float)LimZ.Value;
                if (!Go(currentPos)) return false;
            }
            //再XYU轴到位
            currentPos.X = position.X;
            currentPos.Y = position.Y;
            currentPos.U = position.U;
            if (!Go(currentPos)) return false;
            //最后Z轴到目标高度
            currentPos.Z = position.Z;
            return Go(currentPos);
        }
        public Task<bool> CalibMotionAsync(RPoint position, double? LimZ) => Task.Run(() => CalibMotion(position, LimZ));
        public bool CalibOutIO(bool state) => false;
        public Task<bool> CalibOutIOAsync(bool state) => Task.FromResult(false);
        public bool CalibParame(PickInfo Pick, int Speed, int Accel, bool Power, int WaitSuction, int WaitBlow)
        {
            return false;

        }

        public async Task<bool> CalibParameAsync(PickInfo Pick, int Speed, int Accel, bool Power, int WaitSuction, int WaitBlow)
        {
            if (Plc == null || !Plc.IsConnected) return false;
            try
            {
                Dictionary<string, object> keyValues = new Dictionary<string, object>();
                foreach (var axis in Axes)
                {
                    if (string.IsNullOrWhiteSpace(axis.Parameter.ManuVelocityNode)) continue;
                    string nodeid = axis.Parameter.ManuVelocityNode;
                    float value = Speed;
                    
                    keyValues.Add(nodeid, value);
                }
                // 2. 写入速度给所有轴
                if (!Plc.WriteNodes(keyValues))
                {
                    return false;
                }
                return true;
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("为XYZU平台设置速度时出错！", ex);
                return false;
            }
        }

        /// <summary>
        /// 阻塞式等待运动完成
        /// </summary>
        /// <param name="position"></param>
        /// <returns></returns>
        private bool WaitMoveFinished(RPoint position)
        {
            if (Plc == null || !Plc.IsConnected) return false;
            try
            {
                CanExecute = false;
                Dictionary<string, object> keyValues = new Dictionary<string, object>();

                // 1. 复位所有轴的开始移动命令
                //StopMove();

                keyValues = new Dictionary<string, object>();
                foreach (var axis in Axes)
                {
                    if (string.IsNullOrWhiteSpace(axis.Parameter.ManuPositionNode)) continue;
                    string nodeid = axis.Parameter.ManuPositionNode;
                    float value = 0;
                    switch (axis.Name)
                    {
                        case "X": value = position.X; break;
                        case "Y": value = position.Y; break;
                        case "Z": value = position.Z; break;
                        case "U": value = position.U; break;
                        default: value = 0; break;
                    }
                    keyValues.Add(nodeid, value);
                }
                // 2. 写入位置给所有轴
                if (!Plc.WriteNodes(keyValues))
                {
                    return false;
                }

                // 3. 写入开始移动命令给所有轴
                if (!StartMove())
                {
                    return false;
                }

                // 4. 等待移动完成
                Stopwatch sw = new Stopwatch();
                sw.Start();
                while (true)
                {
                    //检查各轴是否到位和目标位置一致
                    var (isFinished, isError) = CheckMoveFinished(position);
                    if (isFinished)
                    {
                        StopMove();
                        if (isError)
                        {
                            LogHelper.WriteLogInfo($"机器人执行移动时发生错误，位置：X={position.X},Y={position.Y},Z={position.Z},U={position.U}");
                            return false;
                        }
                        return true;
                    }
                    if (sw.ElapsedMilliseconds > Timeout)
                    {
                        LogHelper.WriteLogInfo($"机器人执行移动超时，位置：X={position.X},Y={position.Y},Z={position.Z},U={position.U}");
                        StopMove();
                        return false;
                    }
                    Thread.Sleep(10);
                }
            }
            catch (Exception ex)
            {
                LogHelper.WriteLogError("执行XYZU平台，阻塞式等待运动完成时出错！", ex);
                CanExecute = true;
                return false;
            }
            finally
            {
                CanExecute = true;
            }
        }

        /// <summary>
        /// 开始运动
        /// </summary>
        /// <returns></returns>
        private bool StartMove()
        {
            Dictionary<string, object> keyValues = new Dictionary<string, object>();

            // 1. 复位所有轴的开始移动命令
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Command.ManuPositionNode))
                {
                    keyValues.Add(axis.Command.ManuPositionNode, true);
                }
            }
            return Plc.WriteNodes(keyValues);
        }

        /// <summary>
        /// 停止运动
        /// </summary>
        /// <returns></returns>
        private bool StopMove()
        {
            Dictionary<string, object> keyValues = new Dictionary<string, object>();

            // 1. 复位所有轴的开始移动命令
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Command.ManuPositionNode))
                {
                    keyValues.Add(axis.Command.ManuPositionNode, false);
                }
                if (!string.IsNullOrWhiteSpace(axis.Command.StopNode))
                {
                    keyValues.Add(axis.Command.StopNode, true);
                }
            }
            bool result = Plc.WriteNodes(keyValues);

            // 异步等待100ms后复位停止命令
            //Task.Run(async () =>
            //{
            //    await Task.Delay(100);
            Thread.Sleep(50);
            Dictionary<string, object> resetValues = new Dictionary<string, object>();
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.Command.StopNode))
                {
                    resetValues.Add(axis.Command.StopNode, false);
                }
            }
            Plc.WriteNodes(resetValues);
            //});
            return result;
        }

        /// <summary>
        /// 检查运动是否结束，返回是否停止、是否有错误
        /// </summary>
        /// <param name="position"></param>
        /// <returns></returns>
        private (bool isFinished, bool isError) CheckMoveFinished(RPoint position)
        {
            //检查各轴是否到位和目标位置一致
            //批量获取所有轴的定位状态和位置、是否错误
            var nodeIds = new List<string>();
            foreach (var axis in Axes)
            {
                if (!string.IsNullOrWhiteSpace(axis.State.PosOKNode))
                {
                    nodeIds.Add(axis.State.PosOKNode);
                }
                if (!string.IsNullOrWhiteSpace(axis.State.ActPositionNode))
                {
                    nodeIds.Add(axis.State.ActPositionNode);
                }
                if (!string.IsNullOrWhiteSpace(axis.State.ErrorNode))
                {
                    nodeIds.Add(axis.State.ErrorNode);
                }
            }
            var res = Plc.ReadNodes(nodeIds.ToArray());
            bool allOk = true;
            bool isAnyError = false;
            foreach (var axis in Axes)
            {
                bool posOk = false;
                float actualPos = 0;
                bool isError = false;
                //获取此轴的定位状态、实际位置、错误状态
                if (res.ContainsKey(axis.State.PosOKNode))
                {
                    posOk = (bool)res[axis.State.PosOKNode];
                }
                if (res.ContainsKey(axis.State.ActPositionNode))
                {
                    try { actualPos = Convert.ToSingle(res[axis.State.ActPositionNode]); } catch { }
                }
                if (res.ContainsKey(axis.State.ErrorNode))
                {
                    isError = (bool)res[axis.State.ErrorNode];
                }
                //检查是否到位和位置一致
                float targetPos = 0;
                switch (axis.Name)
                {
                    case "X": targetPos = position.X; break;
                    case "Y": targetPos = position.Y; break;
                    case "Z": targetPos = position.Z; break;
                    case "U": targetPos = position.U; break;
                    default: targetPos = 0; break;
                }
                //如果定位完成且位置一致，则此轴完成，如果有错误则失败
                if (!(posOk && Math.Abs(actualPos - targetPos) < 0.02))
                {
                    if (isError)
                    {
                        isAnyError = true;
                        break;
                    }
                    allOk = false;
                    break;
                }
            }
            return (allOk, isAnyError);
        }

        #endregion

        #region 收发数据
        public string SendAndReceive(string send) => string.Empty;
        public Task<string> SendAndReceiveAsync(string send) => Task.FromResult(string.Empty);
        public string SendAndReceive(ITcpSessionClient client, string send) => string.Empty;
        public Task<string> SendAndReceiveAsync(ITcpSessionClient client, string send) => Task.FromResult(string.Empty);
        public void Send(string send) { }
        public Task SendAsync(string send) => Task.CompletedTask;
        public void Send(ITcpSessionClient client, string send) { }
        public Task SendAsync(ITcpSessionClient client, string send) => Task.CompletedTask;
        public Encoding GetEncoding() => Encoding.Default;
        #endregion

        #region 属性通知
        public event PropertyChangedEventHandler PropertyChanged;
        protected virtual bool SetProperty<T>(ref T storage, T value, [CallerMemberName] string propertyName = null)
        {
            if (EqualityComparer<T>.Default.Equals(storage, value)) return false;
            storage = value;
            OnPropertyChanged(new PropertyChangedEventArgs(propertyName));
            return true;
        }
        protected void OnPropertyChanged(PropertyChangedEventArgs args) => PropertyChanged?.Invoke(this, args);

        #endregion

        #region IRobot SetTool/SelectTool stubs
        public bool SetTool(int index, double x, double y) => true;
        public Task<bool> SetToolAsync(int index, double x, double y) => Task.FromResult(true);
        public bool SelectTool(int index) => true;
        public Task<bool> SelectToolAsync(int index) => Task.FromResult(true);
        #endregion
    }
}