HongHuVR.cs 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368
  1. using Aitex.Core.RT.Log;
  2. using Aitex.Core.RT.SCCore;
  3. using Aitex.Core.Util;
  4. using Aitex.Sorter.Common;
  5. using MECF.Framework.Common.CommonData;
  6. using MECF.Framework.Common.Equipment;
  7. using MECF.Framework.Common.SubstrateTrackings;
  8. using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.LoadPorts;
  9. using System;
  10. using System.Collections.Generic;
  11. using System.Linq;
  12. using System.Text;
  13. using System.Text.RegularExpressions;
  14. using System.Threading.Tasks;
  15. using Venus_Core;
  16. using Venus_RT.Modules;
  17. namespace Venus_RT.Devices.VCE
  18. {
  19. //泓浒
  20. enum VRStep
  21. {
  22. Idle,
  23. Home,
  24. Move,
  25. Halt,
  26. Release,
  27. Goto,
  28. Pick,
  29. PickExtend,
  30. PickRetract,
  31. Place,
  32. PlaceExtend,
  33. PlaceRetract,
  34. Xfer,
  35. CheckLoad_ArmA,
  36. CheckLoad_ArmB,
  37. SetLoad,
  38. ReQueryLoad,
  39. }
  40. public class HongHuVR : ITransferRobot
  41. {
  42. private readonly AsyncSocket _socket;
  43. private const string EOF = "\r";
  44. private RState _status;
  45. private bool _IsHomed;
  46. private VRStep _currentStep = VRStep.Idle;
  47. private Dictionary<ModuleName, int> _StationNumbers = new Dictionary<ModuleName, int>();
  48. private RobotMoveInfo _robotMoveInfo = new RobotMoveInfo();
  49. public RState Status { get { return _status; } }
  50. public bool IsHomed { get { return _IsHomed; } }
  51. private string Hand2Arm(Hand hand) => hand == Hand.Blade1 ? "B" : "A";
  52. private readonly Regex _rex_check_load = new Regex(@"LOAD\s+(A|B)\s+(\w+)\s*");
  53. private readonly Regex _rex_error_code = new Regex(@"_ERR\s+(\d+)\s*");
  54. public RobotMoveInfo VaccumRobotMoveInfo { get { return _robotMoveInfo; } }
  55. public RobotMoveInfo TMRobotMoveInfo => throw new NotImplementedException();
  56. public double Offset_X => 0;
  57. public double Offset_Y => 0;
  58. public double Offset_D => 0;
  59. public Dictionary<string, string> _error2msg = new Dictionary<string, string>()
  60. {
  61. { "701" , "设备检查互锁,发现无法执行"},
  62. { "722" , "设备动作,但是发生异常"},
  63. };
  64. public HongHuVR()
  65. {
  66. _socket = new AsyncSocket("", EOF);
  67. _socket.Connect(SC.GetStringValue($"VacRobot.IPAddress"));
  68. _socket.OnDataChanged += OnReceiveMessage;
  69. _socket.OnErrorHappened += OnErrorHappen;
  70. _status = RState.Init;
  71. _IsHomed = false;
  72. }
  73. //初始化某个轴
  74. //1.清错
  75. //2.设备上电
  76. //3.各轴按顺序运动
  77. public bool Home()
  78. {
  79. _status = RState.Running;
  80. _currentStep = VRStep.Home;
  81. return _SendCommand("HOME ALL");
  82. }
  83. public bool Halt()
  84. {
  85. _status = RState.Running;
  86. _currentStep = VRStep.Halt;
  87. return _SendCommand("HALT");
  88. }
  89. public bool Release()
  90. {
  91. _status = RState.Running;
  92. _currentStep = VRStep.Release;
  93. return _SendCommand("RELEASE");
  94. }
  95. //public bool MOVE()
  96. //{
  97. //}
  98. public bool Pick(ModuleName station, int slot, Hand hand)
  99. {
  100. if (!CheckRobotStatus())
  101. return false;
  102. _currentStep = VRStep.Pick;
  103. _status = RState.Running;
  104. SetRobotMovingInfo(RobotAction.Picking, hand, station);
  105. return _SendCommand($"PICK {_StationNumbers[station]} ARM {Hand2Arm(hand)}");
  106. }
  107. public bool PickExtend(ModuleName station, int slot, Hand hand)
  108. {
  109. if (!CheckRobotStatus())
  110. return false;
  111. _currentStep = VRStep.PickExtend;
  112. _status = RState.Running;
  113. SetRobotMovingInfo(RobotAction.Picking, hand, station);
  114. return _SendCommand($"PICK {_StationNumbers[station]} ARM {Hand2Arm(hand)} ENRT NR");
  115. }
  116. public bool PickRetract(ModuleName station, int slot, Hand hand)
  117. {
  118. if (!CheckRobotStatus())
  119. return false;
  120. _currentStep = VRStep.PickRetract;
  121. _status = RState.Running;
  122. SetRobotMovingInfo(RobotAction.Picking, hand, station);
  123. return _SendCommand($"PICK {_StationNumbers[station]} ARM {Hand2Arm(hand)} STRT NR");
  124. }
  125. public bool Place(ModuleName station, int slot, Hand hand)
  126. {
  127. if (!CheckRobotStatus())
  128. return false;
  129. _currentStep = VRStep.Place;
  130. _status = RState.Running;
  131. SetRobotMovingInfo(RobotAction.Picking, hand, station);
  132. return _SendCommand($"PLACE {_StationNumbers[station]} ARM {Hand2Arm(hand)}");
  133. }
  134. public bool PlaceExtend(ModuleName station, int slot, Hand hand)
  135. {
  136. if (!CheckRobotStatus())
  137. return false;
  138. _currentStep = VRStep.Place;
  139. _status = RState.Running;
  140. SetRobotMovingInfo(RobotAction.Picking, hand, station);
  141. return _SendCommand($"PLACE {_StationNumbers[station]} ARM {Hand2Arm(hand)} ENRT NR");
  142. }
  143. public bool PlaceRetract(ModuleName station, int slot, Hand hand)
  144. {
  145. if (!CheckRobotStatus())
  146. return false;
  147. _currentStep = VRStep.Place;
  148. _status = RState.Running;
  149. SetRobotMovingInfo(RobotAction.Picking, hand, station);
  150. return _SendCommand($"PLACE {_StationNumbers[station]} ARM {Hand2Arm(hand)} STRT NR");
  151. }
  152. public bool Transfer(ModuleName fromstation, ModuleName tostation, Hand hand)
  153. {
  154. if (!CheckRobotStatus())
  155. return false;
  156. _currentStep = VRStep.Xfer;
  157. _status = RState.Running;
  158. return _SendCommand($"XFER ARM {Hand2Arm(hand)} {_StationNumbers[fromstation]} {_StationNumbers[tostation]}");
  159. }
  160. public bool CheckLoad(Hand hand = Hand.Blade1)
  161. {
  162. if (_currentStep != VRStep.Home && _currentStep != VRStep.CheckLoad_ArmA && !CheckRobotStatus())
  163. return false;
  164. _currentStep = hand == Hand.Blade2 ? VRStep.CheckLoad_ArmB : VRStep.CheckLoad_ArmA;
  165. _status = RState.Running;
  166. return _SendCommand($"CHECK LOAD ARM {Hand2Arm(hand)}");
  167. }
  168. public bool QueryAwc()
  169. {
  170. return true;
  171. }
  172. public bool Goto(ModuleName station, int slot, Hand hand)
  173. {
  174. _currentStep = VRStep.Goto;
  175. _status = RState.Running;
  176. return _SendCommand($"GOTO N {_StationNumbers[station]}");
  177. }
  178. public bool MoveTo(ModuleName stnFrom, ModuleName stnTo, Hand hand)
  179. {
  180. _currentStep = VRStep.Move;
  181. _status = RState.Running;
  182. return _SendCommand($"MOVE R ABS 100 ARM {Hand2Arm(hand)}"); ;
  183. }
  184. private bool _SendCommand(string cmd)
  185. {
  186. LOG.WriteSingeLine(eEvent.INFO_TM_ROBOT, ModuleName.VaccumRobot, $"Send Command to HongHu VaccumRobot: {cmd}");
  187. return _socket.Write(cmd + EOF);
  188. }
  189. private bool CheckRobotStatus()
  190. {
  191. if (Status == RState.Init)
  192. {
  193. LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.VaccumRobot, "VaccumRobot is not homed, please home first.");
  194. return false;
  195. }
  196. else if (Status == RState.Running)
  197. {
  198. LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.VaccumRobot, "VaccumRobot is busy, please wait a minute");
  199. return false;
  200. }
  201. else if (Status == RState.Failed || Status == RState.Timeout)
  202. {
  203. LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.VaccumRobot, "VaccumRobot has a error, please check and fix the hardware issue and home it");
  204. return false;
  205. }
  206. return true;
  207. }
  208. private void OnReceiveMessage(string RevMsg)
  209. {
  210. LOG.WriteSingeLine(eEvent.INFO_TM_ROBOT, ModuleName.VaccumRobot, $"Receive message from HongHu VaccumRobot: {RevMsg}, while {_currentStep}");
  211. if (_rex_error_code.IsMatch(RevMsg))
  212. {
  213. _IsHomed = false;
  214. _status = RState.Failed;
  215. var results = _rex_error_code.Match(RevMsg);
  216. ErrorMessageHandler(results.Groups[1].Value);
  217. return;
  218. }
  219. switch (_currentStep)
  220. {
  221. case VRStep.Goto:
  222. case VRStep.Move:
  223. case VRStep.Xfer:
  224. case VRStep.Pick:
  225. case VRStep.PickExtend:
  226. case VRStep.PickRetract:
  227. case VRStep.Place:
  228. case VRStep.PlaceExtend:
  229. case VRStep.PlaceRetract:
  230. {
  231. if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
  232. {
  233. _currentStep = VRStep.Idle;
  234. _status = RState.End;
  235. }
  236. else
  237. {
  238. ReportWrongMsg(RevMsg);
  239. }
  240. if (_currentStep != VRStep.PickExtend && _currentStep != VRStep.PlaceExtend)
  241. SetRobotMovingInfo(RobotAction.None, Hand.Both, ModuleName.VaccumRobot);
  242. }
  243. break;
  244. case VRStep.Home:
  245. {
  246. if (RevMsg.Trim() == "_RDY")
  247. {
  248. //CheckLoad(Hand.Blade1);
  249. _currentStep = VRStep.Idle;
  250. _status = RState.End;
  251. _IsHomed = true;
  252. }
  253. else
  254. ReportWrongMsg(RevMsg);
  255. }
  256. break;
  257. case VRStep.CheckLoad_ArmA:
  258. {
  259. if (_rex_check_load.IsMatch(RevMsg))
  260. {
  261. GetCheckLoadResult(RevMsg);
  262. CheckLoad(Hand.Blade2);
  263. }
  264. else
  265. ReportWrongMsg(RevMsg);
  266. }
  267. break;
  268. case VRStep.CheckLoad_ArmB:
  269. {
  270. if (_rex_check_load.IsMatch(RevMsg))
  271. {
  272. GetCheckLoadResult(RevMsg);
  273. _currentStep = VRStep.Idle;
  274. _status = RState.End;
  275. _IsHomed = true;
  276. }
  277. }
  278. break;
  279. default:
  280. if (!RevMsg.Contains("_EVENT"))
  281. ReportWrongMsg(RevMsg);
  282. break;
  283. }
  284. }
  285. private void GetCheckLoadResult(string revMsg)
  286. {
  287. Match result = _rex_check_load.Match(revMsg);
  288. string Arm = result.Groups[1].Value;
  289. string WaferStatus = result.Groups[2].Value;
  290. if (WaferStatus == "ON")
  291. {
  292. WaferManager.Instance.CreateWafer(ModuleName.VaccumRobot, Arm == "A" ? 0 : 1, Aitex.Core.Common.WaferStatus.Unknown);
  293. }
  294. }
  295. private void ErrorMessageHandler(string errorcode)
  296. {
  297. string ErrorInfo;
  298. if (_error2msg.ContainsKey(errorcode))
  299. {
  300. ErrorInfo = _error2msg[errorcode];
  301. LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.VaccumRobot, ErrorInfo);
  302. }
  303. else
  304. {
  305. LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.VaccumRobot, $"Dictionary Not Contains error code:{errorcode}");
  306. }
  307. }
  308. private void OnErrorHappen(ErrorEventArgs args)
  309. {
  310. Singleton<RouteManager>.Instance.TM.PostMsg(TMEntity.MSG.Error);
  311. LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.VaccumRobot, $"HongHu VaccumRobot Error: {args.Reason} while {_currentStep}");
  312. }
  313. private void ReportWrongMsg(string revMsg)
  314. {
  315. LOG.Write(eEvent.ERR_DEVICE_INFO, ModuleName.VaccumRobot, $"Receive wrong message:{revMsg} while {_currentStep}");
  316. }
  317. public void SetRobotMovingInfo(RobotAction action, Hand hand, ModuleName target)
  318. {
  319. _robotMoveInfo.Action = action;
  320. _robotMoveInfo.ArmTarget = hand == Hand.Blade1 ? RobotArm.ArmA : (hand == Hand.Both ? RobotArm.Both : RobotArm.ArmB);
  321. _robotMoveInfo.BladeTarget = $"{_robotMoveInfo.ArmTarget}.{target}";
  322. }
  323. }
  324. }