public override Result FluidLine(SvValveFludLineParam primaryLineParam, SvValveFludLineParam simulLineParam, double acc)
{
throw new NotImplementedException();
}
public override Result FluidArc(SvValveFludLineParam svValveArcParam, PointD center, short clockwize, double acc)
{
throw new NotImplementedException();
}
public override Result FluidLine(SvValveFludLineParam primaryLineParam, SvValveFludLineParam simulLineParam, double acc)
{
Result ret = Result.OK;
if (Machine.Instance.Robot.IsSimulation)
{
return(Result.OK);
}
if (primaryLineParam.endPos == null)
{
return(Result.FAILED);
}
//TODO 前瞻预处理还没加
//连续插补
List <ICrdable> crdList = new List <ICrdable>();
if (primaryLineParam.transPoints.Length > 0)
{
for (int i = 0; i < primaryLineParam.transPoints.Length; i++)
{
CrdLnXYAB crd = new CrdLnXYAB()
{
EndPosX = primaryLineParam.transPoints[i].X,
EndPosY = primaryLineParam.transPoints[i].Y,
EndPosA = simulLineParam.transPoints[i].X,
EndPosB = simulLineParam.transPoints[i].Y,
Vel = primaryLineParam.vels[i],
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = primaryLineParam.vels[i]
};
crdList.Add(crd);
}
}
if (this.valve1.RunMode == ValveRunMode.Wet)
{
//如果线的终点和关胶点不一致，则可以启用二维比较
if (primaryLineParam.endPos != primaryLineParam.stopSprayPos)
{
this.CmpStop();
this.Cmp2dStart(primaryLineParam.stopSprayPos);
}
else
{
this.Spraying();
}
}
//在起始位置开胶后，延时移动的时间
Thread.Sleep(TimeSpan.FromSeconds(primaryLineParam.startPosDelay));
//开始移动
if (primaryLineParam.transPoints.Length > 0)
{
CommandMoveTrc4Axis command = new CommandMoveTrc4Axis(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.AxisA,
Machine.Instance.Robot.AxisB,
Machine.Instance.Robot.TrcPrm4Axis,
crdList,
(int)Machine.Instance.Setting.CardSelect)
{
EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
};
Log.Dprint(string.Format("Fluid Line[2d ctrl]-> maxErr:{0}", this.svValve1.Prm.Cmp2dMaxErr));
Machine.Instance.Robot.Fire(command);
ret = Machine.Instance.Robot.WaitCommandReply(command);
}
else
{
ret = Machine.Instance.Robot.MovePosXYABAndReply(primaryLineParam.endPos, simulLineParam.endPos, primaryLineParam.vels[0], acc, (int)Machine.Instance.Setting.CardSelect);
}
//如果提前关胶距离是0，就手动关胶
if (primaryLineParam.endPos == primaryLineParam.stopSprayPos)
{
this.SprayOff();
}
//在终点延时
Thread.Sleep(TimeSpan.FromSeconds(primaryLineParam.backTrackDelay));
//抬高
ret = Machine.Instance.Robot.MoveIncZAndReply(primaryLineParam.backTrackGap, primaryLineParam.backTrackVel, acc);
if (!ret.IsOk)
{
return(ret);
}
//开始回走
List <ICrdable> backCrdList = new List <ICrdable>();
if (primaryLineParam.backTransPoints.Length > 0)
{
for (int i = 0; i < primaryLineParam.backTransPoints.Length; i++)
{
CrdLnXYAB crd = new CrdLnXYAB()
{
EndPosX = primaryLineParam.backTransPoints[i].X,
EndPosY = primaryLineParam.backTransPoints[i].Y,
EndPosA = simulLineParam.transPoints[i].X,
EndPosB = simulLineParam.transPoints[i].Y,
Vel = primaryLineParam.vels[i],
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = primaryLineParam.vels[i]
};
backCrdList.Add(crd);
}
}
if (primaryLineParam.backTransPoints.Length > 0)
{
CommandMoveTrc4Axis command = new CommandMoveTrc4Axis(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.AxisA,
Machine.Instance.Robot.AxisB,
Machine.Instance.Robot.TrcPrm4Axis,
backCrdList,
(int)Machine.Instance.Setting.CardSelect)
{
EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
};
Machine.Instance.Robot.Fire(command);
ret = Machine.Instance.Robot.WaitCommandReply(command);
}
else
{
ret = Machine.Instance.Robot.MovePosXYAndReply(primaryLineParam.backTrackPos, primaryLineParam.vels[0], acc);
}
return(ret);
}
public override Result FluidArc(SvValveFludLineParam svValveArcParam, PointD center, short clockwize, double acc)
{
Result ret = Result.OK;
if (Machine.Instance.Robot.IsSimulation)
{
return(Result.OK);
}
if (svValveArcParam.endPos == null)
{
return(Result.FAILED);
}
if (this.valve1.RunMode == ValveRunMode.Wet)
{
//如果线的终点和关胶点不一致，则可以启用二维比较
if (svValveArcParam.endPos != svValveArcParam.stopSprayPos)
{
this.CmpStop();
this.Cmp2dStart(svValveArcParam.stopSprayPos);
}
else
{
this.Spraying();
}
}
//在起始位置开胶后，延时移动的时间
Thread.Sleep(TimeSpan.FromSeconds(svValveArcParam.startPosDelay));
//开始移动
ret = Machine.Instance.Robot.MoveArcAndReply(svValveArcParam.endPos, center, clockwize, svValveArcParam.vels[0]);
//如果提前关胶距离是0，就手动关胶
if (svValveArcParam.endPos == svValveArcParam.stopSprayPos)
{
this.SprayOff();
}
//在终点延时
Thread.Sleep(TimeSpan.FromSeconds(svValveArcParam.backTrackDelay));
//抬高
ret = Machine.Instance.Robot.MoveIncZAndReply(svValveArcParam.backTrackGap, svValveArcParam.backTrackVel, acc);
if (!ret.IsOk)
{
return(ret);
}
//开始回走
short backClockWise = 0;
if (clockwize == 0)
{
backClockWise = 1;
}
else
{
backClockWise = 0;
}
ret = Machine.Instance.Robot.MoveArcAndReply(svValveArcParam.backTrackPos, center, backClockWise, svValveArcParam.backTrackVel);
return(ret);
}
public override Result FluidLine(SvValveFludLineParam svValveLineParam, double acc)
{
Result ret = Result.OK;
if (Machine.Instance.Robot.IsSimulation)
{
return(Result.OK);
}
if (svValveLineParam.endPos == null)
{
return(Result.FAILED);
}
//TODO 前瞻预处理还没加
//连续插补
List <ICrdable> crdList = new List <ICrdable>();
if (!svValveLineParam.IsTraceMode)
{
// 多段线模式
if (svValveLineParam.transPoints.Length > 0)
{
for (int i = 0; i < svValveLineParam.transPoints.Length; i++)
{
CrdLnXY crd = new CrdLnXY()
{
EndPosX = svValveLineParam.transPoints[i].X,
EndPosY = svValveLineParam.transPoints[i].Y,
Vel = svValveLineParam.vels[i],
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = svValveLineParam.vels[i]
};
crdList.Add(crd);
}
}
}
else
{
// 线+圆弧轨迹模式
crdList = GetCrdsBy(svValveLineParam.transTraces, svValveLineParam.vels, acc);
}
if (this.RunMode == ValveRunMode.Wet)
{
//如果线的终点和关胶点不一致，则可以启用二维比较
if (svValveLineParam.endPos != svValveLineParam.stopSprayPos)
{
this.CmpStop();
this.Cmp2dStart(svValveLineParam.stopSprayPos);
}
else
{
this.Spraying();
}
}
//在起始位置开胶后，延时移动的时间
Thread.Sleep(TimeSpan.FromSeconds(svValveLineParam.startPosDelay));
//开始移动
if (!svValveLineParam.IsTraceMode)
{
if (svValveLineParam.transPoints.Length > 0)
{
CommandMoveTrcCmp2d command = new CommandMoveTrcCmp2d(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrmWeight,
crdList, this.Chn, svValveLineParam.transPoints.ToList())
{
EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
};
Log.Dprint(string.Format("Fluid Line[2d ctrl]-> maxErr:{0}", this.Prm.Cmp2dMaxErr));
Machine.Instance.Robot.Fire(command);
ret = Machine.Instance.Robot.WaitCommandReply(command);
}
else
{
ret = Machine.Instance.Robot.MovePosXYAndReply(svValveLineParam.endPos, svValveLineParam.vels[0], acc);
}
}
else
{
ret = Machine.Instance.Robot.MoveTrcXYReply(crdList);
}
//如果提前关胶距离是0，就手动关胶
if (svValveLineParam.endPos == svValveLineParam.stopSprayPos)
{
this.SprayOff();
}
//在终点延时
Thread.Sleep(TimeSpan.FromSeconds(svValveLineParam.backTrackDelay));
//抬高
ret = Machine.Instance.Robot.MoveIncZAndReply(svValveLineParam.backTrackGap, svValveLineParam.backTrackVel, acc);
if (!ret.IsOk)
{
return(ret);
}
//开始回走
//连续插补
List <ICrdable> backCrdList = new List <ICrdable>();
if (!svValveLineParam.IsTraceMode)
{
if (svValveLineParam.backTransPoints.Length > 0)
{
for (int i = 0; i < svValveLineParam.backTransPoints.Length; i++)
{
CrdLnXY crd = new CrdLnXY()
{
EndPosX = svValveLineParam.backTransPoints[i].X,
EndPosY = svValveLineParam.backTransPoints[i].Y,
Vel = svValveLineParam.backTrackVel,
Acc = acc,
VelEnd = svValveLineParam.backTrackVel
};
backCrdList.Add(crd);
}
}
}
else
{
// 线+圆弧轨迹模式
backCrdList = GetCrdsBy(svValveLineParam.backTransTraces, svValveLineParam.backTrackVel, acc);
}
if (!svValveLineParam.IsTraceMode)
{
if (svValveLineParam.backTransPoints.Length > 0)
{
CommandMoveTrcCmp2d command = new CommandMoveTrcCmp2d(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrmWeight,
backCrdList, this.Chn, svValveLineParam.backTransPoints.ToList())
{
EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
};
Machine.Instance.Robot.Fire(command);
ret = Machine.Instance.Robot.WaitCommandReply(command);
}
else
{
ret = Machine.Instance.Robot.MovePosXYAndReply(svValveLineParam.backTrackPos, svValveLineParam.vels[0], acc);
}
}
else
{
ret = Machine.Instance.Robot.MoveTrcXYReply(backCrdList);
}
return(ret);
}
