private short AddCrd2Space()
{
short rtn = 0;
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;
rtn = this.AddCrdArcXYC(crdArcXYC);
break;
case CrdType.ArcXYR:
CrdArcXYR crdArcXYR = crd as CrdArcXYR;
rtn = this.AddCrdArcXYR(crdArcXYR);
break;
case CrdType.BufIO:
rtn = this.AddCrdBufIO(crd as CrdBufIO);
break;
case CrdType.Delay:
rtn = this.AddCrdDelay(crd as CrdDelay);
break;
case CrdType.LnXY:
CrdLnXY crdLnXY = crd as CrdLnXY;
rtn = this.AddCrdLnXY(crdLnXY);
break;
case CrdType.XYGear:
CrdXYGear crdXYGear = crd as CrdXYGear;
rtn = this.AddXYGear(crdXYGear);
break;
case CrdType.BufMove:
rtn = this.AddBufMove(crd as CrdBufMove);
break;
}
}
else
{
break;
}
}
return(rtn);
}
private short AddCrdArcXYC(CrdArcXYC crd)
{
short rtn = this.card.Executor.AddCrdArcXYC(
this.card.CardId,
this.TrcPrm.CsId,
this.Axes[0].ConvertPos2Card(crd.EndPosX),
this.Axes[1].ConvertPos2Card(crd.EndPosY),
this.Axes[0].ConvertPos2Card(crd.CenterX),
this.Axes[1].ConvertPos2Card(crd.CenterY),
crd.Clockwise,
this.Axes[0].ConvertVel2Card(crd.Vel),
crd.Acc,
this.Axes[0].ConvertVel2Card(crd.VelEnd));
return(rtn);
}
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.LnXYR:
CrdLnXYR crdLnXYA = crd as CrdLnXYR;
this.AddCrdLnXYA(crdLnXYA);
break;
}
}
else
{
break;
}
}
}
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;
}
}
else
{
//没有插补数据了一次性将前瞻缓存区数据塞到插补缓存区
this.card.Executor.GT_CrdData(this.card.CardId, this.InitLoodPrm.crd, System.IntPtr.Zero, this.InitLoodPrm.fifo);
break;
}
}
}
private short addCrd(ICrdable crd)
{
short rtn = 0;
switch (crd.Type)
{
case CrdType.ArcXYC:
CrdArcXYC crdArcXYC = crd as CrdArcXYC;
rtn = this.addCrdArcXYC(crdArcXYC);
break;
case CrdType.ArcXYR:
CrdArcXYR crdArcXYR = crd as CrdArcXYR;
rtn = this.addCrdArcXYR(crdArcXYR);
break;
case CrdType.BufIO:
rtn = this.addCrdBufIO(crd as CrdBufIO);
break;
case CrdType.Delay:
rtn = this.addCrdDelay(crd as CrdDelay);
break;
case CrdType.LnXY:
CrdLnXY crdLnXY = crd as CrdLnXY;
rtn = this.addCrdLnXY(crdLnXY);
break;
case CrdType.XYGear:
CrdXYGear crdXYGear = crd as CrdXYGear;
rtn = this.addXYGear(crdXYGear);
break;
case CrdType.BufMove:
rtn = this.addBufMove(crd as CrdBufMove);
break;
}
return(rtn);
}
/// <summary>
/// 根据轨迹集合生成插补列表
/// </summary>
/// <param name="traces"></param>
/// <param name="vels"></param>
/// <param name="acc"></param>
/// <returns></returns>
public static List <ICrdable> GetCrdsBy(List <TraceBase> traces, double[] vels, double acc)
{
List <ICrdable> crdList = new List <ICrdable>();
for (int i = 0; i < traces.Count; i++)
{
TraceBase trace = traces[i];
if (trace is TraceLine)
{
CrdLnXY crd = new CrdLnXY()
{
EndPosX = trace.End.X,
EndPosY = trace.End.Y,
Vel = vels[i],
Acc = acc,
VelEnd = i == (traces.Count - 1) ? 0 : vels[i + 1]
};
crdList.Add(crd);
}
else if (trace is TraceArc)
{
CrdArcXYC crd = new CrdArcXYC()
{
EndPosX = trace.End.X,
EndPosY = trace.End.Y,
CenterX = (trace as TraceArc).Center.X - trace.Start.X,
CenterY = (trace as TraceArc).Center.Y - trace.Start.Y,
Clockwise = (short)((trace as TraceArc).Degree > 0 ? 1 : 0),
Vel = vels[i],
Acc = acc,
VelEnd = i == (traces.Count - 1) ? 0 : vels[i + 1]
};
crdList.Add(crd);
}
}
return(crdList);
}
public override Result FluidArc(PointD accStartPos, PointD arcStartPos, PointD arcEndPos, PointD decEndPos, PointD center, short clockwize, double vel, PointD[] points, double intervalSec, 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>();
//加速段-直线
CrdLnXY crdAcc = new CrdLnXY()
{
EndPosX = arcStartPos.X,
EndPosY = arcStartPos.Y,
Vel = vel,
Acc = acc,
VelEnd = vel
};
crdList.Add(crdAcc);
//匀速段-圆
for (int i = 1; i < points.Length; i++)
{
CrdArcXYC crdArc = new CrdArcXYC()
{
EndPosX = points[i].X,
EndPosY = points[i].Y,
CenterX = center.X - points[i - 1].X,
CenterY = center.Y - points[i - 1].Y,
Clockwise = clockwize,
Vel = vel,
Acc = acc,
VelEnd = vel
};
crdList.Add(crdArc);
}
//减速段-直线
CrdLnXY crdDec = new CrdLnXY()
{
EndPosX = decEndPos.X,
EndPosY = decEndPos.Y,
Vel = vel,
Acc = acc,
VelEnd = 0
};
crdList.Add(crdDec);
Result rtn = Result.OK;
int intervalTime = (int)(intervalSec * 1000000);
Action timeCtrlAction = () =>
{
CommandMoveTrc command = new CommandMoveTrc(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrm,
crdList);
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);
}
CommandMoveTrc command = new CommandMoveTrc(
Machine.Instance.Robot.AxisX,
Machine.Instance.Robot.AxisY,
Machine.Instance.Robot.TrcPrm,
crdList);
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);
}
