From b26310b608fa9f99a6992b32cc217680fd88b7e3 Mon Sep 17 00:00:00 2001 From: HCL <1625932291@qq.com> Date: Tue, 5 Dec 2023 16:47:04 +0800 Subject: [PATCH] =?UTF-8?q?1.Map=20=E8=BF=9B=E5=BA=A6=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Robots/Sunway/SunwayRobot.cs | 1 - .../Robots/Sunway/SunwayRobotHandler.cs | 193 +++++++++++++----- .../Robots/Sunway/SunwayRobotSimulator.cs | 2 + 3 files changed, 148 insertions(+), 48 deletions(-) diff --git a/MECF.Framework.RT.EquipmentLibrary/HardwareUnits/Robots/Sunway/SunwayRobot.cs b/MECF.Framework.RT.EquipmentLibrary/HardwareUnits/Robots/Sunway/SunwayRobot.cs index 61086b5..ca730b0 100644 --- a/MECF.Framework.RT.EquipmentLibrary/HardwareUnits/Robots/Sunway/SunwayRobot.cs +++ b/MECF.Framework.RT.EquipmentLibrary/HardwareUnits/Robots/Sunway/SunwayRobot.cs @@ -974,7 +974,6 @@ namespace MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.Sunway { if (param.Length >= 1) { - lock (_locker) { _lstHandler.AddLast(new SunwayRobotMapHandler(this, param[0].ToString(), _scMotionTimeout.IntValue)); diff --git a/MECF.Framework.RT.EquipmentLibrary/HardwareUnits/Robots/Sunway/SunwayRobotHandler.cs b/MECF.Framework.RT.EquipmentLibrary/HardwareUnits/Robots/Sunway/SunwayRobotHandler.cs index 55ceb92..23d41f8 100644 --- a/MECF.Framework.RT.EquipmentLibrary/HardwareUnits/Robots/Sunway/SunwayRobotHandler.cs +++ b/MECF.Framework.RT.EquipmentLibrary/HardwareUnits/Robots/Sunway/SunwayRobotHandler.cs @@ -1,6 +1,8 @@ -using MECF.Framework.Common.CommonData; +using Aitex.Core.RT.Device; +using MECF.Framework.Common.CommonData; using MECF.Framework.Common.Communications; using MECF.Framework.Common.Equipment; +using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.SiasunPhoenixB; using System; using System.Collections.Generic; using System.Linq; @@ -160,6 +162,148 @@ namespace MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.Sunway } } + public class SunwayRobotGotoMapHandler : SunwayRobotHandler + { + //GOTO N station [MAP (EX|RE)] [[RX] R1|R2|R3] + public SunwayRobotGotoMapHandler(SunwayRobot device, ModuleName module, bool isRetract,int timeout = 60) + : base(device, "GOTO", $"N {device.ModuleAssociateStationDic[module.ToString()]} MAP {(isRetract ? "RE" : "EX")}") + { + AckTimeout = TimeSpan.FromSeconds(timeout); + CompleteTimeout = TimeSpan.FromSeconds(timeout); + + device.MoveInfo = new RobotMoveInfo() + { + Action = RobotAction.Moving, + ArmTarget = RobotArm.Both, + BladeTarget = "ArmA.System", + }; + } + + public override bool HandleMessage(MessageBase msg, out bool handled) + { + var result = msg as SunwayRobotMessage; + handled = false; + if (!result.IsResponse) + { + Device.NoteError("Robot Goto Map Timeout"); + return true; + } + + if (result.Data == "_RDY") + { + Device.IsBusy = false; + } + else if (result.Data.Contains("ERR")) + { + Device.NoteError("Robot Goto Map Response Error"); + return false; + } + + Device.MoveInfo = new RobotMoveInfo() + { + Action = RobotAction.Moving, + ArmTarget = RobotArm.ArmA, + BladeTarget = ModuleName.System.ToString(), + }; + + ResponseMessage = msg; + handled = true; + _onHandlerEndTask?.Invoke(); + Device.NoteActionCompleted(); + return true; + } + } + + public class RobotSiasunPhoenixBGotoHandler : RobotSiasunPhoenixBHandler + { + //GOTO N station [R (EX|RE)] [Z (UP|DN)] [SLOT num] [[ARM] arm] + public RobotSiasunPhoenixBGotoHandler(RobotSiasunPhoenixB device, ModuleName module, int slot, RobotArmEnum blade, + bool isRetract = true, bool isZaxisDown = true, int timeout = 60) + : base(device, "GOTO", $"N {device.ModuleAssociateStationDic[module.ToString()]} R {(isRetract ? "RE" : "EX")}" + + $" Z {(isZaxisDown ? "DN" : "UP")} SLOT {slot} ARM {(blade == RobotArmEnum.Blade1 ? "A" : "B")}") + { + AckTimeout = TimeSpan.FromSeconds(timeout); + CompleteTimeout = TimeSpan.FromSeconds(timeout); + _target = (blade == RobotArmEnum.Blade2 ? "ArmB" : "ArmA") + "." + module; + _blade = blade; + + device.MoveInfo = new RobotMoveInfo() + { + Action = isRetract ? RobotAction.Moving : RobotAction.Picking, + ArmTarget = blade == RobotArmEnum.Blade1 ? RobotArm.ArmA : RobotArm.ArmB, + BladeTarget = _target, + }; + } + + public override bool HandleMessage(MessageBase msg, out bool handled) + { + var result = msg as RobotSiasunPhoenixBMessage; + if (result.IsError) + { + Device.NoteError(result.Data); + } + else + { + Device.NoteError(null); + } + + ResponseMessage = msg; + handled = true; + Device.NoteActionCompleted(); + + return true; + } + + } + + public class SunwayRobotMapHandler : SunwayRobotHandler + { + //MAP [module] + public SunwayRobotMapHandler(SunwayRobot device, string module, int timeout = 60) + : base(device, $"MAP {device.ModuleAssociateStationDic[module]}") + { + AckTimeout = TimeSpan.FromSeconds(timeout); + CompleteTimeout = TimeSpan.FromSeconds(timeout); + + device.MoveInfo = new RobotMoveInfo() + { + Action = RobotAction.Picking, + ArmTarget = RobotArm.ArmA, + BladeTarget = module, + }; + } + + public override bool HandleMessage(MessageBase msg, out bool handled) + { + var result = msg as SunwayRobotMessage; + handled = false; + if (!result.IsResponse) + { + Device.NoteError("Robot MAP Timeout"); + return true; + } + + if (result.Data == "_RDY") + { + + } + else if (result.Data.Contains("ERR")) + { + Device.NoteError("Robot MAP Response Error"); + return false; + } + Device.MoveInfo = new RobotMoveInfo() + { + Action = RobotAction.Moving, + ArmTarget = RobotArm.ArmA, + BladeTarget = ModuleName.System.ToString(), + }; + ResponseMessage = msg; + handled = true; + return true; + } + } + public class SunwayRobotSTATHandler : SunwayRobotHandler { @@ -945,52 +1089,7 @@ namespace MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.Sunway } } - public class SunwayRobotMapHandler : SunwayRobotHandler - { - //MAP [module] - public SunwayRobotMapHandler(SunwayRobot device, string module, int timeout = 60) - : base(device, $"MAP {device.ModuleAssociateStationDic[module]}") - { - AckTimeout = TimeSpan.FromSeconds(timeout); - CompleteTimeout = TimeSpan.FromSeconds(timeout); - - device.MoveInfo = new RobotMoveInfo() - { - Action = RobotAction.Picking, - ArmTarget = RobotArm.ArmA, - BladeTarget = module, - }; - } - - public override bool HandleMessage(MessageBase msg, out bool handled) - { - var result = msg as SunwayRobotMessage; - handled = false; - if (!result.IsResponse) - { - Device.NoteError("Robot MAP Timeout"); - return true; - } - if (result.Data == "_RDY") - { - - } - else if (result.Data.Contains("ERR")) - { - Device.NoteError("Robot MAP Response Error"); - return false; - } - Device.MoveInfo = new RobotMoveInfo() - { - Action = RobotAction.Moving, - ArmTarget = RobotArm.ArmA, - BladeTarget = ModuleName.System.ToString(), - }; - ResponseMessage = msg; - handled = true; - return true; - } - } + public class SunwayRobotRSRHandler : SunwayRobotHandler { diff --git a/SimulatorCore/Robots/Sunway/SunwayRobotSimulator.cs b/SimulatorCore/Robots/Sunway/SunwayRobotSimulator.cs index 9550baf..f613f73 100644 --- a/SimulatorCore/Robots/Sunway/SunwayRobotSimulator.cs +++ b/SimulatorCore/Robots/Sunway/SunwayRobotSimulator.cs @@ -50,6 +50,7 @@ namespace MECF.Framework.Simulator.Core.Robots.Sunway { AddCommandHandler("HOME", HandleHome); AddCommandHandler("RQ", HandleRQLoad); + AddCommandHandler("GOTO", HandleHome); AddCommandHandler("MAP", HandleMap); @@ -99,6 +100,7 @@ namespace MECF.Framework.Simulator.Core.Robots.Sunway OnWriteMessage("LOAD A OFF"); } + internal void HandleGetSp(string msg) { // GETSP B 11 1