using System; using System.Collections.Generic; using System.IO.Ports; using System.Linq; using System.Text; using Aitex.Core.Common.DeviceData; using Aitex.Core.RT.Device; using Aitex.Core.RT.Device.Unit; using Aitex.Core.RT.Event; using Aitex.Core.RT.Log; using Aitex.Core.RT.OperationCenter; using Aitex.Core.RT.SCCore; using Aitex.Core.Util; using MECF.Framework.Common.Communications; using MECF.Framework.Common.Device.Bases; using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Common; using Newtonsoft.Json; using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.RobotBase; using MECF.Framework.Common.Equipment; using MECF.Framework.Common.SubstrateTrackings; using System.Threading; using Aitex.Core.Common; using Aitex.Core.RT.DataCenter; using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robot; using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.LoadPorts.LoadPortBase; using System.Xml; using Aitex.Core.RT.IOCore; using Aitex.Core.RT.Routine; using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots; using MECF.Framework.Common.CommonData; using EventType = Aitex.Core.RT.Event.EventType; namespace Aitex.Core.RT.Device.Unit { public class GAPlcRobot:RobotBaseDevice { public GAPlcRobot(string module,XmlElement node,string ioModule =""): base(node.GetAttribute("module"), node.GetAttribute("id")) { base.Module = node.GetAttribute("module");//string.IsNullOrEmpty(node.GetAttribute("module")) ? module : node.GetAttribute("module"); base.Name = node.GetAttribute("id"); ioModule = node.GetAttribute("ioModule"); _aiCurrentXPos_L = ParseAiNode("AI_CurrentXPos_L", node, ioModule); _aiCurrentXPos_H = ParseAiNode("AI_CurrentXPos_H", node, ioModule); _aiCurrentYPos_L = ParseAiNode("AI_CurrentYPos_L", node, ioModule); _aiCurrentYPos_H = ParseAiNode("AI_CurrentYPos_H", node, ioModule); _aiCurrentZPos_L = ParseAiNode("AI_CurrentZPos_L", node, ioModule); _aiCurrentZPos_H = ParseAiNode("AI_CurrentZPos_H", node, ioModule); _aiZAxisPitch_L = ParseAiNode("AI_ZAxisPitch_L", node, ioModule); _aiZAxisPitch_H = ParseAiNode("AI_ZAxisPitch_H", node, ioModule); _aiZAxisFirstPos_L = ParseAiNode("AI_ZAxisFirstPos_L", node, ioModule); _aiZAxisFirstPos_H = ParseAiNode("AI_ZAxisFirstPos_H", node, ioModule); _aiZAxisPlacePos_L = ParseAiNode("AI_ZAxisPlacePos_L", node, ioModule); _aiZAxisPlacePos_H = ParseAiNode("AI_ZAxisPlacePos_H", node, ioModule); _aiXAxisMoveDistance_L = ParseAiNode("AI_XAxisMoveDistance_L", node, ioModule); _aiXAxisMoveDistance_H = ParseAiNode("AI_XAxisMoveDistance_H", node, ioModule); _aiYAxisMoveDistance_L = ParseAiNode("AI_YAxisMoveDistance_L", node, ioModule); _aiYAxisMoveDistance_H = ParseAiNode("AI_YAxisMoveDistance_H", node, ioModule); _aiZAxisMoveSpeed_L = ParseAiNode("AI_ZAxisMoveSpeed_L", node, ioModule); _aiZAxisMoveSpeed_H = ParseAiNode("AI_ZAxisMoveSpeed_H", node, ioModule); _aiXAxisMoveSpeed_L = ParseAiNode("AI_XAxisMoveSpeed_L", node, ioModule); _aiXAxisMoveSpeed_H = ParseAiNode("AI_XAxisMoveSpeed_H", node, ioModule); _aiYAxisMoveSpeed_L = ParseAiNode("AI_YAxisMoveSpeed_L", node, ioModule); _aiYAxisMoveSpeed_H = ParseAiNode("AI_YAxisMoveSpeed_H", node, ioModule); _aiTargetPos = ParseAiNode("AI_TargetPos", node, ioModule); _aoZAxisPitch_L = ParseAoNode("AO_ZAxisPitch_L", node, ioModule); _aoZAxisPitch_H = ParseAoNode("AO_ZAxisPitch_H", node, ioModule); _aoZAxisFirstPos_L = ParseAoNode("AO_ZAxisFirstPos_L", node, ioModule); _aoZAxisFirstPos_H = ParseAoNode("AO_ZAxisFirstPos_H", node, ioModule); _aoZAxisPlacePos_L = ParseAoNode("AO_ZAxisPlacePos_L", node, ioModule); _aoZAxisPlacePos_H = ParseAoNode("AO_ZAxisPlacePos_H", node, ioModule); _aoXAxisMoveDistance_L = ParseAoNode("AO_XAxisMoveDistance_L", node, ioModule); _aoXAxisMoveDistance_H = ParseAoNode("AO_XAxisMoveDistance_H", node, ioModule); _aoYAxisMoveDistance_L = ParseAoNode("AO_YAxisMoveDistance_L", node, ioModule); _aoYAxisMoveDistance_H = ParseAoNode("AO_YAxisMoveDistance_H", node, ioModule); _aoZAxisMoveSpeed_L = ParseAoNode("AO_ZAxisMoveSpeed_L", node, ioModule); _aoZAxisMoveSpeed_H = ParseAoNode("AO_ZAxisMoveSpeed_H", node, ioModule); _aoXAxisMoveSpeed_L = ParseAoNode("AO_XAxisMoveSpeed_L", node, ioModule); _aoXAxisMoveSpeed_H = ParseAoNode("AO_XAxisMoveSpeed_H", node, ioModule); _aoYAxisMoveSpeed_L = ParseAoNode("AO_YAxisMoveSpeed_L", node, ioModule); _aoYAxisMoveSpeed_H = ParseAoNode("AO_YAxisMoveSpeed_H", node, ioModule); _aoTargetPos = ParseAoNode("AO_TargetPos", node, ioModule); _aoXMoveFwdDistance_L = ParseAoNode("AO_XMoveFwdDistance_L", node, ioModule); _aoXMoveFwdDistance_H = ParseAoNode("AO_XMoveFwdDistance_H", node, ioModule); _aoYMoveFwdDistance_L = ParseAoNode("AO_YMoveFwdDistance_L", node, ioModule); _aoYMoveFwdDistance_H = ParseAoNode("AO_YMoveFwdDistance_H", node, ioModule); _aoZMoveFwdDistance_L = ParseAoNode("AO_ZMoveFwdDistance_L", node, ioModule); _aoZMoveFwdDistance_H = ParseAoNode("AO_ZMoveFwdDistance_H", node, ioModule); _aoXMoveRevDistance_L = ParseAoNode("AO_XMoveRevDistance_L", node, ioModule); _aoXMoveRevDistance_H = ParseAoNode("AO_XMoveRevDistance_H", node, ioModule); _aoYMoveRevDistance_L = ParseAoNode("AO_YMoveRevDistance_L", node, ioModule); _aoYMoveRevDistance_H = ParseAoNode("AO_YMoveRevDistance_H", node, ioModule); _aoZMoveRevDistance_L = ParseAoNode("AO_ZMoveRevDistance_L", node, ioModule); _aoZMoveRevDistance_H = ParseAoNode("AO_ZMoveRevDistance_H", node, ioModule); _diReset_Ack = ParseDiNode("DI_Reset_Ack", node, ioModule); _diHome_Ack = ParseDiNode("DI_Home_Ack", node, ioModule); _diSave_Ack = ParseDiNode("DI_Save_Ack", node, ioModule); _diPick_Ack = ParseDiNode("DI_Pick_Ack", node, ioModule); _diPlace_Ack = ParseDiNode("DI_Place_Ack", node, ioModule); _diX_Fwd_Ack = ParseDiNode("DI_X_Fwd_Ack", node, ioModule); _diX_Rev_Ack = ParseDiNode("DI_X_Rev_Ack", node, ioModule); _diY_Fwd_Ack = ParseDiNode("DI_Y_Fwd_Ack", node, ioModule); _diY_Rev_Ack = ParseDiNode("DI_Y_Rev_Ack", node, ioModule); _diZ_Fwd_Ack = ParseDiNode("DI_Z_Fwd_Ack", node, ioModule); _diZ_Rev_Ack = ParseDiNode("DI_Z_Rev_Ack", node, ioModule); _diPlugError = ParseDiNode("DI_PlugError", node, ioModule); _diZMotionError = ParseDiNode("DI_ZMotionError", node, ioModule); _diXMotionError = ParseDiNode("DI_XMotionError", node, ioModule); _diYMotionError = ParseDiNode("DI_YMotionError", node, ioModule); _diZMotionOutLimit = ParseDiNode("DI_ZMotionOutLimit", node, ioModule); _diXMotionOutLimit = ParseDiNode("DI_XMotionOutLimit", node, ioModule); _diYMotionOutLimit = ParseDiNode("DI_YMotionOutLimit", node, ioModule); _diPLCReady = ParseDiNode("DI_PLCReady", node, ioModule); _diMotionIdle = ParseDiNode("DI_MotionIdle", node, ioModule); _doResetCmd = ParseDoNode("DO_ResetCmd", node, ioModule); _doHomeCmd = ParseDoNode("DO_HomeCmd", node, ioModule); _doSaveCmd = ParseDoNode("DO_SaveCmd", node, ioModule); _doPickCmd = ParseDoNode("DO_PickCmd", node, ioModule); _doPlaceCmd = ParseDoNode("DO_PlaceCmd", node, ioModule); _doX_FwdCmd = ParseDoNode("DO_X_FwdCmd", node, ioModule); _doX_RevCmd = ParseDoNode("DO_X_RevCmd", node, ioModule); _doY_FwdCmd = ParseDoNode("DO_Y_FwdCmd", node, ioModule); _doY_RevCmd = ParseDoNode("DO_Y_RevCmd", node, ioModule); _doZ_FwdCmd = ParseDoNode("DO_Z_FwdCmd", node, ioModule); _doZ_RevCmd = ParseDoNode("DO_Z_RevCmd", node, ioModule); EV.Subscribe(new EventItem("Alarm", AlarmRobotError, $"Robot occurred error.", EventLevel.Alarm, EventType.EventUI_Notify)); DATA.Subscribe($"{Name}.PLCReady", () => _diPLCReady.Value); DATA.Subscribe($"{Name}.MotionIdle", () => _diMotionIdle.Value); DATA.Subscribe($"{Name}.SlotPitch", () => _slotPitch); DATA.Subscribe($"{Name}.FirstZPosition", () => _zFirstPosition); DATA.Subscribe($"{Name}.PlaceWaferZPosition", () => _zPutWaferPosition); DATA.Subscribe($"{Name}.XMoveDistance", () => _xMoveDistance); DATA.Subscribe($"{Name}.YMoveDistance", () => _yMoveDistance); DATA.Subscribe($"{Name}.ZMoveSpeed", () => _zMoveSpeed); DATA.Subscribe($"{Name}.YMoveSpeed", () => _yMoveSpeed); DATA.Subscribe($"{Name}.XMoveSpeed", () => _xMoveSpeed); DATA.Subscribe($"{Name}.CurrentXPosition", () => _currentXPosition); DATA.Subscribe($"{Name}.CurrentYPosition", () => _currentYPosition); DATA.Subscribe($"{Name}.CurrentZPosition", () => _currentZPosition); _trigErrorOccurred = new R_TRIG(); _thread = new PeriodicJob(10, OnTimer, $"{Module}.{Name} MonitorHandler", true); } private string AlarmRobotError = "RobotOccurredError"; private AIAccessor _aiZAxisPitch_L; private AIAccessor _aiZAxisPitch_H; private AIAccessor _aiZAxisFirstPos_L; private AIAccessor _aiZAxisFirstPos_H; private AIAccessor _aiZAxisPlacePos_L; private AIAccessor _aiZAxisPlacePos_H; private AIAccessor _aiXAxisMoveDistance_L; private AIAccessor _aiXAxisMoveDistance_H; private AIAccessor _aiYAxisMoveDistance_L; private AIAccessor _aiYAxisMoveDistance_H; private AIAccessor _aiZAxisMoveSpeed_L; private AIAccessor _aiZAxisMoveSpeed_H; private AIAccessor _aiXAxisMoveSpeed_L; private AIAccessor _aiXAxisMoveSpeed_H; private AIAccessor _aiYAxisMoveSpeed_L; private AIAccessor _aiYAxisMoveSpeed_H; private AIAccessor _aiTargetPos; private AIAccessor _aiCurrentXPos_L; private AIAccessor _aiCurrentXPos_H; private AIAccessor _aiCurrentYPos_L; private AIAccessor _aiCurrentYPos_H; private AIAccessor _aiCurrentZPos_L; private AIAccessor _aiCurrentZPos_H; private AOAccessor _aoZAxisPitch_L; private AOAccessor _aoZAxisPitch_H; private AOAccessor _aoZAxisFirstPos_L; private AOAccessor _aoZAxisFirstPos_H; private AOAccessor _aoZAxisPlacePos_L; private AOAccessor _aoZAxisPlacePos_H; private AOAccessor _aoXAxisMoveDistance_L; private AOAccessor _aoXAxisMoveDistance_H; private AOAccessor _aoYAxisMoveDistance_L; private AOAccessor _aoYAxisMoveDistance_H; private AOAccessor _aoZAxisMoveSpeed_L; private AOAccessor _aoZAxisMoveSpeed_H; private AOAccessor _aoXAxisMoveSpeed_L; private AOAccessor _aoXAxisMoveSpeed_H; private AOAccessor _aoYAxisMoveSpeed_L; private AOAccessor _aoYAxisMoveSpeed_H; private AOAccessor _aoTargetPos; private AOAccessor _aoXMoveFwdDistance_L; private AOAccessor _aoXMoveFwdDistance_H; private AOAccessor _aoYMoveFwdDistance_L; private AOAccessor _aoYMoveFwdDistance_H; private AOAccessor _aoZMoveFwdDistance_L; private AOAccessor _aoZMoveFwdDistance_H; private AOAccessor _aoXMoveRevDistance_L; private AOAccessor _aoXMoveRevDistance_H; private AOAccessor _aoYMoveRevDistance_L; private AOAccessor _aoYMoveRevDistance_H; private AOAccessor _aoZMoveRevDistance_L; private AOAccessor _aoZMoveRevDistance_H; private DIAccessor _diReset_Ack; private DIAccessor _diHome_Ack; private DIAccessor _diSave_Ack; private DIAccessor _diPick_Ack; private DIAccessor _diPlace_Ack; private DIAccessor _diX_Fwd_Ack; private DIAccessor _diX_Rev_Ack; private DIAccessor _diY_Fwd_Ack; private DIAccessor _diY_Rev_Ack; private DIAccessor _diZ_Fwd_Ack; private DIAccessor _diZ_Rev_Ack; private DIAccessor _diPlugError; private DIAccessor _diZMotionError; private DIAccessor _diXMotionError; private DIAccessor _diYMotionError; private DIAccessor _diZMotionOutLimit; private DIAccessor _diXMotionOutLimit; private DIAccessor _diYMotionOutLimit; private DIAccessor _diPLCReady; private DIAccessor _diMotionIdle; private DOAccessor _doResetCmd; private DOAccessor _doHomeCmd; private DOAccessor _doSaveCmd; private DOAccessor _doPickCmd; private DOAccessor _doPlaceCmd; private DOAccessor _doX_FwdCmd; private DOAccessor _doX_RevCmd; private DOAccessor _doY_FwdCmd; private DOAccessor _doY_RevCmd; private DOAccessor _doZ_FwdCmd; private DOAccessor _doZ_RevCmd; private PeriodicJob _thread; private R_TRIG _trigErrorOccurred; private int _currentXPosition { get { return TwoShortConvInt(_aiCurrentXPos_L.Value, _aiCurrentXPos_H.Value); } } private int _currentYPosition { get { return TwoShortConvInt(_aiCurrentYPos_L.Value, _aiCurrentYPos_H.Value); } } private int _currentZPosition { get { return TwoShortConvInt(_aiCurrentZPos_L.Value, _aiCurrentZPos_H.Value); } } private int _slotPitch { get { return TwoShortConvInt(_aiZAxisPitch_L.Value, _aiZAxisPitch_H.Value); } } private int _zFirstPosition { get { return TwoShortConvInt(_aiZAxisFirstPos_L .Value, _aiZAxisFirstPos_H.Value); } } private int _zPutWaferPosition { get { return TwoShortConvInt(_aiZAxisPlacePos_L.Value, _aiZAxisPlacePos_H.Value); } } private int _xMoveDistance { get { return TwoShortConvInt(_aiXAxisMoveDistance_L.Value, _aiXAxisMoveDistance_H.Value); } } private int _yMoveDistance { get { return TwoShortConvInt(_aiYAxisMoveDistance_L.Value, _aiYAxisMoveDistance_H.Value); } } private int _zMoveSpeed { get { return TwoShortConvInt(_aiZAxisMoveSpeed_L.Value, _aiZAxisMoveSpeed_H.Value); } } private int _yMoveSpeed { get { return TwoShortConvInt(_aiYAxisMoveSpeed_L.Value, _aiYAxisMoveSpeed_H.Value); } } private int _xMoveSpeed { get { return TwoShortConvInt(_aiXAxisMoveSpeed_L.Value, _aiXAxisMoveSpeed_H.Value); } } private bool OnTimer() { _trigErrorOccurred.CLK = _diPlugError.Value || _diZMotionError.Value || _diXMotionError.Value || _diYMotionError.Value || _diZMotionOutLimit.Value || _diXMotionOutLimit.Value || _diYMotionOutLimit.Value; if(_trigErrorOccurred.Q) { EV.Notify(AlarmRobotError); if (_diPlugError.Value) { EV.PostAlarmLog("Robot", $"Robot occurred error: _diPlugError."); } if (_diZMotionError.Value) { EV.PostAlarmLog("Robot", $"Robot occurred error: _diZMotionError."); } if (_diXMotionError.Value) { EV.PostAlarmLog("Robot", $"Robot occurred error: _diXMotionError."); } if (_diYMotionError.Value) { EV.PostAlarmLog("Robot", $"Robot occurred error: _diYMotionError."); } if (_diZMotionOutLimit.Value) { EV.PostAlarmLog("Robot", $"Robot occurred error: _diZMotionOutLimit."); } if (_diYMotionOutLimit.Value) { EV.PostAlarmLog("Robot", $"Robot occurred error: _diYMotionOutLimit."); } if (_diXMotionOutLimit.Value) { EV.PostAlarmLog("Robot", $"Robot occurred error: _diXMotionOutLimit."); } OnError("Robot Error"); } return true; } #region Override Robot base function private enum RobotStepEnum { ActionStep1, ActionStep2, ActionStep3, ActionStep4, ActionStep5, ActionStep6, ActionStep7, ActionStep8, ActionStep9, ActionStep10, ActionStep11, ActionStep12, ActionStep13, ActionStep14, ActionStep15, ActionStep16, } private DateTime _dtActionStart; protected override bool fClear(object[] param) { return true; } protected override bool fStartReadData(object[] param) { return true; } protected override bool fStartSetParameters(object[] param) { try { _aoZAxisPitch_L.Value = _aiZAxisPitch_L.Value; _aoZAxisPitch_H.Value = _aiZAxisPitch_H.Value; _aoZAxisFirstPos_L.Value = _aiZAxisFirstPos_L.Value; _aoZAxisFirstPos_H.Value = _aiZAxisFirstPos_H.Value; _aoZAxisPlacePos_L.Value = _aiZAxisPlacePos_L.Value; _aoZAxisPlacePos_H.Value = _aiZAxisPlacePos_H.Value; _aoXAxisMoveDistance_L.Value = _aiXAxisMoveDistance_L.Value; _aoXAxisMoveDistance_H.Value = _aiXAxisMoveDistance_H.Value; _aoYAxisMoveDistance_L.Value = _aiYAxisMoveDistance_L.Value; _aoYAxisMoveDistance_H.Value = _aiYAxisMoveDistance_H.Value; _aoZAxisMoveSpeed_L.Value = _aiZAxisMoveSpeed_L.Value; _aoZAxisMoveSpeed_H.Value = _aiZAxisMoveSpeed_H.Value; _aoXAxisMoveSpeed_L.Value = _aiXAxisMoveSpeed_L.Value; _aoXAxisMoveSpeed_H.Value = _aiXAxisMoveSpeed_H.Value; _aoYAxisMoveSpeed_L.Value = _aiYAxisMoveSpeed_L.Value; _aoYAxisMoveSpeed_H.Value = _aiYAxisMoveSpeed_H.Value; string setcommand = param[0].ToString(); int setvalue = Convert.ToInt32(param[1].ToString()); short lowbit=0, highbit=0; IntConvTwoShort(setvalue, ref lowbit, ref highbit); _dtActionStart = DateTime.Now; ResetRoutine(); switch (setcommand) { case "SlotPitch": _aoZAxisPitch_L.Value = lowbit; _aoZAxisPitch_H.Value = highbit; break; case "FirstZPosition": _aoZAxisFirstPos_L.Value = lowbit; _aoZAxisFirstPos_H.Value = highbit; break; case "PlaceWaferZPosition": _aoZAxisPlacePos_L.Value = lowbit; _aoZAxisPlacePos_H.Value = highbit; break; case "XMoveDistance": _aoXAxisMoveDistance_L.Value = lowbit; _aoXAxisMoveDistance_H.Value = highbit; break; case "YMoveDistance": _aoYAxisMoveDistance_L.Value = lowbit; _aoYAxisMoveDistance_H.Value = highbit; break; case "ZMoveSpeed": _aoZAxisMoveSpeed_L.Value = lowbit; _aoZAxisMoveSpeed_H.Value = highbit; break; case "XMoveSpeed": _aoXAxisMoveSpeed_L.Value = lowbit; _aoXAxisMoveSpeed_H.Value = highbit; break; case "YMoveSpeed": _aoYAxisMoveSpeed_L.Value = lowbit; _aoYAxisMoveSpeed_H.Value = highbit; break; default: return false; } } catch(Exception ex) { LOG.Write(ex); } return true; } protected override bool fMonitorSetParamter(object[] param) { IsBusy = false; if (DateTime.Now - _dtActionStart > TimeSpan.FromSeconds(RobotCommandTimeout)) { OnError("Command execution timeout"); return true; } try { SetDoState((int)RobotStepEnum.ActionStep2, _doSaveCmd, true, Notify); WaitDiState((int)RobotStepEnum.ActionStep3, RobotCommandTimeout, _diSave_Ack, true, Notify, Stop); SetDoState((int)RobotStepEnum.ActionStep4, _doSaveCmd, false, Notify); WaitDiState((int)RobotStepEnum.ActionStep5, RobotCommandTimeout, _diSave_Ack, false, Notify, Stop); } catch (RoutineBreakException) { return false; } catch (RoutineFaildException) { EV.PostAlarmLog("Alarm", "Pick wafer failed."); OnError("PickFailed"); } return true; } protected override bool fStartTransferWafer(object[] param) { return true; } protected override bool fStartUnGrip(object[] param) { return false; } protected override bool fStartGrip(object[] param) { return false; } protected override bool fStartGoTo(object[] param) { return false; } protected override bool fStartMapWafer(object[] param) { return false; } protected override bool fStartSwapWafer(object[] param) { return false; } protected override bool fStartPlaceWafer(object[] param) { if(!_diPLCReady.Value) { EV.PostAlarmLog("Robot", "PLC ready signal is OFF."); return false; } if(!_diMotionIdle.Value) { EV.PostAlarmLog("Robot", "Motion idle signal is OFF."); return false; } _dtActionStart = DateTime.Now; ResetRoutine(); try { //RobotArmEnum arm = (RobotArmEnum)param[0]; ModuleName tempmodule = (ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString()); int slotindex = int.Parse(param[2].ToString()); if (ModuleHelper.IsLoadPort(tempmodule)) { _aoTargetPos.Value = (short)(slotindex + 1); } else _aoTargetPos.Value = 26; Blade1Target = tempmodule; Blade2Target = tempmodule; CmdTarget = tempmodule; MoveInfo = new RobotMoveInfo() { Action = RobotAction.Picking, ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB, BladeTarget = BuildBladeTarget(), }; } catch (Exception ex) { LOG.Write(ex); return false; } return true; } protected override bool fMonitorPlace(object[] param) { IsBusy = false; if (DateTime.Now - _dtActionStart > TimeSpan.FromSeconds(RobotCommandTimeout)) { OnError("Command execution timeout"); return true; } try { WaitAiValue((int)RobotStepEnum.ActionStep1, RobotCommandTimeout, _aiTargetPos, _aoTargetPos.Value, Notify, Stop); SetDoState((int)RobotStepEnum.ActionStep2, _doPlaceCmd, true, Notify); WaitDiState((int)RobotStepEnum.ActionStep3, RobotCommandTimeout, _diPlace_Ack, true, Notify, Stop); SetDoState((int)RobotStepEnum.ActionStep4, _doPlaceCmd, false, Notify); WaitDiState((int)RobotStepEnum.ActionStep5, RobotCommandTimeout, _diPlace_Ack, false, Notify, Stop); } catch (RoutineBreakException) { return false; } catch (RoutineFaildException) { EV.PostAlarmLog("Alarm", "Pick wafer failed."); OnError("PickFailed"); return true; } Blade1Target = ModuleName.System; Blade2Target = ModuleName.System; CmdTarget = ModuleName.System; MoveInfo = new RobotMoveInfo() { Action = RobotAction.Picking, ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB, BladeTarget = BuildBladeTarget(), }; ModuleName sourcemodule; if (!Enum.TryParse(CurrentParamter[1].ToString(), out sourcemodule)) return false; int SourceslotIndex; if (!int.TryParse(CurrentParamter[2].ToString(), out SourceslotIndex)) return false; WaferManager.Instance.WaferMoved(RobotModuleName, 0,sourcemodule, SourceslotIndex); return true; } protected override bool fMonitorPick(object[] param) { IsBusy = false; if (DateTime.Now - _dtActionStart > TimeSpan.FromSeconds(RobotCommandTimeout)) { OnError("Command execution timeout"); return true; } try { WaitAiValue((int)RobotStepEnum.ActionStep1, RobotCommandTimeout, _aiTargetPos, _aoTargetPos.Value, Notify, Stop); SetDoState((int)RobotStepEnum.ActionStep2, _doPickCmd, true, Notify); WaitDiState((int)RobotStepEnum.ActionStep3, RobotCommandTimeout, _diPick_Ack, true, Notify, Stop); SetDoState((int)RobotStepEnum.ActionStep4, _doPickCmd, false, Notify); WaitDiState((int)RobotStepEnum.ActionStep5, RobotCommandTimeout, _diPick_Ack, false, Notify, Stop); } catch (RoutineBreakException) { return false; } catch (RoutineFaildException) { EV.PostAlarmLog("Alarm","Pick wafer failed."); OnError("PickFailed"); return true; } Blade1Target = ModuleName.System; Blade2Target = ModuleName.System; CmdTarget = ModuleName.System; MoveInfo = new RobotMoveInfo() { Action = RobotAction.Picking, ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB, BladeTarget = BuildBladeTarget(), }; ModuleName sourcemodule; if (!Enum.TryParse(CurrentParamter[1].ToString(), out sourcemodule)) return false; int SourceslotIndex; if (!int.TryParse(CurrentParamter[2].ToString(), out SourceslotIndex)) return false; WaferManager.Instance.WaferMoved(sourcemodule, SourceslotIndex, RobotModuleName, 0); return true; } protected override bool fStartPickWafer(object[] param) { if (!_diPLCReady.Value) { EV.PostAlarmLog("Robot", "PLC ready signal is OFF."); return false; } if (!_diMotionIdle.Value) { EV.PostAlarmLog("Robot", "Motion idle signal is OFF."); return false; } _dtActionStart = DateTime.Now; ResetRoutine(); try { ModuleName tempmodule = (ModuleName)Enum.Parse(typeof(ModuleName), param[1].ToString()); int slotindex = int.Parse(param[2].ToString()); if (ModuleHelper.IsLoadPort(tempmodule)) { _aoTargetPos.Value = (short)(slotindex + 1); } else _aoTargetPos.Value = 26; Blade1Target = tempmodule; Blade2Target = tempmodule; CmdTarget = tempmodule; MoveInfo = new RobotMoveInfo() { Action = RobotAction.Picking, ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB, BladeTarget = BuildBladeTarget(), }; } catch (Exception ex) { LOG.Write(ex); return false; } return true; } protected override bool fResetToReady(object[] param) { return true; } protected override bool fReset(object[] param) { _dtActionStart = DateTime.Now; ResetRoutine(); return true; } protected override bool fMonitorReset(object[] param) { IsBusy = false; if (DateTime.Now - _dtActionStart > TimeSpan.FromSeconds(RobotCommandTimeout)) { OnError("Command execution timeout"); return true; } try { SetDoState((int)RobotStepEnum.ActionStep1, _doPickCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep2, _doPlaceCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep3, _doHomeCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep4, _doSaveCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep5, _doX_FwdCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep6, _doX_RevCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep7, _doY_FwdCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep8, _doY_RevCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep9, _doZ_FwdCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep10, _doZ_RevCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep11, _doResetCmd, true, Notify); WaitDiState((int)RobotStepEnum.ActionStep12, RobotCommandTimeout, _diReset_Ack, true, Notify, Stop); SetDoState((int)RobotStepEnum.ActionStep13, _doResetCmd, false, Notify); WaitDiState((int)RobotStepEnum.ActionStep14, RobotCommandTimeout, _diReset_Ack, false, Notify, Stop); } catch (RoutineBreakException) { return false; } catch (RoutineFaildException) { EV.PostAlarmLog("Alarm", "Pick wafer failed."); OnError("PickFailed"); return true; } return true; } protected override bool fStartInit(object[] param) { _dtActionStart = DateTime.Now; ResetRoutine(); return true; } protected override bool fMonitorInit(object[] param) { IsBusy = false; if (DateTime.Now - _dtActionStart > TimeSpan.FromSeconds(RobotCommandTimeout)) { OnError("Command execution timeout"); return true; } try { SetDoState((int)RobotStepEnum.ActionStep1, _doPickCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep2, _doPlaceCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep3, _doResetCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep4, _doSaveCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep5, _doX_FwdCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep6, _doX_RevCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep7, _doY_FwdCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep8, _doY_RevCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep9, _doZ_FwdCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep10, _doZ_RevCmd, false, Notify); SetDoState((int)RobotStepEnum.ActionStep11, _doHomeCmd, true, Notify); WaitDiState((int)RobotStepEnum.ActionStep12, RobotCommandTimeout, _diHome_Ack, true, Notify, Stop); SetDoState((int)RobotStepEnum.ActionStep13, _doHomeCmd, false, Notify); WaitDiState((int)RobotStepEnum.ActionStep14, RobotCommandTimeout, _diHome_Ack, false, Notify, Stop); } catch (RoutineBreakException) { return false; } catch (RoutineFaildException) { EV.PostAlarmLog("Alarm", "Pick wafer failed."); OnError("PickFailed"); return true; } Blade1Target = ModuleName.System; Blade2Target = ModuleName.System; CmdTarget = ModuleName.System; MoveInfo = new RobotMoveInfo() { Action = RobotAction.Picking, ArmTarget = CmdRobotArm == RobotArmEnum.Lower ? RobotArm.ArmA : RobotArm.ArmB, BladeTarget = BuildBladeTarget(), }; return true; } protected override bool fStartExecuteCommand(object[] param) { CurrentExecuteCmd = param[0].ToString(); if (string.IsNullOrEmpty(CurrentExecuteCmd)) return false; _dtActionStart = DateTime.Now; ResetRoutine(); return true; } public string CurrentExecuteCmd { get; private set; } protected override bool fMonitorExecuting(object[] param) { string command = CurrentParamter[0].ToString(); Int32 pnum = Convert.ToInt32(CurrentParamter[1]); short high16bit = (short)(pnum >> 16); short low16bit = (short)(pnum & ushort.MaxValue); try { switch (command) { case "XMoveFwd": SetAoValue((int)RobotStepEnum.ActionStep1, _aoXMoveFwdDistance_H, high16bit, Notify); SetAoValue((int)RobotStepEnum.ActionStep2, _aoXMoveFwdDistance_L, low16bit, Notify); SetDoState((int)RobotStepEnum.ActionStep3, _doX_FwdCmd, true, Notify); WaitDiState((int)RobotStepEnum.ActionStep4, RobotCommandTimeout, _diX_Fwd_Ack, true, Notify,Stop); SetDoState((int)RobotStepEnum.ActionStep5, _doX_FwdCmd, false, Notify); WaitDiState((int)RobotStepEnum.ActionStep6, RobotCommandTimeout, _diX_Fwd_Ack, false, Notify, Stop); SetAoValue((int)RobotStepEnum.ActionStep7, _aoXMoveFwdDistance_H, 0, Notify); SetAoValue((int)RobotStepEnum.ActionStep8, _aoXMoveFwdDistance_L, 0, Notify); break; case "XMoveRev": high16bit = (short)((-1*pnum) >> 16); low16bit = (short)((-1 * pnum) & ushort.MaxValue); SetAoValue((int)RobotStepEnum.ActionStep1, _aoXMoveRevDistance_H, high16bit, Notify); SetAoValue((int)RobotStepEnum.ActionStep2, _aoXMoveRevDistance_L, low16bit, Notify); SetDoState((int)RobotStepEnum.ActionStep3, _doX_RevCmd, true, Notify); WaitDiState((int)RobotStepEnum.ActionStep4, RobotCommandTimeout, _diX_Rev_Ack, true, Notify, Stop); SetDoState((int)RobotStepEnum.ActionStep5, _doX_RevCmd, false, Notify); WaitDiState((int)RobotStepEnum.ActionStep6, RobotCommandTimeout, _diX_Rev_Ack, false, Notify, Stop); SetAoValue((int)RobotStepEnum.ActionStep7, _aoXMoveRevDistance_H, 0, Notify); SetAoValue((int)RobotStepEnum.ActionStep8, _aoXMoveRevDistance_L, 0, Notify); break; case "YMoveFwd": SetAoValue((int)RobotStepEnum.ActionStep1, _aoYMoveFwdDistance_H, high16bit, Notify); SetAoValue((int)RobotStepEnum.ActionStep2, _aoYMoveFwdDistance_L, low16bit, Notify); SetDoState((int)RobotStepEnum.ActionStep3, _doY_FwdCmd, true, Notify); WaitDiState((int)RobotStepEnum.ActionStep4, RobotCommandTimeout, _diY_Fwd_Ack, true, Notify, Stop); SetDoState((int)RobotStepEnum.ActionStep5, _doY_FwdCmd, false, Notify); WaitDiState((int)RobotStepEnum.ActionStep6, RobotCommandTimeout, _diY_Fwd_Ack, false, Notify, Stop); SetAoValue((int)RobotStepEnum.ActionStep7, _aoYMoveFwdDistance_H, 0, Notify); SetAoValue((int)RobotStepEnum.ActionStep8, _aoYMoveFwdDistance_L, 0, Notify); break; case "YMoveRev": high16bit = (short)((-1 * pnum) >> 16); low16bit = (short)((-1 * pnum) & ushort.MaxValue); SetAoValue((int)RobotStepEnum.ActionStep1, _aoYMoveRevDistance_H, high16bit, Notify); SetAoValue((int)RobotStepEnum.ActionStep2, _aoYMoveRevDistance_L, low16bit, Notify); SetDoState((int)RobotStepEnum.ActionStep3, _doY_RevCmd, true, Notify); WaitDiState((int)RobotStepEnum.ActionStep4, RobotCommandTimeout, _diY_Rev_Ack, true, Notify, Stop); SetDoState((int)RobotStepEnum.ActionStep5, _doY_RevCmd, false, Notify); WaitDiState((int)RobotStepEnum.ActionStep6, RobotCommandTimeout, _diY_Rev_Ack, false, Notify, Stop); SetAoValue((int)RobotStepEnum.ActionStep7, _aoYMoveRevDistance_H, 0, Notify); SetAoValue((int)RobotStepEnum.ActionStep8, _aoYMoveRevDistance_L, 0, Notify); break; case "ZMoveFwd": SetAoValue((int)RobotStepEnum.ActionStep1, _aoZMoveFwdDistance_H, high16bit, Notify); SetAoValue((int)RobotStepEnum.ActionStep2, _aoZMoveFwdDistance_L, low16bit, Notify); SetDoState((int)RobotStepEnum.ActionStep3, _doZ_FwdCmd, true, Notify); WaitDiState((int)RobotStepEnum.ActionStep4, RobotCommandTimeout, _diZ_Fwd_Ack, true, Notify, Stop); SetDoState((int)RobotStepEnum.ActionStep5, _doZ_FwdCmd, false, Notify); WaitDiState((int)RobotStepEnum.ActionStep6, RobotCommandTimeout, _diZ_Fwd_Ack, false, Notify, Stop); SetAoValue((int)RobotStepEnum.ActionStep7, _aoZMoveFwdDistance_H, 0, Notify); SetAoValue((int)RobotStepEnum.ActionStep8, _aoZMoveFwdDistance_L, 0, Notify); break; case "ZMoveRev": high16bit = (short)((-1 * pnum) >> 16); low16bit = (short)((-1 * pnum) & ushort.MaxValue); SetAoValue((int)RobotStepEnum.ActionStep1, _aoZMoveRevDistance_H, high16bit, Notify); SetAoValue((int)RobotStepEnum.ActionStep2, _aoZMoveRevDistance_L, low16bit, Notify); SetDoState((int)RobotStepEnum.ActionStep3, _doZ_RevCmd, true, Notify); WaitDiState((int)RobotStepEnum.ActionStep4, RobotCommandTimeout, _diZ_Rev_Ack, true, Notify, Stop); SetDoState((int)RobotStepEnum.ActionStep5, _doZ_RevCmd, false, Notify); WaitDiState((int)RobotStepEnum.ActionStep6, RobotCommandTimeout, _diZ_Rev_Ack, false, Notify, Stop); SetAoValue((int)RobotStepEnum.ActionStep7, _aoZMoveRevDistance_H, 0, Notify); SetAoValue((int)RobotStepEnum.ActionStep8, _aoZMoveRevDistance_L, 0, Notify); break; default: break; } } catch (RoutineBreakException) { return false; } catch (RoutineFaildException) { EV.PostAlarmLog("Alarm", $"Execute command:{command} failed."); OnError($"Execute {command} failed"); return true; } return true; } protected override bool fError(object[] param) { return true; } protected override bool fStartExtendForPick(object[] param) { return true; } protected override bool fStartExtendForPlace(object[] param) { return true; } protected override bool fStartRetractFromPick(object[] param) { return true; } protected override bool fStartRetractFromPlace(object[] param) { return true; } public override RobotArmWaferStateEnum GetWaferState(RobotArmEnum arm) { if (WaferManager.Instance.CheckHasWafer(RobotModuleName, 0)) return RobotArmWaferStateEnum.Present; return RobotArmWaferStateEnum.Absent; } private string BuildBladeTarget() { return (CmdRobotArm == RobotArmEnum.Upper ? "ArmB" : "ArmA") + "." + CmdTarget; } private static int TwoShortConvInt(short low16bit, short high16bit) { return (high16bit << 16) + low16bit + (low16bit<0 ? UInt16.MaxValue+1 :0); } private static void IntConvTwoShort(int num,ref short low16bit,ref short high16bit) { high16bit = (short)(num >> 16); low16bit = (short)(num & ushort.MaxValue); } #endregion public DOAccessor ParseDoNode(string name, XmlElement node, string ioModule = "") { if (!string.IsNullOrEmpty(node.GetAttribute(name).Trim())) return IO.DO[string.IsNullOrEmpty(ioModule) ? node.GetAttribute(name).Trim() : $"{ioModule}.{node.GetAttribute(name).Trim()}"]; return null; } public DIAccessor ParseDiNode(string name, XmlElement node, string ioModule = "") { if (!string.IsNullOrEmpty(node.GetAttribute(name).Trim())) return IO.DI[string.IsNullOrEmpty(ioModule) ? node.GetAttribute(name).Trim() : $"{ioModule}.{node.GetAttribute(name).Trim()}"]; return null; } public AOAccessor ParseAoNode(string name, XmlElement node, string ioModule = "") { if (!string.IsNullOrEmpty(node.GetAttribute(name).Trim())) return IO.AO[string.IsNullOrEmpty(ioModule) ? node.GetAttribute(name).Trim() : $"{ioModule}.{node.GetAttribute(name).Trim()}"]; return null; } public AIAccessor ParseAiNode(string name, XmlElement node, string ioModule = "") { if (!string.IsNullOrEmpty(node.GetAttribute(name).Trim())) return IO.AI[string.IsNullOrEmpty(ioModule) ? node.GetAttribute(name).Trim() : $"{ioModule}.{node.GetAttribute(name).Trim()}"]; return null; } public SCConfigItem ParseScNode(string name, XmlElement node, string ioModule = "", string defaultScPath = "") { SCConfigItem result = null; if (!string.IsNullOrEmpty(node.GetAttribute(name).Trim())) result = SC.GetConfigItem(node.GetAttribute(name)); if (result == null && !string.IsNullOrEmpty(defaultScPath) && SC.ContainsItem(defaultScPath)) result = SC.GetConfigItem(defaultScPath); return result; } public static T ParseDeviceNode(string name, XmlElement node) where T : class, IDevice { if (!string.IsNullOrEmpty(node.GetAttribute(name).Trim())) return DEVICE.GetDevice(node.GetAttribute(name)); LOG.Write(string.Format("{0}，未定义{1}", node.InnerXml, name)); return null; } public static T ParseDeviceNode(string module, string name, XmlElement node) where T : class, IDevice { string device_id = node.GetAttribute(name); if (!string.IsNullOrEmpty(device_id) && !string.IsNullOrEmpty(device_id.Trim())) { return DEVICE.GetDevice($"{module}.{device_id}"); } LOG.Write(string.Format("{0},undefined {1}", node.InnerXml, name)); return null; } public enum CarrierMode { Foup, Fosb, OpenCassette, } private void SetAoValue(int id, AOAccessor ao, short value, Action notify) { var ret = Execute(id, () => { notify($"{RobotModuleName} set {ao.Name} to {value}."); ao.Value = value; return true; }); if (ret.Item1) { if (ret.Item2 == Result.FAIL) { throw new RoutineFaildException(); } } } private void WaitAiValue(int id, int time, AIAccessor ai, short value, Action notify, Action error) { var ret = ExecuteAndWait(id, () => { notify($"Wait {RobotModuleName} {ai.Name} to be {value}"); return true; }, () => { if (ai.Value == value) return true; return false; }, time * 1000); if (ret.Item1) { if (ret.Item2 == Result.FAIL) { throw new RoutineFaildException(); } else if (ret.Item2 == Result.TIMEOUT) //timeout { //EV.Notify(RobotModuleName); error($"Wait {RobotModuleName} {ai.Name} to be {value} timeout after {time} seconds"); throw new RoutineFaildException(); } else { throw new RoutineBreakException(); } } } private void WaitDiState(int id, int time, DIAccessor di, bool state, Action notify, Action error) { var ret = ExecuteAndWait(id, () => { notify($"Wait {RobotModuleName} {di.Name} to be {state}"); return true; }, () => { if (di.Value == state) return true; return false; }, time * 1000); if (ret.Item1) { if (ret.Item2 == Result.FAIL) { throw new RoutineFaildException(); } else if (ret.Item2 == Result.TIMEOUT) //timeout { //EV.Notify(); error($"Wait {RobotModuleName} {di.Name} to be {state} timeout after {time} seconds"); throw new RoutineFaildException(); } else { throw new RoutineBreakException(); } } } private void SetDoState(int id, DOAccessor _do, bool state, Action notify) { var ret = Execute(id, () => { notify($"{RobotModuleName} start set {_do.Name} to {state}."); //if (_do.Value == state) //{ // _do.Value = !state; // Thread.Sleep(500); //} return _do.SetValue(state, out _); }); if (ret.Item1) { if (ret.Item2 == Result.FAIL) { throw new RoutineFaildException(); } } } protected void Notify(string message) { EV.PostMessage(Name, EventEnum.GeneralInfo, string.Format("{0}:{1}", Name, message)); } protected void Stop(string failReason) { OnError(string.Format("Failed {0}, {1} ", Name, failReason)); } #region Routine Executor //timer, 计算routine时间 protected DeviceTimer counter = new DeviceTimer(); protected DeviceTimer delayTimer = new DeviceTimer(); private enum STATE { IDLE, WAIT, } public int TokenId { get { return _id; } } private int _id; //step index /// /// already done steps /// private Stack _steps = new Stack(); private STATE state; //step state //idel,wait, //loop control private int loop = 0; private int loopCount = 0; private int loopID = 0; private DeviceTimer timer = new DeviceTimer(); public int LoopCounter { get { return loop; } } public int LoopTotalTime { get { return loopCount; } } // public int Timeout { get { return (int)(timer.GetTotalTime() / 1000); } } //状态持续时间，单位为秒 public int Elapsed { get { return (int)(timer.GetElapseTime() / 1000); } } protected RoutineResult RoutineToken = new RoutineResult() { Result = RoutineState.Running }; public void ResetRoutine() { _id = 0; _steps.Clear(); loop = 0; loopCount = 0; state = STATE.IDLE; counter.Start(60 * 60 * 100); //默认1小时 RoutineToken.Result = RoutineState.Running; } protected void PerformRoutineStep(int id, Func execution, RoutineResult result) { if (!Acitve(id)) return; result.Result = execution(); } #region interface public void StopLoop() { loop = loopCount; } public Tuple Loop(T id, Func func, int count) { int idx = Convert.ToInt32(id); bool bActive = Acitve(idx); if (bActive) { if (!func()) { return Tuple.Create(bActive, Result.FAIL); //执行错误 } loopID = idx; loopCount = count; next(); return Tuple.Create(true, Result.RUN); } return Tuple.Create(false, Result.RUN); } public Tuple EndLoop(T id, Func func) { int idx = Convert.ToInt32(id); bool bActive = Acitve(idx); if (bActive) { if (++loop >= loopCount) //Loop 结束 { if (!func()) { return Tuple.Create(bActive, Result.FAIL); //执行错误 } loop = 0; loopCount = 0; // Loop 结束时，当前loop和loop总数都清零 next(); return Tuple.Create(true, Result.RUN); } //继续下一LOOP next(loopID); return Tuple.Create(true, Result.RUN); } return Tuple.Create(false, Result.RUN); } public Tuple ExecuteAndWait(T id, IRoutine routine) { int idx = Convert.ToInt32(id); bool bActive = Acitve(idx); if (bActive) { if (state == STATE.IDLE) { Result startRet = routine.Start(); if (startRet == Result.FAIL) { return Tuple.Create(true, Result.FAIL); //执行错误 } else if (startRet == Result.DONE) { next(); return Tuple.Create(true, Result.DONE); } state = STATE.WAIT; } Result ret = routine.Monitor(); if (ret == Result.DONE) { next(); return Tuple.Create(true, Result.DONE); } else if (ret == Result.FAIL || ret == Result.TIMEOUT) { return Tuple.Create(true, Result.FAIL); } else { return Tuple.Create(true, Result.RUN); } } return Tuple.Create(false, Result.RUN); } public Tuple ExecuteAndWait(T id, List routines) { int idx = Convert.ToInt32(id); bool bActive = Acitve(idx); if (bActive) { if (state == STATE.IDLE) { foreach (var item in routines) { if (item.Start() == Result.FAIL) return Tuple.Create(true, Result.FAIL); } state = STATE.WAIT; } //wait all sub failed or completedboo bool bFail = false; bool bDone = true; foreach (var item in routines) { Result ret = item.Monitor(); bDone &= (ret == Result.FAIL || ret == Result.DONE); bFail |= ret == Result.FAIL; } if (bDone) { next(); if (bFail) return Tuple.Create(true, Result.FAIL); return Tuple.Create(true, Result.DONE); } return Tuple.Create(true, Result.RUN); } return Tuple.Create(false, Result.RUN); } public Tuple Check(T id, Func func) //顺序执行 { return Check(Check(Convert.ToInt32(id), func)); } public Tuple Execute(T id, Func func) //顺序执行 { return Check(execute(Convert.ToInt32(id), func)); } public Tuple Wait(T id, Func func, double timeout = int.MaxValue) //Wait condition { return Check(wait(Convert.ToInt32(id), func, timeout)); } public Tuple Wait(T id, Func func, double timeout = int.MaxValue) //Wait condition { return Check(wait(Convert.ToInt32(id), func, timeout)); } public Tuple ExecuteAndWait(T id, Func execute, Func check, double timeout = int.MaxValue) { int idx = Convert.ToInt32(id); bool bActive = Acitve(idx); bool? bExecute = false; if (bActive) { if (state == STATE.IDLE) { if (!execute()) { return Tuple.Create(bActive, Result.FAIL); //执行错误 } timer.Start(timeout); state = STATE.WAIT; } bExecute = check(); if (bExecute == null) { return Tuple.Create(bActive, Result.FAIL); //Termianate } else { if (bExecute.Value) //检查Success, next { next(); return Tuple.Create(true, Result.RUN); } } if (timer.IsTimeout()) return Tuple.Create(true, Result.TIMEOUT); return Tuple.Create(true, Result.RUN); } return Tuple.Create(false, Result.RUN); } public Tuple ExecuteAndWait(T id, Func execute, Func check, Func time) { int idx = Convert.ToInt32(id); bool bActive = Acitve(idx); bool? bExecute = false; double timeout = 0; if (bActive) { if (state == STATE.IDLE) { timeout = time(); if (!execute()) { return Tuple.Create(true, Result.FAIL); //执行错误 } timer.Start(timeout); state = STATE.WAIT; } bExecute = check(); if (bExecute == null) { return Tuple.Create(true, Result.FAIL); //Termianate } if (bExecute.Value) //检查Success, next { next(); return Tuple.Create(true, Result.RUN); } if (timer.IsTimeout()) return Tuple.Create(true, Result.TIMEOUT); return Tuple.Create(true, Result.RUN); } return Tuple.Create(false, Result.RUN); } public Tuple Wait(T id, IRoutine rt) { int idx = Convert.ToInt32(id); bool bActive = Acitve(idx); if (bActive) { if (state == STATE.IDLE) { rt.Start(); state = STATE.WAIT; } Result ret = rt.Monitor(); return Tuple.Create(true, ret); } return Tuple.Create(false, Result.RUN); } //Monitor public Tuple Monitor(T id, Func func, Func check, double time) { int idx = Convert.ToInt32(id); bool bActive = Acitve(idx); bool bCheck = false; if (bActive) { if (state == STATE.IDLE) { if ((func != null) && !func()) { return Tuple.Create(true, Result.FAIL); } timer.Start(time); state = STATE.WAIT; } bCheck = check(); if (!bCheck) { return Tuple.Create(true, Result.FAIL); //Termianate } if (timer.IsTimeout()) { next(); } return Tuple.Create(true, Result.RUN); } return Tuple.Create(false, Result.RUN); } //Delay public Tuple Delay(T id, Func func, double time) { int idx = Convert.ToInt32(id); bool bActive = Acitve(idx); if (bActive) { if (state == STATE.IDLE) { if ((func != null) && !func()) { return Tuple.Create(true, Result.FAIL); } timer.Start(time); state = STATE.WAIT; } if (timer.IsTimeout()) { next(); } return Tuple.Create(true, Result.RUN); } return Tuple.Create(false, Result.RUN); } //先delay 再运行 public Tuple DelayCheck(T id, Func func, double time) { int idx = Convert.ToInt32(id); bool bActive = Acitve(idx); if (bActive) { if (state == STATE.IDLE) { timer.Start(time); state = STATE.WAIT; } if (timer.IsTimeout()) { if (func != null && !func()) { return Tuple.Create(true, Result.FAIL); } next(); } return Tuple.Create(true, Result.RUN); } return Tuple.Create(false, Result.RUN); } #endregion private Tuple execute(int id, Func func) //顺序执行 { bool bActive = Acitve(id); bool bExecute = false; if (bActive) { bExecute = func(); if (bExecute) { next(); } } return Tuple.Create(bActive, bExecute); } private Tuple Check(int id, Func func) //check { bool bActive = Acitve(id); bool bExecute = false; if (bActive) { bExecute = func(); next(); } return Tuple.Create(bActive, bExecute); } /// /// /// /// /// /// /// item1 Active /// item2 execute /// item3 Timeout /// private Tuple wait(int id, Func func, double timeout = int.MaxValue) //Wait condition { bool bActive = Acitve(id); bool bExecute = false; bool bTimeout = false; if (bActive) { if (state == STATE.IDLE) { timer.Start(timeout); state = STATE.WAIT; } bExecute = func(); if (bExecute) { next(); } bTimeout = timer.IsTimeout(); } return Tuple.Create(bActive, bExecute, bTimeout); } private Tuple wait(int id, Func func, double timeout = int.MaxValue) //Wait condition && Check error { bool bActive = Acitve(id); bool? bExecute = false; bool bTimeout = false; if (bActive) { if (state == STATE.IDLE) { timer.Start(timeout); state = STATE.WAIT; } bExecute = func(); if (bExecute.HasValue && bExecute.Value) { next(); } bTimeout = timer.IsTimeout(); } return Tuple.Create(bActive, bExecute, bTimeout); } /// /// /// /// /// item1 true, return item2 /// private Tuple Check(Tuple value) { if (value.Item1) { if (!value.Item2) { return Tuple.Create(true, Result.FAIL); } return Tuple.Create(true, Result.RUN); } return Tuple.Create(false, Result.RUN); } private Tuple Check(Tuple value) { if (value.Item1) // 当前执行 { if (CheckTimeout(value)) //timeout { return Tuple.Create(true, Result.TIMEOUT); } return Tuple.Create(true, Result.RUN); } return Tuple.Create(false, Result.RUN); } private Tuple Check(Tuple value) { if (value.Item1) // 当前执行 { if (value.Item2 == null) { return Tuple.Create(true, Result.FAIL); } else { if (value.Item2 == false && value.Item3 == true) //timeout { return Tuple.Create(true, Result.TIMEOUT); } return Tuple.Create(true, Result.RUN); } } return Tuple.Create(false, Result.RUN); } private bool CheckTimeout(Tuple value) { return value.Item1 == true && value.Item2 == false && value.Item3 == true; } private bool Acitve(int id) // { if (_steps.Contains(id)) return false; this._id = id; return true; } private void next() { _steps.Push(this._id); state = STATE.IDLE; } private void next(int step) //loop { while (_steps.Pop() != step) ; state = STATE.IDLE; } public void Delay(int id, double delaySeconds) { Tuple ret = Delay(id, () => { return true; }, delaySeconds * 1000); if (ret.Item1) { if (ret.Item2 == Result.RUN) { throw (new RoutineBreakException()); } } } public bool IsActived(int id) { return _steps.Contains(id); } #endregion } }