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); }