|
|
@@ -0,0 +1,881 @@
|
|
|
+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 TeamAAS_VP.Resources.Languages;
|
|
|
+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 XYZURWV_Platform : 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 XYZURWV_Platform(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;
|
|
|
+ SafeZ = robot.SafeZ;
|
|
|
+ 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);
|
|
|
+
|
|
|
+ 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);
|
|
|
+
|
|
|
+ var ar = new Axis();
|
|
|
+ ar.Name = "R";
|
|
|
+ ar.Index = 5;
|
|
|
+ ar.Command = robot.PlcRobotParameter.AxixList[4].Command;
|
|
|
+ ar.Parameter = robot.PlcRobotParameter.AxixList[4].Parameter;
|
|
|
+ ar.State = robot.PlcRobotParameter.AxixList[4].State;
|
|
|
+ Axes.Add(ar);
|
|
|
+
|
|
|
+ var aw = new Axis();
|
|
|
+ aw.Name = "W";
|
|
|
+ aw.Index = 6;
|
|
|
+ aw.Command = robot.PlcRobotParameter.AxixList[5].Command;
|
|
|
+ aw.Parameter = robot.PlcRobotParameter.AxixList[5].Parameter;
|
|
|
+ aw.State = robot.PlcRobotParameter.AxixList[5].State;
|
|
|
+ Axes.Add(aw);
|
|
|
+
|
|
|
+ var av = new Axis();
|
|
|
+ av.Name = "V";
|
|
|
+ av.Index = 7;
|
|
|
+ av.Command = robot.PlcRobotParameter.AxixList[6].Command;
|
|
|
+ av.Parameter = robot.PlcRobotParameter.AxixList[6].Parameter;
|
|
|
+ av.State = robot.PlcRobotParameter.AxixList[6].State;
|
|
|
+ Axes.Add(av);
|
|
|
+ Brand = RobotBrand.XYZURWV_Platform;
|
|
|
+
|
|
|
+
|
|
|
+ if (Plc.IsConnected)
|
|
|
+ {
|
|
|
+ try
|
|
|
+ {
|
|
|
+ //所有轴的位置节点
|
|
|
+ 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;
|
|
|
+ case "R": CurrentPosition.R = v; break;
|
|
|
+ case "W": CurrentPosition.W = v; break;
|
|
|
+ case "V": CurrentPosition.V = v; break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ });
|
|
|
+ }
|
|
|
+ catch (Exception)
|
|
|
+ {
|
|
|
+
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ 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; } = 5000;
|
|
|
+
|
|
|
+ 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); }
|
|
|
+ }
|
|
|
+
|
|
|
+ private double _SafeZ;
|
|
|
+ /// <summary>
|
|
|
+ /// Z轴的安全高度
|
|
|
+ /// </summary>
|
|
|
+ public double SafeZ
|
|
|
+ {
|
|
|
+ get { return _SafeZ; }
|
|
|
+ set { SetProperty(ref _SafeZ, value); }
|
|
|
+ }
|
|
|
+ #endregion
|
|
|
+
|
|
|
+ #region 连接
|
|
|
+ public void Connect()
|
|
|
+ {
|
|
|
+ if (Plc.IsConnected)
|
|
|
+ {
|
|
|
+ StopMove();
|
|
|
+ ConnectedEvent?.Invoke(Id, this, null);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ public Task ConnectAsync()
|
|
|
+ {
|
|
|
+ if (Plc.IsConnected)
|
|
|
+ {
|
|
|
+ StopMove();
|
|
|
+ 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;
|
|
|
+ case "V": point.V = v; break;
|
|
|
+ case "W": point.W = v; break;
|
|
|
+ case "R": point.R = v; break;
|
|
|
+ default: break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return point;
|
|
|
+ }
|
|
|
+ catch (Exception ex)
|
|
|
+ {
|
|
|
+ LogHelper.WriteLogError(Lang.获取机器人位置出错, 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)
|
|
|
+ {
|
|
|
+ return CalibMotion(position, LimZ);
|
|
|
+ }
|
|
|
+ public Task<bool> JumpAsync(RPoint position, double? LimZ) => CalibMotionAsync(position, LimZ);
|
|
|
+ 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;
|
|
|
+ case "R":
|
|
|
+ currentPos.R += (float)distance;
|
|
|
+ break;
|
|
|
+ case "W":
|
|
|
+ currentPos.W += (float)distance;
|
|
|
+ break;
|
|
|
+ case "V":
|
|
|
+ currentPos.V += (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;
|
|
|
+
|
|
|
+ currentPos.U = position.U;
|
|
|
+ currentPos.R = position.R;
|
|
|
+ if (!Go(currentPos)) return false;
|
|
|
+
|
|
|
+ if (LimZ.HasValue)
|
|
|
+ {
|
|
|
+ currentPos.Z = (float)LimZ.Value;
|
|
|
+ if (!Go(currentPos)) return false;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ currentPos.Z = (float)SafeZ;
|
|
|
+ if (!Go(currentPos)) return false;
|
|
|
+ }
|
|
|
+ //再XYU轴到位
|
|
|
+ currentPos.X = position.X;
|
|
|
+ currentPos.Y = position.Y;
|
|
|
+ if (!Go(currentPos)) return false;
|
|
|
+ //最后Z轴到目标高度
|
|
|
+ currentPos.Z = position.Z;
|
|
|
+ var isSucceed = Go(currentPos);
|
|
|
+ Thread.Sleep(150);
|
|
|
+ return isSucceed;
|
|
|
+ }
|
|
|
+ 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(Lang.为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;
|
|
|
+ Motor(true);
|
|
|
+ Dictionary<string, object> keyValues = new Dictionary<string, object>();
|
|
|
+
|
|
|
+ // 1. 写入目标位置到各轴的 ManuPositionNode
|
|
|
+ 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;
|
|
|
+ case "R": value = position.R; break;
|
|
|
+ case "W": value = position.W; break;
|
|
|
+ case "V": value = position.V; break;
|
|
|
+ default: value = 0; break;
|
|
|
+ }
|
|
|
+ keyValues.Add(nodeid, value);
|
|
|
+ }
|
|
|
+ // 2. 写入位置给所有轴
|
|
|
+ if (!Plc.WriteNodes(keyValues))
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 3. 写入开始移动命令给所有轴
|
|
|
+ if (!StartMove())
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 准备停滞检测:收集所有实际位置节点
|
|
|
+ var actNodes = Axes.Where(a => !string.IsNullOrWhiteSpace(a.State.ActPositionNode))
|
|
|
+ .Select(a => a.State.ActPositionNode)
|
|
|
+ .Distinct()
|
|
|
+ .ToArray();
|
|
|
+
|
|
|
+ // lastPos 存储上一次读取到的位置,lastChange 存储上次发生“实质性”变化的时间
|
|
|
+ Dictionary<string, float> lastPos = new Dictionary<string, float>();
|
|
|
+ Dictionary<string, DateTime> lastChange = new Dictionary<string, DateTime>();
|
|
|
+ foreach (var node in actNodes)
|
|
|
+ {
|
|
|
+ lastPos[node] = float.NaN;
|
|
|
+ lastChange[node] = DateTime.Now;
|
|
|
+ }
|
|
|
+
|
|
|
+ const float movementThreshold = 0.01f; // 判断位置变化的阈值,避免噪声
|
|
|
+ Stopwatch sw = new Stopwatch();
|
|
|
+ sw.Start();
|
|
|
+ while (true)
|
|
|
+ {
|
|
|
+ // 先读取各轴的实际位置,用于停滞检测
|
|
|
+ Dictionary<string, object> posRead = new Dictionary<string, object>();
|
|
|
+ try
|
|
|
+ {
|
|
|
+ if (actNodes.Length > 0)
|
|
|
+ {
|
|
|
+ posRead = Plc.ReadNodes(actNodes);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ catch
|
|
|
+ {
|
|
|
+ // 如果读取失败,继续让 CheckMoveFinished 来处理状态或超时
|
|
|
+ }
|
|
|
+
|
|
|
+ var now = DateTime.Now;
|
|
|
+
|
|
|
+ // 更新每个节点的变化时间:只要任意一个轴在最近 Timeout 时间内有变化,就视为系统仍在运动
|
|
|
+ foreach (var node in actNodes)
|
|
|
+ {
|
|
|
+ float actual = 0;
|
|
|
+ if (posRead != null && posRead.ContainsKey(node))
|
|
|
+ {
|
|
|
+ var raw = posRead[node];
|
|
|
+ if (raw != null)
|
|
|
+ {
|
|
|
+ try { actual = Convert.ToSingle(raw); } catch { /* 保持 actual = 0 */ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if (float.IsNaN(lastPos[node]))
|
|
|
+ {
|
|
|
+ lastPos[node] = actual;
|
|
|
+ lastChange[node] = now;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if (Math.Abs(actual - lastPos[node]) > movementThreshold)
|
|
|
+ {
|
|
|
+ // 有实质性变化,更新记录时间和值
|
|
|
+ lastPos[node] = actual;
|
|
|
+ lastChange[node] = now;
|
|
|
+ }
|
|
|
+ // 否则保持 lastChange 不变(表示该轴最近一次变化的时间)
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ // 判断是否至少有一个轴在最近 Timeout 时间内发生过变化(即认为还在运动)
|
|
|
+ bool anyAxisMovedRecently = actNodes.Any(node => (now - lastChange[node]).TotalMilliseconds <= 500);
|
|
|
+
|
|
|
+ // 如果没有任何轴在最近 Timeout 时间内发生变化,则认为出现停滞异常
|
|
|
+ if (!anyAxisMovedRecently && actNodes.Length > 0)
|
|
|
+ {
|
|
|
+ StopMove();
|
|
|
+ LogHelper.WriteLogInfo(string.Format(Lang.轴停滞超时整体判定目标位置X0Y1Z2U3超时阈值ms4, position.X, position.Y, position.Z, position.U, Timeout));
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 检查是否整体到位或有错误(保留原有逻辑)
|
|
|
+ var (isFinished, isError) = CheckMoveFinished(position);
|
|
|
+ if (isFinished)
|
|
|
+ {
|
|
|
+ StopMove();
|
|
|
+ if (isError)
|
|
|
+ {
|
|
|
+ LogHelper.WriteLogInfo(string.Format(Lang.机器人执行移动时发生错误位置X0Y1Z2U3, position.X, position.Y, position.Z, position.U));
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ if (isError)
|
|
|
+ {
|
|
|
+ StopMove();
|
|
|
+ LogHelper.WriteLogInfo($"机器人执行移动时发生错误,位置:X={position.X},Y={position.Y},Z={position.Z},U={position.U}");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 检查整体超时(防止一直有抖动导致 anyAxisMovedRecently 一直为 true)
|
|
|
+ if (sw.ElapsedMilliseconds > 60000) // 保守超时,一分钟,可根据需求调整
|
|
|
+ {
|
|
|
+ LogHelper.WriteLogInfo(string.Format(Lang.机器人执行移动超时位置X0Y1Z2U3, position.X, position.Y, position.Z, position.U));
|
|
|
+ StopMove();
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ Thread.Sleep(10);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ catch (Exception ex)
|
|
|
+ {
|
|
|
+ LogHelper.WriteLogError(Lang.执行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);
|
|
|
+ }
|
|
|
+ if (!string.IsNullOrWhiteSpace(axis.State.PausedNode))
|
|
|
+ {
|
|
|
+ nodeIds.Add(axis.State.PausedNode);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ 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];
|
|
|
+ }
|
|
|
+ if (res.ContainsKey(axis.State.PausedNode))
|
|
|
+ {
|
|
|
+ isError = (bool)res[axis.State.PausedNode];
|
|
|
+ }
|
|
|
+ //if (isError)
|
|
|
+ //{
|
|
|
+ // isAnyError = true;
|
|
|
+ // break;
|
|
|
+ //}
|
|
|
+ //检查是否到位和位置一致
|
|
|
+ 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;
|
|
|
+ case "R": targetPos = position.R; break;
|
|
|
+ case "W": targetPos = position.W; break;
|
|
|
+ case "V": targetPos = position.V; break;
|
|
|
+ default: targetPos = 0; break;
|
|
|
+ }
|
|
|
+ //如果定位完成且位置一致,则此轴完成,如果有错误则失败
|
|
|
+ if (!(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
|
|
|
+ }
|
|
|
+}
|