private static double getLimit(Coordinate pos)
{
double limtZ = pos.Z > limitz ? pos.Z + 0.2 : limitz;
limtZ = limtZ >= ScaraRobot.GetInstance().CurrentPosition.Z ? limtZ : ScaraRobot.GetInstance().CurrentPosition.Z + 0.2;
return(limtZ);
}
/// <summary>
///按位获取输出信号
/// </summary>
/// <param name="nIndex"></param>
/// <returns></returns>
public override bool ReadIoOutBit(int nIndex)
{
if (!ScaraRobot.GetInstance().IsInit)
{
return(false);
}
return(1 == BitOperat.GetBit32(ScaraRobot.GetInstance().dOut, (byte)nIndex));
}
/// <summary>
/// 按位输出信号
/// </summary>
/// <param name="nIndex"></param>
/// <param name="bBit"></param>
/// <returns></returns>
public override bool WriteIoBit(int nIndex, bool bBit)
{
if (!ScaraRobot.GetInstance().IsInit)
{
return(false);
}
ScaraRobot.GetInstance().SetOutput(nIndex, bBit);
return(true);
}
/// <summary>
///按位获取输入信号
/// </summary>
/// <param name="nIndex"></param>
/// <returns></returns>
public override bool ReadIoInBit(int nIndex)
{
if (!ScaraRobot.GetInstance().IsInit)
{
return(false);
}
UInt32 t = ScaraRobot.GetInstance().dIn;
return(BitOperat.GetBit32(ScaraRobot.GetInstance().dIn, nIndex) == 1);
// return true;
}
/// <summary>
/// 初始化Epson 4轴机械手
/// </summary>
public static void InitEpson4Robot()
{
if (IsHaveEpson == false)
{
return;
}
Task.Run(() =>
{
IOMgr.GetInstace().WriteIoBit("机器人程序启动", true);
Thread.Sleep(10000);
ScaraRobot.GetInstance().Init(out string msg);
});
}
public static WaranResult JumpInPos(this ScaraRobot scaraRobot, Coordinate coordinate, HandDirection direction, bool bCheckHandleSys = false, bool bmauanl = false, int nTimeout = 20000)
{
logger.Info($"JumpInPos start pos{coordinate.X}，{coordinate.Y}，{coordinate.Z}，{coordinate.U}");
try
{
WaranResult waran = WaranResult.Failture;
bool bcmd = ScaraRobot.GetInstance().Jump(coordinate, direction, getLimit(coordinate));
DoWhile doWhile = new DoWhile((nTimeed, doWhile2, bmanual, objs) =>
{
double dfine = 0.05;
bool bInPosX = Math.Abs(ScaraRobot.GetInstance().CurrentPosition.X - coordinate.X) < dfine;
bool bInPosY = Math.Abs(ScaraRobot.GetInstance().CurrentPosition.Y - coordinate.Y) < dfine;
// bool bInPosZ = Math.Abs(ScaraRobot.GetInstance().CurrentPosition.Z - coordinate.Z) < dfine;
bool bInPosU = Math.Abs(ScaraRobot.GetInstance().CurrentPosition.U - coordinate.U) < 0.1;
bool bInPos = bInPosX && bInPosY && true && bInPosU;
if (!bmanual)
{
if (GlobalVariable.g_StationState == StationState.StationStatePause)
{
logger.Info($"JumpInPos 程序状态： {GlobalVariable.g_StationState }");
ScaraRobot.GetInstance().SetStopActionFlag();
return(WaranResult.CheckAgain);
}
else if (GlobalVariable.g_StationState == StationState.StationStateRun)
{
Thread.Sleep(50);
bInPosX = Math.Abs(ScaraRobot.GetInstance().CurrentPosition.X - coordinate.X) < dfine;
bInPosY = Math.Abs(ScaraRobot.GetInstance().CurrentPosition.Y - coordinate.Y) < dfine;
// bInPosZ = Math.Abs(ScaraRobot.GetInstance().CurrentPosition.Z - coordinate.Z) < dfine;
bInPosU = Math.Abs(ScaraRobot.GetInstance().CurrentPosition.U - coordinate.U) < 0.1;
bInPos = bInPosX && bInPosY && true && bInPosU;
if (!bInPos)
{
Coordinate coordinatetemp = ScaraRobot.GetInstance().GetCurrentImmediately();
bInPosX = Math.Abs(coordinatetemp.X - coordinate.X) < dfine;
bInPosY = Math.Abs(coordinatetemp.Y - coordinate.Y) < dfine;
// bInPosZ = Math.Abs(coordinatetemp.Z - coordinate.Z) < dfine;
bInPosU = Math.Abs(coordinatetemp.U - coordinate.U) < 0.1;
bInPos = bInPosX && bInPosY && true && bInPosU;
}
logger.Info($"JumpInPos 程序状态：{GlobalVariable.g_StationState} bInPos {bInPos}, 目标位置 {coordinate.X}，{coordinate.Y}，{coordinate.Z}，{coordinate.U}" +
$"实际位置： {ScaraRobot.GetInstance().CurrentPosition.X}，{ScaraRobot.GetInstance().CurrentPosition.Y}，{ScaraRobot.GetInstance().CurrentPosition.Z}，{ScaraRobot.GetInstance().CurrentPosition.U}");
if (bInPos)
{
return(WaranResult.Run);
}
else if (ScaraRobot.GetInstance().InPos&& !bInPos)
{
ScaraRobot.GetInstance().ReasetStopActionFlag();
ScaraRobot.GetInstance().Jump(coordinate, direction, getLimit(coordinate));
return(WaranResult.CheckAgain);
}
}
}
if (nTimeed > nTimeout)
{
logger.Info($"JumpInPos 程序状态：{GlobalVariable.g_StationState} bInPos {bInPos}, 超时， 目标位置 {coordinate.X}，{coordinate.Y}，{coordinate.Z}，{coordinate.U}" +
$"实际位置： {ScaraRobot.GetInstance().CurrentPosition.X}，{ScaraRobot.GetInstance().CurrentPosition.Y}，{ScaraRobot.GetInstance().CurrentPosition.Z}，{ScaraRobot.GetInstance().CurrentPosition.U}");
ScaraRobot.GetInstance().SetStopActionFlag();
ScaraRobot.GetInstance().SetStopActionFlag();
ScaraRobot.GetInstance().ReasetStopActionFlag();
logger.Info($"JumpInPos 程序状态：{GlobalVariable.g_StationState} bInPos {bInPos}, 复位， 目标位置 {coordinate.X}，{coordinate.Y}，{coordinate.Z}，{coordinate.U}" +
$"实际位置： {ScaraRobot.GetInstance().CurrentPosition.X}，{ScaraRobot.GetInstance().CurrentPosition.Y}，{ScaraRobot.GetInstance().CurrentPosition.Z}，{ScaraRobot.GetInstance().CurrentPosition.U}");
return(WaranResult.TimeOut);
}
if (ScaraRobot.GetInstance().InPos&& bInPos)
{
return(WaranResult.Run);
}
else
{
return(WaranResult.CheckAgain);
}
}
, 30000);
return(doWhile.doSomething2(null, doWhile, bmauanl, null));
}
catch (Exception ex)
{
ScaraRobot.GetInstance().SetStopActionFlag();
ScaraRobot.GetInstance().SetStopActionFlag();
ScaraRobot.GetInstance().ReasetStopActionFlag();
throw ex;
}
}