XYZU_Robot.cs 29 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792
  1. using MahApps.Metro.Controls;
  2. using Opc.Ua;
  3. using Prism;
  4. using Prism.Ioc;
  5. using System;
  6. using System.Collections.Generic;
  7. using System.ComponentModel;
  8. using System.Diagnostics;
  9. using System.Linq;
  10. using System.Runtime.CompilerServices;
  11. using System.Text;
  12. using System.Threading;
  13. using System.Threading.Tasks;
  14. using TeamAAS_VP.Core.PLCs;
  15. using TeamAAS_VP.Enums;
  16. using TeamAAS_VP.Interfaces;
  17. using TeamAAS_VP.Models;
  18. using TouchSocket.Core;
  19. using TouchSocket.Sockets;
  20. using static System.Windows.Forms.AxHost;
  21. namespace TeamAAS_VP.Core.Robots
  22. {
  23. /// <summary>
  24. /// Generic multi-axis robot that composes axis objects and uses PLC nodes configured in RobotInfo.PlcRobotParameter
  25. /// to perform coordinated moves (Go/Move/CalibMotion) for 3 or 4 axes.
  26. /// This implementation maps X/Y/Z/(U) axes by name and writes their position nodes then triggers ExecuteMove node.
  27. /// </summary>
  28. public class XYZU_Robot : IRobot, INotifyPropertyChanged
  29. {
  30. public event Action<Guid, object, ConnectedEventArgs> ConnectedEvent;
  31. public event Action<Guid, object, ClosedEventArgs> DisconnectedEvent;
  32. public event Action<Guid, object, ReceivedDataEventArgs> ReceivedEvent;
  33. public event Action<Guid, object, string> SendEvent;
  34. public XYZU_Robot(RobotInfo robot, OpcUaClientPLC pLC)
  35. {
  36. RobotInfo = robot;
  37. Name = robot.RobotName;
  38. Id = robot.Id;
  39. RobotNo = robot.RobotNo;
  40. RobotIp = robot.IP;
  41. RobotPort = robot.Port;
  42. ConnectType = robot.ConnectType;
  43. Terminator = robot.Terminator;
  44. DataEncoding = robot.DataEncoding;
  45. Brand = RobotBrand.XYZ_Platform;
  46. Plc = pLC;
  47. Plc.ConnectChangedEvent += Plc_ConnectChangedEvent;
  48. // build axis list from known RobotInfo.PlcRobotParameter nodes (best-effort)
  49. Axes = new List<Axis>();
  50. // X
  51. var ax = new Axis();
  52. ax.Name = "X";
  53. ax.Index = 1;
  54. ax.Command = robot.PlcRobotParameter.AxixList[0].Command;
  55. ax.Parameter = robot.PlcRobotParameter.AxixList[0].Parameter;
  56. ax.State = robot.PlcRobotParameter.AxixList[0].State;
  57. Axes.Add(ax);
  58. // Y
  59. var ay = new Axis();
  60. ay.Name = "Y";
  61. ay.Index = 2;
  62. ay.Command = robot.PlcRobotParameter.AxixList[1].Command;
  63. ay.Parameter = robot.PlcRobotParameter.AxixList[1].Parameter;
  64. ay.State = robot.PlcRobotParameter.AxixList[1].State;
  65. Axes.Add(ay);
  66. // Z
  67. var az = new Axis();
  68. az.Name = "Z";
  69. az.Index = 3;
  70. az.Command = robot.PlcRobotParameter.AxixList[2].Command;
  71. az.Parameter = robot.PlcRobotParameter.AxixList[2].Parameter;
  72. az.State = robot.PlcRobotParameter.AxixList[2].State;
  73. Axes.Add(az);
  74. // U optional
  75. if (robot.PlcRobotParameter.AxixList.Count >= 4)
  76. {
  77. var au = new Axis();
  78. au.Name = "U";
  79. au.Index = 4;
  80. au.Command = robot.PlcRobotParameter.AxixList[3].Command;
  81. au.Parameter = robot.PlcRobotParameter.AxixList[3].Parameter;
  82. au.State = robot.PlcRobotParameter.AxixList[3].State;
  83. Axes.Add(au);
  84. Brand = RobotBrand.XYZU_Platform;
  85. }
  86. if (Plc.IsConnected)
  87. {
  88. try
  89. {
  90. //所有轴的位置节点
  91. var nodeIds = Axes.Where(a => !string.IsNullOrWhiteSpace(a.State.ActPositionNode)).Select(a => a.State.ActPositionNode).ToArray();
  92. List<string> positionNodes = new List<string>(nodeIds);
  93. pLC.SubscribeNodes("AxisPosition", positionNodes, (res) =>
  94. {
  95. if (res.key != "AxisPosition") return;
  96. for (int i = 0; i < Axes.Count; i++)
  97. {
  98. var axis = Axes[i];
  99. if (axis.State.ActPositionNode.Contains(res.nodeId))
  100. {
  101. float v = Convert.ToSingle(res.value);
  102. switch (axis.Name)
  103. {
  104. case "X": CurrentPosition.X = v; break;
  105. case "Y": CurrentPosition.Y = v; break;
  106. case "Z": CurrentPosition.Z = v; break;
  107. case "U": CurrentPosition.U = v; break;
  108. }
  109. }
  110. }
  111. });
  112. }
  113. catch (Exception)
  114. {
  115. }
  116. }
  117. }
  118. private void Plc_ConnectChangedEvent(object arg1, bool arg2)
  119. {
  120. CanExecute = arg2;
  121. if (arg2)
  122. ConnectedEvent?.Invoke(Id, this, null);
  123. else
  124. DisconnectedEvent?.Invoke(Id, this, null);
  125. }
  126. #region 属性
  127. public TcpClient TcpClient { get; private set; }
  128. public TcpService TcpService { get; private set; }
  129. public Guid Id { get; set; }
  130. public string Name { get; set; }
  131. /// <summary>
  132. /// 机器人编号
  133. /// </summary>
  134. public int RobotNo { get; set; }
  135. public int RobotPort { get; private set; }
  136. public string RobotIp { get; private set; }
  137. public TCPConnectType ConnectType { get; private set; }
  138. public Terminator Terminator { get; private set; }
  139. public DataEncoding DataEncoding { get; private set; }
  140. public bool IsConnected
  141. {
  142. get
  143. {
  144. if (Plc != null)
  145. {
  146. return Plc.IsConnected;
  147. }
  148. else
  149. {
  150. return false;
  151. }
  152. }
  153. }
  154. public int Timeout { get; set; } = 5000;
  155. private bool _CanExecute = true;
  156. public bool CanExecute
  157. {
  158. get { return _CanExecute; }
  159. set { SetProperty(ref _CanExecute, value); }
  160. }
  161. public int SelectedTool { get; private set; } = 0;
  162. public RobotBrand Brand { get; private set; }
  163. public OpcUaClientPLC Plc { get; private set; }
  164. public RobotInfo RobotInfo { get; private set; }
  165. public List<Axis> Axes { get; private set; }
  166. /// <summary>
  167. /// 进入调试模式
  168. /// </summary>
  169. /// <returns></returns>
  170. public bool EnterDebugMode { get; set; }
  171. private RPoint _CurrentPosition = new RPoint();
  172. /// <summary>
  173. /// 当前位置
  174. /// </summary>
  175. public RPoint CurrentPosition
  176. {
  177. get { return _CurrentPosition; }
  178. set { SetProperty(ref _CurrentPosition, value); }
  179. }
  180. #endregion
  181. #region 连接
  182. public void Connect()
  183. {
  184. if (Plc.IsConnected)
  185. {
  186. ConnectedEvent?.Invoke(Id, this, null);
  187. }
  188. }
  189. public Task ConnectAsync()
  190. {
  191. if (Plc.IsConnected)
  192. {
  193. ConnectedEvent?.Invoke(Id, this, null);
  194. }
  195. return Task.CompletedTask;
  196. }
  197. public void Disconnect()
  198. {
  199. }
  200. public void Dispose()
  201. {
  202. }
  203. #endregion
  204. #region 控制
  205. public bool Reset() => true;
  206. public Task<bool> ResetAsync() { return Task.Run(() => Reset()); }
  207. public bool Motor(bool state)
  208. {
  209. //遍历所有轴,设置电机状态
  210. //获取所有轴的Poer节点,组成一个集合,一次性写入
  211. Dictionary<string, object> nodesToWrite = new Dictionary<string, object>();
  212. foreach (var axis in Axes)
  213. {
  214. if (!string.IsNullOrWhiteSpace(axis.Command.PowerNode))
  215. {
  216. nodesToWrite[axis.Command.PowerNode] = state;
  217. }
  218. }
  219. if (Plc == null || !Plc.IsConnected) return false;
  220. return Plc.WriteNodes(nodesToWrite);
  221. }
  222. public async Task<bool> MotorAsync(bool state)
  223. {
  224. //遍历所有轴,设置电机状态
  225. //获取所有轴的Poer节点,组成一个集合,一次性写入
  226. Dictionary<string, object> nodesToWrite = new Dictionary<string, object>();
  227. foreach (var axis in Axes)
  228. {
  229. if (!string.IsNullOrWhiteSpace(axis.Command.PowerNode))
  230. {
  231. nodesToWrite[axis.Command.PowerNode] = state;
  232. }
  233. }
  234. if (Plc == null || !Plc.IsConnected) return false;
  235. return await Plc.WriteNodesAsync(nodesToWrite);
  236. }
  237. public bool Power(bool state) => true;
  238. public Task<bool> PowerAsync(bool state) => Task.FromResult(true);
  239. public bool Speed(int value)
  240. {
  241. return true;
  242. }
  243. public Task<bool> SpeedAsync(int value) => Task.FromResult(true);
  244. public bool Speedfactor(int value) => true;
  245. public Task<bool> SpeedfactorAsync(int value) => Task.FromResult(true);
  246. public bool Speeds(double value)
  247. {
  248. //遍历所有轴,设置电机状态
  249. //获取所有轴的Poer节点,组成一个集合,一次性写入
  250. Dictionary<string, object> nodesToWrite = new Dictionary<string, object>();
  251. foreach (var axis in Axes)
  252. {
  253. if (!string.IsNullOrWhiteSpace(axis.Parameter.ManuVelocityNode))
  254. {
  255. nodesToWrite[axis.Command.PowerNode] = (float)value;
  256. }
  257. }
  258. if (Plc == null || !Plc.IsConnected) return false;
  259. return Plc.WriteNodes(nodesToWrite);
  260. }
  261. public async Task<bool> SpeedsAsync(double value)
  262. {
  263. //遍历所有轴,设置电机状态
  264. //获取所有轴的Poer节点,组成一个集合,一次性写入
  265. Dictionary<string, object> nodesToWrite = new Dictionary<string, object>();
  266. foreach (var axis in Axes)
  267. {
  268. if (!string.IsNullOrWhiteSpace(axis.Parameter.ManuVelocityNode))
  269. {
  270. nodesToWrite[axis.Command.PowerNode] = (float)value;
  271. }
  272. }
  273. if (Plc == null || !Plc.IsConnected) return false;
  274. return await Plc.WriteNodesAsync(nodesToWrite);
  275. }
  276. public bool Accel(int value) => true;
  277. public Task<bool> AccelAsync(int value) => Task.FromResult(true);
  278. public bool Accels(double value) => true;
  279. public Task<bool> AccelsAsync(double value) => Task.FromResult(true);
  280. public RPoint GetRobotPos()
  281. {
  282. if (Plc == null || !Plc.IsConnected) return null;
  283. try
  284. {
  285. var nodeIds = Axes.Where(a => !string.IsNullOrWhiteSpace(a.State.ActPositionNode)).Select(a => a.State.ActPositionNode).ToArray();
  286. var data = Plc.ReadNodes(nodeIds);
  287. if (data == null || data.Count == 0) return null;
  288. //获取所有的值
  289. var values = data.Values.ToArray();
  290. RPoint point = new RPoint();
  291. for (int i = 0; i < Axes.Count && i < values.Length; i++)
  292. {
  293. var val = values[i];
  294. float v = 0;
  295. if (val != null)
  296. {
  297. try { v = Convert.ToSingle(val); } catch { }
  298. }
  299. switch (Axes[i].Name)
  300. {
  301. case "X": point.X = v; break;
  302. case "Y": point.Y = v; break;
  303. case "Z": point.Z = v; break;
  304. case "U": point.U = v; break;
  305. default: break;
  306. }
  307. }
  308. return point;
  309. }
  310. catch (Exception ex)
  311. {
  312. LogHelper.WriteLogError("获取机器人位置出错", ex);
  313. throw;
  314. }
  315. }
  316. public Task<RPoint> GetRobotPosAsync()
  317. {
  318. return Task.Run(() => GetRobotPos());
  319. }
  320. public bool Go(RPoint position)
  321. {
  322. return WaitMoveFinished(position);
  323. }
  324. public Task<bool> GoAsync(RPoint position)
  325. {
  326. return Task.Run(() => Go(position));
  327. }
  328. public bool Move(RPoint position) => Go(position);
  329. public Task<bool> MoveAsync(RPoint position) => GoAsync(position);
  330. public bool Jump(RPoint position, double? LimZ) => Go(position);
  331. public Task<bool> JumpAsync(RPoint position, double? LimZ) => GoAsync(position);
  332. public bool Jog(string axis, double distance)
  333. {
  334. //获取当前机器人坐标
  335. var currentPos = GetRobotPos();
  336. if (currentPos == null) return false;
  337. switch (axis.ToUpper())
  338. {
  339. case "X":
  340. currentPos.X += (float)distance;
  341. break;
  342. case "Y":
  343. currentPos.Y += (float)distance;
  344. break;
  345. case "Z":
  346. currentPos.Z += (float)distance;
  347. break;
  348. case "U":
  349. currentPos.U += (float)distance;
  350. break;
  351. default:
  352. return false;
  353. }
  354. return Go(currentPos);
  355. }
  356. public Task<bool> JogAsync(string axis, double distance)
  357. {
  358. return Task.Run(() => Jog(axis, distance));
  359. }
  360. public bool Joint(int joint, double distance)
  361. {
  362. return false;
  363. }
  364. public Task<bool> JointAsync(int joint, double distance) => Task.FromResult(false);
  365. public bool SFree()
  366. {
  367. return Motor(false);
  368. }
  369. public Task<bool> SFreeAsync() => Task.Run(() => SFree());
  370. public bool SLock() => Motor(true);
  371. public Task<bool> SLockAsync() => Task.Run(() => SLock());
  372. public bool CalibMotion(RPoint position, double? LimZ)
  373. {
  374. //先Z轴到安全高度
  375. var currentPos = GetRobotPos();
  376. if (currentPos == null) return false;
  377. if (LimZ.HasValue)
  378. {
  379. currentPos.Z = (float)LimZ.Value;
  380. if (!Go(currentPos)) return false;
  381. }
  382. //再XYU轴到位
  383. currentPos.X = position.X;
  384. currentPos.Y = position.Y;
  385. currentPos.U = position.U;
  386. if (!Go(currentPos)) return false;
  387. //最后Z轴到目标高度
  388. currentPos.Z = position.Z;
  389. return Go(currentPos);
  390. }
  391. public Task<bool> CalibMotionAsync(RPoint position, double? LimZ) => Task.Run(() => CalibMotion(position, LimZ));
  392. public bool CalibOutIO(bool state) => false;
  393. public Task<bool> CalibOutIOAsync(bool state) => Task.FromResult(false);
  394. public bool CalibParame(PickInfo Pick, int Speed, int Accel, bool Power, int WaitSuction, int WaitBlow)
  395. {
  396. return false;
  397. }
  398. public async Task<bool> CalibParameAsync(PickInfo Pick, int Speed, int Accel, bool Power, int WaitSuction, int WaitBlow)
  399. {
  400. if (Plc == null || !Plc.IsConnected) return false;
  401. try
  402. {
  403. Dictionary<string, object> keyValues = new Dictionary<string, object>();
  404. foreach (var axis in Axes)
  405. {
  406. if (string.IsNullOrWhiteSpace(axis.Parameter.ManuVelocityNode)) continue;
  407. string nodeid = axis.Parameter.ManuVelocityNode;
  408. float value = Speed;
  409. keyValues.Add(nodeid, value);
  410. }
  411. // 2. 写入速度给所有轴
  412. if (!Plc.WriteNodes(keyValues))
  413. {
  414. return false;
  415. }
  416. return true;
  417. }
  418. catch (Exception ex)
  419. {
  420. LogHelper.WriteLogError("为XYZU平台设置速度时出错!", ex);
  421. return false;
  422. }
  423. }
  424. /// <summary>
  425. /// 阻塞式等待运动完成
  426. /// </summary>
  427. /// <param name="position"></param>
  428. /// <returns></returns>
  429. private bool WaitMoveFinished(RPoint position)
  430. {
  431. if (Plc == null || !Plc.IsConnected) return false;
  432. try
  433. {
  434. CanExecute = false;
  435. Dictionary<string, object> keyValues = new Dictionary<string, object>();
  436. // 1. 写入目标位置到各轴的 ManuPositionNode
  437. keyValues = new Dictionary<string, object>();
  438. foreach (var axis in Axes)
  439. {
  440. if (string.IsNullOrWhiteSpace(axis.Parameter.ManuPositionNode)) continue;
  441. string nodeid = axis.Parameter.ManuPositionNode;
  442. float value = 0;
  443. switch (axis.Name)
  444. {
  445. case "X": value = position.X; break;
  446. case "Y": value = position.Y; break;
  447. case "Z": value = position.Z; break;
  448. case "U": value = position.U; break;
  449. default: value = 0; break;
  450. }
  451. keyValues.Add(nodeid, value);
  452. }
  453. // 2. 写入位置给所有轴
  454. if (!Plc.WriteNodes(keyValues))
  455. {
  456. return false;
  457. }
  458. // 3. 写入开始移动命令给所有轴
  459. if (!StartMove())
  460. {
  461. return false;
  462. }
  463. // 准备停滞检测:收集所有实际位置节点
  464. var actNodes = Axes.Where(a => !string.IsNullOrWhiteSpace(a.State.ActPositionNode))
  465. .Select(a => a.State.ActPositionNode)
  466. .Distinct()
  467. .ToArray();
  468. // lastPos 存储上一次读取到的位置,lastChange 存储上次发生“实质性”变化的时间
  469. Dictionary<string, float> lastPos = new Dictionary<string, float>();
  470. Dictionary<string, DateTime> lastChange = new Dictionary<string, DateTime>();
  471. foreach (var node in actNodes)
  472. {
  473. lastPos[node] = float.NaN;
  474. lastChange[node] = DateTime.UtcNow;
  475. }
  476. const float movementThreshold = 0.01f; // 判断位置变化的阈值,避免噪声
  477. Stopwatch sw = new Stopwatch();
  478. sw.Start();
  479. while (true)
  480. {
  481. // 先读取各轴的实际位置,用于停滞检测
  482. Dictionary<string, object> posRead = new Dictionary<string, object>();
  483. try
  484. {
  485. if (actNodes.Length > 0)
  486. {
  487. posRead = Plc.ReadNodes(actNodes);
  488. }
  489. }
  490. catch
  491. {
  492. // 如果读取失败,继续让 CheckMoveFinished 来处理状态或超时
  493. }
  494. var now = DateTime.UtcNow;
  495. // 更新每个节点的变化时间:只要任意一个轴在最近 Timeout 时间内有变化,就视为系统仍在运动
  496. foreach (var node in actNodes)
  497. {
  498. float actual = 0;
  499. if (posRead != null && posRead.ContainsKey(node))
  500. {
  501. var raw = posRead[node];
  502. if (raw != null)
  503. {
  504. try { actual = Convert.ToSingle(raw); } catch { /* 保持 actual = 0 */ }
  505. }
  506. }
  507. if (float.IsNaN(lastPos[node]))
  508. {
  509. lastPos[node] = actual;
  510. lastChange[node] = now;
  511. }
  512. else
  513. {
  514. if (Math.Abs(actual - lastPos[node]) > movementThreshold)
  515. {
  516. // 有实质性变化,更新记录时间和值
  517. lastPos[node] = actual;
  518. lastChange[node] = now;
  519. }
  520. // 否则保持 lastChange 不变(表示该轴最近一次变化的时间)
  521. }
  522. }
  523. // 判断是否至少有一个轴在最近 Timeout 时间内发生过变化(即认为还在运动)
  524. bool anyAxisMovedRecently = actNodes.Any(node => (now - lastChange[node]).TotalMilliseconds <= Timeout);
  525. // 如果没有任何轴在最近 Timeout 时间内发生变化,则认为出现停滞异常
  526. if (!anyAxisMovedRecently && actNodes.Length > 0)
  527. {
  528. StopMove();
  529. LogHelper.WriteLogInfo($"轴停滞超时(整体判定),目标位置:X={position.X},Y={position.Y},Z={position.Z},U={position.U},超时阈值(ms)={Timeout}");
  530. return false;
  531. }
  532. // 检查是否整体到位或有错误(保留原有逻辑)
  533. var (isFinished, isError) = CheckMoveFinished(position);
  534. if (isFinished)
  535. {
  536. StopMove();
  537. if (isError)
  538. {
  539. LogHelper.WriteLogInfo($"机器人执行移动时发生错误,位置:X={position.X},Y={position.Y},Z={position.Z},U={position.U}");
  540. return false;
  541. }
  542. return true;
  543. }
  544. // 检查整体超时(防止一直有抖动导致 anyAxisMovedRecently 一直为 true)
  545. if (sw.ElapsedMilliseconds > 60000) // 保守超时,一分钟,可根据需求调整
  546. {
  547. LogHelper.WriteLogInfo($"机器人执行移动超时,位置:X={position.X},Y={position.Y},Z={position.Z},U={position.U}");
  548. StopMove();
  549. return false;
  550. }
  551. Thread.Sleep(10);
  552. }
  553. }
  554. catch (Exception ex)
  555. {
  556. LogHelper.WriteLogError("执行XYZU平台,阻塞式等待运动完成时出错!", ex);
  557. CanExecute = true;
  558. return false;
  559. }
  560. finally
  561. {
  562. CanExecute = true;
  563. }
  564. }
  565. /// <summary>
  566. /// 开始运动
  567. /// </summary>
  568. /// <returns></returns>
  569. private bool StartMove()
  570. {
  571. Dictionary<string, object> keyValues = new Dictionary<string, object>();
  572. // 1. 复位所有轴的开始移动命令
  573. foreach (var axis in Axes)
  574. {
  575. if (!string.IsNullOrWhiteSpace(axis.Command.ManuPositionNode))
  576. {
  577. keyValues.Add(axis.Command.ManuPositionNode, true);
  578. }
  579. }
  580. return Plc.WriteNodes(keyValues);
  581. }
  582. /// <summary>
  583. /// 停止运动
  584. /// </summary>
  585. /// <returns></returns>
  586. private bool StopMove()
  587. {
  588. Dictionary<string, object> keyValues = new Dictionary<string, object>();
  589. // 1. 复位所有轴的开始移动命令
  590. foreach (var axis in Axes)
  591. {
  592. if (!string.IsNullOrWhiteSpace(axis.Command.ManuPositionNode))
  593. {
  594. keyValues.Add(axis.Command.ManuPositionNode, false);
  595. }
  596. if (!string.IsNullOrWhiteSpace(axis.Command.StopNode))
  597. {
  598. keyValues.Add(axis.Command.StopNode, true);
  599. }
  600. }
  601. bool result = Plc.WriteNodes(keyValues);
  602. // 异步等待100ms后复位停止命令
  603. //Task.Run(async () =>
  604. //{
  605. // await Task.Delay(100);
  606. Thread.Sleep(50);
  607. Dictionary<string, object> resetValues = new Dictionary<string, object>();
  608. foreach (var axis in Axes)
  609. {
  610. if (!string.IsNullOrWhiteSpace(axis.Command.StopNode))
  611. {
  612. resetValues.Add(axis.Command.StopNode, false);
  613. }
  614. }
  615. Plc.WriteNodes(resetValues);
  616. //});
  617. return result;
  618. }
  619. /// <summary>
  620. /// 检查运动是否结束,返回是否停止、是否有错误
  621. /// </summary>
  622. /// <param name="position"></param>
  623. /// <returns></returns>
  624. private (bool isFinished, bool isError) CheckMoveFinished(RPoint position)
  625. {
  626. //检查各轴是否到位和目标位置一致
  627. //批量获取所有轴的定位状态和位置、是否错误
  628. var nodeIds = new List<string>();
  629. foreach (var axis in Axes)
  630. {
  631. if (!string.IsNullOrWhiteSpace(axis.State.PosOKNode))
  632. {
  633. nodeIds.Add(axis.State.PosOKNode);
  634. }
  635. if (!string.IsNullOrWhiteSpace(axis.State.ActPositionNode))
  636. {
  637. nodeIds.Add(axis.State.ActPositionNode);
  638. }
  639. if (!string.IsNullOrWhiteSpace(axis.State.ErrorNode))
  640. {
  641. nodeIds.Add(axis.State.ErrorNode);
  642. }
  643. }
  644. var res = Plc.ReadNodes(nodeIds.ToArray());
  645. bool allOk = true;
  646. bool isAnyError = false;
  647. foreach (var axis in Axes)
  648. {
  649. bool posOk = false;
  650. float actualPos = 0;
  651. bool isError = false;
  652. //获取此轴的定位状态、实际位置、错误状态
  653. if (res.ContainsKey(axis.State.PosOKNode))
  654. {
  655. posOk = (bool)res[axis.State.PosOKNode];
  656. }
  657. if (res.ContainsKey(axis.State.ActPositionNode))
  658. {
  659. try { actualPos = Convert.ToSingle(res[axis.State.ActPositionNode]); } catch { }
  660. }
  661. if (res.ContainsKey(axis.State.ErrorNode))
  662. {
  663. isError = (bool)res[axis.State.ErrorNode];
  664. }
  665. //检查是否到位和位置一致
  666. float targetPos = 0;
  667. switch (axis.Name)
  668. {
  669. case "X": targetPos = position.X; break;
  670. case "Y": targetPos = position.Y; break;
  671. case "Z": targetPos = position.Z; break;
  672. case "U": targetPos = position.U; break;
  673. default: targetPos = 0; break;
  674. }
  675. //如果定位完成且位置一致,则此轴完成,如果有错误则失败
  676. if (!(posOk && Math.Abs(actualPos - targetPos) < 0.02))
  677. {
  678. if (isError)
  679. {
  680. isAnyError = true;
  681. break;
  682. }
  683. allOk = false;
  684. break;
  685. }
  686. }
  687. return (allOk, isAnyError);
  688. }
  689. #endregion
  690. #region 收发数据
  691. public string SendAndReceive(string send) => string.Empty;
  692. public Task<string> SendAndReceiveAsync(string send) => Task.FromResult(string.Empty);
  693. public string SendAndReceive(ITcpSessionClient client, string send) => string.Empty;
  694. public Task<string> SendAndReceiveAsync(ITcpSessionClient client, string send) => Task.FromResult(string.Empty);
  695. public void Send(string send) { }
  696. public Task SendAsync(string send) => Task.CompletedTask;
  697. public void Send(ITcpSessionClient client, string send) { }
  698. public Task SendAsync(ITcpSessionClient client, string send) => Task.CompletedTask;
  699. public Encoding GetEncoding() => Encoding.Default;
  700. #endregion
  701. #region 属性通知
  702. public event PropertyChangedEventHandler PropertyChanged;
  703. protected virtual bool SetProperty<T>(ref T storage, T value, [CallerMemberName] string propertyName = null)
  704. {
  705. if (EqualityComparer<T>.Default.Equals(storage, value)) return false;
  706. storage = value;
  707. OnPropertyChanged(new PropertyChangedEventArgs(propertyName));
  708. return true;
  709. }
  710. protected void OnPropertyChanged(PropertyChangedEventArgs args) => PropertyChanged?.Invoke(this, args);
  711. #endregion
  712. #region IRobot SetTool/SelectTool stubs
  713. public bool SetTool(int index, double x, double y) => true;
  714. public Task<bool> SetToolAsync(int index, double x, double y) => Task.FromResult(true);
  715. public bool SelectTool(int index) => true;
  716. public Task<bool> SelectToolAsync(int index) => Task.FromResult(true);
  717. #endregion
  718. }
  719. }