XYZU_Robot.cs 30 KB

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