129 lines
4.0 KiB
C#
129 lines
4.0 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using Aitex.Core.RT.Event;
|
|
using Aitex.Core.RT.Log;
|
|
using Aitex.Core.RT.Routine;
|
|
using Aitex.Core.RT.SCCore;
|
|
using MECF.Framework.Common.Equipment;
|
|
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots;
|
|
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.RobotBase;
|
|
|
|
namespace SicModules.TMs
|
|
{
|
|
public class TMRobotGotoRoutine : TMBaseRoutine
|
|
{
|
|
enum RoutineStep
|
|
{
|
|
RobotGoto,
|
|
}
|
|
|
|
private ModuleName _source;
|
|
private int _sourceSlot;
|
|
private RobotArmEnum _blade;
|
|
private string _EX;
|
|
private string _UP;
|
|
private int _timeout;
|
|
private TMSlitValveRoutine _closeSlitValveRoutine = new TMSlitValveRoutine();
|
|
private double _shutterAndSlitValveMotionInterval;
|
|
|
|
public TMRobotGotoRoutine()
|
|
{
|
|
Module = "TMRobot";
|
|
Name = "Goto";
|
|
}
|
|
|
|
public void Init(ModuleName source, int sourceSlot, RobotArmEnum blade,string PE,string NE)
|
|
{
|
|
_source = source;
|
|
_sourceSlot = sourceSlot;
|
|
_blade = blade;
|
|
_EX = PE;
|
|
_UP = NE;
|
|
}
|
|
|
|
public void Init(ModuleName source, int sourceSlot, int blade,string EX, string UP)
|
|
{
|
|
Init(source, sourceSlot, blade == 0 ? RobotArmEnum.Blade1 : RobotArmEnum.Blade2,EX,UP);
|
|
}
|
|
public override Result Start(params object[] objs)
|
|
{
|
|
Reset();
|
|
|
|
if (RobotDevice.RobotState != RobotStateEnum.Idle)
|
|
{
|
|
EV.PostWarningLog(Module, $"Can not Goto, TMRobot Is Not IDLE");
|
|
return Result.FAIL;
|
|
}
|
|
_timeout = SC.GetValue<int>("TMRobot.GotoTimeout");
|
|
|
|
Notify($"Start, goto {_source} slot {_sourceSlot + 1} with {(_blade == RobotArmEnum.Blade1 ? "Blade1" : "Blade2")}");
|
|
|
|
return Result.RUN;
|
|
}
|
|
|
|
public override void Abort()
|
|
{
|
|
RobotDevice.Stop();
|
|
Notify("Abort");
|
|
}
|
|
|
|
public override Result Monitor()
|
|
{
|
|
try
|
|
{
|
|
RobotGoto((int)RoutineStep.RobotGoto, RobotDevice, _source, _sourceSlot, _blade, _timeout);
|
|
}
|
|
catch (RoutineBreakException)
|
|
{
|
|
return Result.RUN;
|
|
}
|
|
catch (RoutineFaildException ex)
|
|
{
|
|
LOG.Error(ex.ToString());
|
|
RobotDevice.Stop();
|
|
return Result.FAIL;
|
|
}
|
|
|
|
Notify($"Finish, goto {_source} slot {_sourceSlot + 1} with {(_blade == RobotArmEnum.Blade1 ? "Blade1" : "Blade2")}");
|
|
|
|
return Result.DONE;
|
|
}
|
|
|
|
private void RobotGoto(int id, RobotBaseDevice robot, ModuleName source, int slot, RobotArmEnum hand, int timeout)
|
|
{
|
|
Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
|
|
{
|
|
Notify($"Goto {source} {slot + 1} use {(hand == RobotArmEnum.Blade1 ? "Blade1" : "Blade2")}");
|
|
List<object> para = new List<object>() { "GoTo", hand, source, slot, _EX == "EX" ? false : true , _UP == "UP" ? false : true };
|
|
if (!robot.GoTo(para.ToArray()))
|
|
{
|
|
return false;
|
|
}
|
|
return true;
|
|
}, () =>
|
|
{
|
|
if (robot.IsReady())
|
|
return true;
|
|
|
|
return false;
|
|
}, timeout * 1000);
|
|
|
|
if (ret.Item1)
|
|
{
|
|
if (ret.Item2 == Result.FAIL)
|
|
{
|
|
Stop($"Goto failed, error {robot.ErrorCode}");
|
|
throw (new RoutineFaildException());
|
|
}
|
|
else if (ret.Item2 == Result.TIMEOUT) //timeout
|
|
{
|
|
Stop(string.Format("timeout, can not complete in {0} seconds", timeout));
|
|
throw (new RoutineFaildException());
|
|
}
|
|
else
|
|
throw (new RoutineBreakException());
|
|
}
|
|
}
|
|
}
|
|
}
|