private short AddCrdLnXYAB(CrdLnXYAB crd)
{
return(this.card.Executor.AddCrdLnXYAB(
this.card.CardId,
this.TrcPrm.CsId,
this.Axes[0].ConvertPos2Card(crd.EndPosX),
this.Axes[1].ConvertPos2Card(crd.EndPosY),
this.Axes[2].ConvertPos2Card(crd.EndPosA),
this.Axes[3].ConvertPos2Card(crd.EndPosB),
this.Axes[0].ConvertVel2Card(crd.Vel),
crd.Acc,
this.Axes[0].ConvertVel2Card(crd.VelEnd)));
}
private void AddCrd2Space()
{
this.card.Executor.GetCrdSpace(this.card.CardId, this.TrcPrm.CsId, out space);
int count = (space < MAX_ONCE) ? space : MAX_ONCE;
for (int i = 0; i < count; i++)
{
if (this.crdQueue.Count > 0)
{
ICrdable crd = this.crdQueue.Dequeue();
switch (crd.Type)
{
case CrdType.ArcXYC:
CrdArcXYC crdArcXYC = crd as CrdArcXYC;
this.AddCrdArcXYC(crdArcXYC);
break;
case CrdType.ArcXYR:
CrdArcXYR crdArcXYR = crd as CrdArcXYR;
this.AddCrdArcXYR(crdArcXYR);
break;
case CrdType.BufIO:
this.AddCrdBufIO(crd as CrdBufIO);
break;
case CrdType.Delay:
this.AddCrdDelay(crd as CrdDelay);
break;
case CrdType.LnXY:
CrdLnXY crdLnXY = crd as CrdLnXY;
this.AddCrdLnXY(crdLnXY);
break;
case CrdType.LnXYAB:
CrdLnXYAB crdLnXYAB = crd as CrdLnXYAB;
this.AddCrdLnXYAB(crdLnXYAB);
break;
}
}
else
{
break;
}
}
}
public override Result FluidArc(PointD accStartPos, PointD arcStartPos, PointD arcEndPos, PointD decEndPos, PointD center, short clockwize, double vel, PointD[] points, double intervalSec, PointD simulStartPos, PointD[] simulPoints, double acc)
{
if (Machine.Instance.Robot.IsSimulation)
{
return(Result.OK);
}
if (points == null)
{
return(Result.FAILED);
}
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet || Machine.Instance.Valve1.RunMode == ValveRunMode.Dry)
{
accStartPos = accStartPos.ToNeedle(this.valve1.Key);
arcStartPos = arcStartPos.ToNeedle(this.valve1.Key);
arcEndPos = arcEndPos.ToNeedle(this.valve1.Key);
decEndPos = decEndPos.ToNeedle(this.valve1.Key);
center = center.ToNeedle(this.valve1.Key);
for (int i = 0; i < points.Length; i++)
{
points[i] = points[i].ToNeedle(this.valve1.Key);
}
}
double interval = 0;
if (points.Length >= 2)
{
interval = points[0].DistanceTo(points[1]);
}
double accTime = this.CalcAccTime(accStartPos, arcStartPos, vel, acc * 1000);
//连续差补
List <ICrdable> crdList = new List <ICrdable>();
//加速段-直线
CrdLnXYAB crdAcc = new CrdLnXYAB()
{
EndPosX = arcStartPos.X,
EndPosY = arcStartPos.Y,
EndPosA = simulStartPos.X,
EndPosB = simulStartPos.Y,
Vel = vel,
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = vel
};
crdList.Add(crdAcc);
//匀速段-圆
for (int i = 1; i < points.Length; i++)
{
CrdArcXYABC crdArc = new CrdArcXYABC()
{
EndPosX = points[i].X,
EndPosY = points[i].Y,
EndPosA = simulPoints[i].X,
EndPosB = simulPoints[i].Y,
CenterX = center.X - points[i - 1].X,
CenterY = center.Y - points[i - 1].Y,
Clockwise = clockwize,
Vel = vel,
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = vel
};
crdList.Add(crdArc);
}
//减速段-直线
CrdLnXYAB crdDec = new CrdLnXYAB()
{
EndPosX = decEndPos.X,
EndPosY = decEndPos.Y,
EndPosA = simulPoints[simulPoints.Length - 1].X,
EndPosB = simulPoints[simulPoints.Length - 1].Y,
Vel = vel,
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = 0
};
crdList.Add(crdDec);
Result rtn = Result.OK;
int intervalTime = (int)(intervalSec * 1000000);
Action timeCtrlAction = () =>
{
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);
command.Starting += () =>
{
ValveSprayServer.Instance.StartSprayEvent.Set();
};
ValveSprayServer.Instance.SleepSpan = TimeSpan.FromSeconds(accTime);
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
ValveSprayServer.Instance.SprayAction = () =>
{
this.SprayCycle((short)points.Count(), (short)(intervalTime - this.jtValve1.Prm.OnTime));
};
}
else
{
ValveSprayServer.Instance.SprayAction = null;
}
Log.Dprint(string.Format("Fluid Line[time ctrl]-> accTime:{0}, intervalTime:{1}, onTime:{2}, offTime{3}.",
accTime, intervalTime, this.jtValve1.Prm.OnTime, this.jtValve1.Prm.OffTime));
Machine.Instance.Robot.Fire(command);
rtn = Machine.Instance.Robot.WaitCommandReply(command);
};
Action cmp2dAction = () =>
{
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
SimulCmp2dStart((short)Cmp2dSrcType.编码器, (short)this.jtValve1.Prm.Cmp2dMaxErr, points);
}
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);
Log.Dprint(string.Format("Fluid Line[2d ctrl]-> maxErr:{0}, interval:{1}", this.jtValve1.Prm.Cmp2dMaxErr, interval));
Machine.Instance.Robot.Fire(command);
rtn = Machine.Instance.Robot.WaitCommandReply(command);
//if (this.RunMode == ValveRunMode.Wet)
//{
// Cmp2dStop();
//}
};
if (this.jtValve1.Prm.FluidMode == ValveFluidMode.时间控制优先)
{
if (intervalTime - this.jtValve1.Prm.OnTime < short.MaxValue)
{//判断脉宽是否short类型溢出，
timeCtrlAction();
}
else
{//溢出：位置比较模式
cmp2dAction();
}
}
else
{
if (interval * 1000 > this.jtValve1.Prm.Cmp2dMaxErr || points.Length < 2)
{
cmp2dAction();
}
else
{
timeCtrlAction();
}
}
return(rtn);
}
public override Result FluidLine(PointD accStartPos, PointD lineStartPos, PointD lineEndPos, PointD decEndPos, double vel, PointD[] points, double intervalSec, PointD simulStartPos, PointD[] simulPoints, double acc)
{
if (Machine.Instance.Robot.IsSimulation)
{
return(Result.OK);
}
if (points == null)
{
return(Result.FAILED);
}
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet || Machine.Instance.Valve1.RunMode == ValveRunMode.Dry)
{
accStartPos = accStartPos.ToNeedle(this.valve1.Key);
lineStartPos = lineStartPos.ToNeedle(this.valve1.Key);
lineEndPos = lineEndPos.ToNeedle(this.valve1.Key);
decEndPos = decEndPos.ToNeedle(this.valve1.Key);
for (int i = 0; i < points.Length; i++)
{
points[i] = points[i].ToNeedle(this.valve1.Key);
}
}
PointD simulDecEndPos = simulPoints[simulPoints.Length - 1];
double interval = 0;
if (points.Length >= 2)
{
interval = points[0].DistanceTo(points[1]);
}
//计算加速段时间，用于启动时间控制打胶
double accTime = this.CalcAccTime(accStartPos, lineStartPos, vel, acc * 1000);
//连续插补
List <ICrdable> crdList = new List <ICrdable>();
//加速段
CrdLnXYAB crdAcc = new CrdLnXYAB()
{
EndPosX = lineStartPos.X,
EndPosY = lineStartPos.Y,
EndPosA = simulStartPos.X,
EndPosB = simulStartPos.Y,
Vel = vel,
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = vel
};
crdList.Add(crdAcc);
//匀速段
for (int i = 1; i < points.Length; i++)
{
CrdLnXYAB crdLn = new CrdLnXYAB()
{
EndPosX = points[i].X,
EndPosY = points[i].Y,
EndPosA = simulPoints[i].X,
EndPosB = simulPoints[i].Y,
Vel = vel,
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = vel
};
crdList.Add(crdLn);
}
//减速段
CrdLnXYAB crdDec = new CrdLnXYAB()
{
EndPosX = decEndPos.X,
EndPosY = decEndPos.Y,
EndPosA = simulPoints[simulPoints.Length - 1].X,
EndPosB = simulPoints[simulPoints.Length - 1].Y,
Vel = vel,
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = 0
};
crdList.Add(crdDec);
DateTime t1 = DateTime.Now;
DateTime t2 = DateTime.Now;
Result rtn = Result.OK;
int intervalTime = (int)(intervalSec * 1000000);
//时间控制函数
Action timeCtrlAction = () =>
{
ICommandable command = null;
if (this.jtValve1.Prm.MoveMode == ValveMoveMode.单次插补)
{//单次插补
CrdLnXYAB crd = new CrdLnXYAB()
{
EndPosX = decEndPos.X,
EndPosY = decEndPos.Y,
EndPosA = simulPoints[simulPoints.Length - 1].X,
EndPosB = simulPoints[simulPoints.Length - 1].Y,
Vel = vel,
Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc,
VelEnd = vel
};
command = new CommandMoveTrc4Axis(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.AxisA,
Machine.Instance.Robot.AxisB,
Machine.Instance.Robot.TrcPrm4Axis,
crd,
() =>
{
t1 = DateTime.Now;
ValveSprayServer.Instance.StartSprayEvent.Set();
},
(int)Machine.Instance.Setting.CardSelect
)
{
EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
};
}
else
{//多次插补
command = new CommandMoveTrc4Axis(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.AxisA,
Machine.Instance.Robot.AxisB,
Machine.Instance.Robot.TrcPrm4Axis,
crdList,
() =>
{
t1 = DateTime.Now;
ValveSprayServer.Instance.StartSprayEvent.Set();
},
(int)Machine.Instance.Setting.CardSelect
)
{
EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP
};
}
//设置打胶线程启动时间
ValveSprayServer.Instance.SleepSpan = TimeSpan.FromSeconds(accTime);
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
ValveSprayServer.Instance.SprayAction = () =>
{
t2 = DateTime.Now;
this.SprayCycle((short)points.Count(), (short)(intervalTime - this.jtValve1.Prm.OnTime));
};
}
else
{
ValveSprayServer.Instance.SprayAction = null;
}
Log.Dprint(string.Format("Fluid Line[time ctrl]-> accTime:{0}, intervalTime:{1}, onTime:{2}, offTime{3}.",
accTime, intervalTime, this.jtValve1.Prm.OnTime, this.jtValve1.Prm.OffTime));
Machine.Instance.Robot.Fire(command);
rtn = Machine.Instance.Robot.WaitCommandReply(command);
Debug.WriteLine(t1.ToString("HH::mm::ss::fff"));
Debug.WriteLine(t2.ToString("HH::mm::ss::fff"));
};
//2d比较控制函数
Action cmp2dAction = () =>
{
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
CmpStop();
SimulCmp2dStart((short)Cmp2dSrcType.编码器, (short)this.jtValve1.Prm.Cmp2dMaxErr, points);
}
if (this.jtValve1.Prm.MoveMode == ValveMoveMode.单次插补)
{
rtn = Machine.Instance.Robot.MovePosXYABAndReply(decEndPos, simulDecEndPos, vel, acc, (int)Machine.Instance.Setting.CardSelect);
}
else
{
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}, interval:{1}", this.jtValve1.Prm.Cmp2dMaxErr, interval));
Machine.Instance.Robot.Fire(command);
rtn = Machine.Instance.Robot.WaitCommandReply(command);
}
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
//Cmp2dStop();
}
};
//一维比较控制函数
Action cmp1dAction = () =>
{
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
SimulCmp2dStop();
CmpStart((short)Cmp2dSrcType.编码器, points);
}
if (this.jtValve1.Prm.MoveMode == ValveMoveMode.单次插补)
{
rtn = Machine.Instance.Robot.MovePosXYABAndReply(decEndPos, simulDecEndPos, vel, acc, (int)Machine.Instance.Setting.CardSelect);
}
else
{
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}, interval:{1}", this.jtValve1.Prm.Cmp2dMaxErr, interval));
Machine.Instance.Robot.Fire(command);
rtn = Machine.Instance.Robot.WaitCommandReply(command);
}
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Wet)
{
//CmpStop();
}
};
switch (this.jtValve1.Prm.FluidMode)
{
case ValveFluidMode.一维比较优先:
cmp1dAction();
break;
case ValveFluidMode.二维比较优先:
if (interval * 1000 > this.jtValve1.Prm.Cmp2dMaxErr || points.Length < 2)
{
cmp2dAction();
}
else
{
//timeCtrlAction();
cmp1dAction();
}
break;
case ValveFluidMode.时间控制优先:
if (intervalTime - this.jtValve1.Prm.OnTime < short.MaxValue)
{ //判断脉宽是否short类型溢出，
timeCtrlAction();
}
else
{ //溢出：位置比较模式
//cmp2dAction();
cmp1dAction();
}
break;
}
return(rtn);
}
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);
}
