BoatZAxisManualMove.cs 8.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230
  1. using Aitex.Core.RT.Device;
  2. using Aitex.Core.RT.Device.Unit;
  3. using Aitex.Core.RT.Event;
  4. using Aitex.Core.RT.Routine;
  5. using Aitex.Core.RT.SCCore;
  6. using Aitex.Core.Util;
  7. using Aitex.Sorter.Common;
  8. using MECF.Framework.Common.Alarms;
  9. using MECF.Framework.Common.Device.Bases;
  10. using MECF.Framework.Common.Equipment;
  11. using MECF.Framework.Common.SubstrateTrackings;
  12. using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robot;
  13. using System;
  14. using System.Collections.Generic;
  15. using System.Linq;
  16. using System.Text;
  17. using System.Threading.Tasks;
  18. using FurnaceRT.Equipments.Systems;
  19. using static Aitex.Core.RT.Device.Unit.IoBoat;
  20. using FurnaceRT.Devices;
  21. namespace FurnaceRT.Equipments.Boats
  22. {
  23. public class BoatZAxisManualMove : ModuleRoutine, IRoutine
  24. {
  25. enum RoutineStep
  26. {
  27. AutoShutterOpen,
  28. AutoShutterClose,
  29. AutoShutterUp,
  30. AutoShutterDown,
  31. SetBoatSpeed,
  32. BoatZAxisMove,
  33. CheckPrepareMove,
  34. }
  35. private BoatPosition _targetPosition;
  36. private float _speed;
  37. private BoatModule _boatModule;
  38. private int _timeout = 0;
  39. private int _shutterTimeout = 0;
  40. private IoSensor _sensorBEHumanInterlock;
  41. private IoSensor _sensorR5ArmNotExtendToBE;
  42. private IoSensor _sensorASOpenStatus;
  43. private IoSensor _sensorASTiltStatus;
  44. public BoatZAxisManualMove(BoatModule boatModule)
  45. {
  46. _boatModule = boatModule;
  47. Module = boatModule.Module;
  48. Name = "ZAxis Manual Move";
  49. }
  50. public void Init(string targetPosition, string speed)
  51. {
  52. _targetPosition = (BoatPosition)Enum.Parse(typeof(BoatPosition), targetPosition);
  53. float.TryParse(speed, out _speed);
  54. var para = new List<object> { _targetPosition, _speed };
  55. _boatModule.BoatZAxisMoveFailedForInterlock.RetryMessage = (int)BoatModule.MSG.ZAxisManualMoveRetry;
  56. _boatModule.BoatZAxisMoveFailedForInterlock.RetryMessageParas = para.ToArray();
  57. _boatModule.BoatZAxisMoveTimeOut.RetryMessage = (int)BoatModule.MSG.ZAxisManualMoveRetry;
  58. _boatModule.BoatZAxisMoveTimeOut.RetryMessageParas = para.ToArray();
  59. }
  60. public Result Start(params object[] objs)
  61. {
  62. Reset();
  63. string reason = string.Empty;
  64. if (_targetPosition == BoatPosition.None)
  65. return Result.DONE;
  66. _timeout = SC.GetValue<int>($"{Module}.MotionTimeout");
  67. _shutterTimeout = SC.GetValue<int>($"{Module}.AutoShutter.MotionTimeout");
  68. if (!_boatModule.CheckPrepareMove(out reason, false))
  69. {
  70. _boatModule.BoatZAxisMoveFailedForInterlock.Set(reason);
  71. return Result.FAIL;
  72. }
  73. Notify($"Start");
  74. return Result.RUN;
  75. }
  76. public void Abort()
  77. {
  78. _boatModule.BoatZAxisStop();
  79. }
  80. public override Result Monitor()
  81. {
  82. try
  83. {
  84. PauseRountine(_boatModule.RAxisDevice.IsPause);
  85. if (_boatModule.RAxisDevice.IsPause)
  86. return Result.RUN;
  87. if (_boatModule.ZAxisDevice.IsError)
  88. return Result.FAIL;
  89. AutoShutterOpen((int)RoutineStep.AutoShutterOpen, true, _shutterTimeout);
  90. CheckPrepareMove((int)RoutineStep.CheckPrepareMove, 2);
  91. SetBoatZAxisMove((int)RoutineStep.BoatZAxisMove, _targetPosition, _speed, _timeout);
  92. }
  93. catch (RoutineBreakException)
  94. {
  95. return Result.RUN;
  96. }
  97. catch (RoutineFaildException ex)
  98. {
  99. return Result.FAIL;
  100. }
  101. Notify("Finished");
  102. return Result.DONE;
  103. }
  104. private void AutoShutterOpen(int id, bool isOpen, int timeout)
  105. {
  106. Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
  107. {
  108. Notify($"shutter {(isOpen ? "open" : "close")}");
  109. string reason;
  110. if (!_boatModule.ShutterDevice.SetOpen(isOpen, out reason))
  111. {
  112. _boatModule.ShutterDevice.AutoShutterMoveFailedForInterlock.Set();
  113. return false;
  114. }
  115. return true;
  116. }, () =>
  117. {
  118. return isOpen ? _boatModule.ShutterDevice.OpenCloseStatus == DeviceStatus.Open : _boatModule.ShutterDevice.OpenCloseStatus == DeviceStatus.Close;
  119. }, timeout * 1000);
  120. if (ret.Item1)
  121. {
  122. if (ret.Item2 == Result.FAIL)
  123. {
  124. throw (new RoutineFaildException());
  125. }
  126. else if (ret.Item2 == Result.TIMEOUT) //timeout
  127. {
  128. if (isOpen)
  129. {
  130. _boatModule.ShutterDevice.AutoShutterOpenTimeOut.Set($"can not complete in {timeout} seconds");
  131. }
  132. else
  133. {
  134. _boatModule.ShutterDevice.AutoShutterCloseTimeOut.Set($"can not complete in {timeout} seconds");
  135. }
  136. throw (new RoutineFaildException());
  137. }
  138. else
  139. throw (new RoutineBreakException());
  140. }
  141. }
  142. private void CheckPrepareMove(int id, int timeout)
  143. {
  144. var reason = "";
  145. Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
  146. {
  147. Notify($"Check boat move enable");
  148. return true;
  149. }, () =>
  150. {
  151. return _boatModule.CheckPrepareMove(out reason);
  152. }, timeout * 2 * 1000);
  153. if (ret.Item1)
  154. {
  155. if (ret.Item2 == Result.FAIL)
  156. {
  157. throw (new RoutineFaildException());
  158. }
  159. else if (ret.Item2 == Result.TIMEOUT) //timeout
  160. {
  161. _boatModule.BoatZAxisMoveFailedForInterlock.Set($"{reason} is not OK, can not complete in {timeout} seconds");
  162. throw (new RoutineFaildException());
  163. }
  164. else
  165. throw (new RoutineBreakException());
  166. }
  167. }
  168. private void SetBoatZAxisMove(int id, BoatPosition position, float speed, int timeout)
  169. {
  170. Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
  171. {
  172. Notify($"Boat ZAxis movet to {position}");
  173. string reason;
  174. if (!_boatModule.ZAxisDevice.SetServoMoveTo(position.ToString(), out reason, speed))
  175. {
  176. //_boatModule.BoatDevice.BoatRAxisMoveFailedForInterlock.Description = reason;
  177. _boatModule.BoatZAxisMoveFailedForInterlock.Set(reason);
  178. }
  179. return true;
  180. }, () =>
  181. {
  182. if (_boatModule.ZAxisDevice.IsError)
  183. return null;
  184. return _boatModule.ZAxisDevice.CheckServoAtPosition(position.ToString());
  185. }, timeout * 2 * 1000);
  186. if (ret.Item1)
  187. {
  188. if (ret.Item2 == Result.FAIL)
  189. {
  190. _boatModule.ZAxisDevice.ServoStop();
  191. throw (new RoutineFaildException());
  192. }
  193. else if (ret.Item2 == Result.TIMEOUT) //timeout
  194. {
  195. _boatModule.ZAxisDevice.ServoStop();
  196. _boatModule.BoatZAxisMoveTimeOut.Set($"can not complete in {timeout} seconds");
  197. throw (new RoutineFaildException());
  198. }
  199. else
  200. throw (new RoutineBreakException());
  201. }
  202. }
  203. }
  204. }