public override Result FluidSymbolLines(List <CrdSymbolLine> symbolLinesCrdData, GearValveFluidSymbolLinesPrm fluidPrm, double acc, double offsetX = 0, double offsetY = 0) { Result ret = Result.OK; //如果不是CV模式,则要把坐标都转换到阀上 if (this.RunMode != ValveRunMode.Look) { PointD offset = new PointD(offsetX, offsetY); foreach (var item in symbolLinesCrdData) { if (item.Type == 0) { item.Points[0] = item.Points[0].ToNeedle(this.ValveType) + offset; item.Points[1] = item.Points[1].ToNeedle(this.ValveType) + offset; } else { item.Points[0] = item.Points[0].ToNeedle(this.ValveType) + offset; item.Points[1] = item.Points[1].ToNeedle(this.ValveType) + offset; item.Points[2] = item.Points[2].ToNeedle(this.ValveType) + offset; } } } //解析复合线段为对应的插补指令 List <ICrdable> crdList = new List <ICrdable>(); for (int i = 0; i < symbolLinesCrdData.Count; i++) { //如果是直线,则先要判断高度是否一致,一致的话添加一个两轴直线插补,不一致的话要先添加一个BufGear插补 if (symbolLinesCrdData[i].Type == 0) { if (Machine.Instance.Robot.AxisZ.Pos != symbolLinesCrdData[i].EndZ) { CrdXYGear crdXYGear = new CrdXYGear() { GearAxis = Machine.Instance.Robot.AxisZ, DeltaPulse = Machine.Instance.Robot.AxisZ.ConvertPos2Card(symbolLinesCrdData[i].EndZ) }; crdList.Add(crdXYGear); } CrdLnXY crd = new CrdLnXY() { EndPosX = symbolLinesCrdData[i].Points[1].X, EndPosY = symbolLinesCrdData[i].Points[1].Y, Vel = fluidPrm.Vel, Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc, VelEnd = (i != symbolLinesCrdData.Count - 1 ? 0 : fluidPrm.Vel), }; crdList.Add(crd); } //如果是圆弧,则要先添加一个BufGear插补,再添加一个圆弧插补 else if (symbolLinesCrdData[i].Type == 1) { CrdXYGear crdXYGear = new CrdXYGear() { GearAxis = Machine.Instance.Robot.AxisR, DeltaPulse = Machine.Instance.Robot.AxisR.ConvertPos2Card(symbolLinesCrdData[i].TrackSweep) }; crdList.Add(crdXYGear); double r = Math.Sqrt(Math.Pow(symbolLinesCrdData[i].Points[0].X - symbolLinesCrdData[i].Points[1].X, 2) + Math.Pow(symbolLinesCrdData[i].Points[0].Y - symbolLinesCrdData[i].Points[1].Y, 2)); CrdArcXYR crd = new CrdArcXYR { EndPosX = symbolLinesCrdData[i].Points[2].X, EndPosY = symbolLinesCrdData[i].Points[2].Y, R = r, Clockwise = (short)symbolLinesCrdData[i].Clockwise, Vel = fluidPrm.ArcSpeed, Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc, VelEnd = (i != symbolLinesCrdData.Count - 1 ? 0 : fluidPrm.Vel), }; if (Math.Abs(symbolLinesCrdData[i].EndAngle) > 180) { crd.R = crd.R * -1; } crdList.Add(crd); } } //计算关胶位置 PointD stopSprayPos = this.CalculateStopSprayPos(symbolLinesCrdData, fluidPrm.StopSprayDistance); //终点位置 PointD endPos = new PointD(); //如果是Wet模式 if (this.RunMode == ValveRunMode.Wet) { //得到终点位置 if (symbolLinesCrdData.Last().Type == 0) { endPos = symbolLinesCrdData.Last().Points[1]; } else { endPos = symbolLinesCrdData.Last().Points[2]; } endPos.X = Math.Round(endPos.X, 3); endPos.Y = Math.Round(endPos.Y, 3); //如果线的终点和关胶点不一致,则可以启用二维比较 if (endPos != stopSprayPos) { this.CmpStop(); this.Cmp2dStart(stopSprayPos); } else { this.Spraying(); } } //移动前的出胶延时 Thread.Sleep(TimeSpan.FromSeconds(fluidPrm.StartSprayDelay)); //开始移动 CommandMoveTrc command = new CommandMoveTrc( Machine.Instance.Robot.AxisX, Machine.Instance.Robot.AxisY, Machine.Instance.Robot.TrcPrm, crdList) { 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); if (!ret.IsOk) { return(ret); } if (this.RunMode == ValveRunMode.Wet) { //如果提前关胶距离是0,就直接关胶 if (fluidPrm.StopSprayDistance == 0) { this.SprayOff(); } } //在终点延时 Thread.Sleep(TimeSpan.FromSeconds(fluidPrm.EndPosDelay)); //if (this.RunMode != ValveRunMode.Look) //{ // //下压 // ret = Machine.Instance.Robot.MoveIncZAndReply(-fluidPrm.PressDistance, fluidPrm.PressVel, fluidPrm.PressAcc); // if (!ret.IsOk) // return ret; // //下压保持时间 // Thread.Sleep(TimeSpan.FromSeconds(fluidPrm.PressTime)); // //抬起 // ret = Machine.Instance.Robot.MoveIncZAndReply(fluidPrm.RaiseDistance, fluidPrm.RaiseVel, fluidPrm.RaiseAcc); //} //开始回走 //如果是Wet模式或者Dry模式 if (fluidPrm.BacktrackDistance == 0 || fluidPrm.BacktrackDistance.Equals(0)) { } else { if (this.RunMode == ValveRunMode.Wet || this.RunMode == ValveRunMode.Dry) { //回抬 ret = Machine.Instance.Robot.MoveIncZAndReply(fluidPrm.BacktrackGap, fluidPrm.BacktrackSpeed, acc); //计算回走终点位置 PointD backTrackPoint = CalculateBackTrackPos(symbolLinesCrdData, fluidPrm.BacktrackDistance); //执行回走 List <ICrdable> crdXYZList = new List <ICrdable>(); if (symbolLinesCrdData.Last().Type == 0) //直线 { CrdXYGear crdXYGear = new CrdXYGear() { GearAxis = Machine.Instance.Robot.AxisZ, DeltaPulse = Machine.Instance.Robot.AxisZ.ConvertPos2Card(fluidPrm.BacktrackEndGap - fluidPrm.BacktrackGap) }; crdXYZList.Add(crdXYGear); CrdLnXY crd = new CrdLnXY() { EndPosX = backTrackPoint.X, EndPosY = backTrackPoint.Y, Vel = fluidPrm.BacktrackSpeed, Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc, VelEnd = 0, }; crdXYZList.Add(crd); } else //圆弧 { CrdXYGear crdXYZGear = new CrdXYGear() { GearAxis = Machine.Instance.Robot.AxisZ, DeltaPulse = Machine.Instance.Robot.AxisZ.ConvertPos2Card(fluidPrm.BacktrackEndGap - fluidPrm.BacktrackGap) }; crdXYZList.Add(crdXYZGear); CrdXYGear crdXYRGear = new CrdXYGear() { GearAxis = Machine.Instance.Robot.AxisR, DeltaPulse = Machine.Instance.Robot.AxisR.ConvertPos2Card(-symbolLinesCrdData.Last().TrackSweep *fluidPrm.BacktrackDistance / 100) }; crdXYZList.Add(crdXYRGear); double r = Math.Sqrt(Math.Pow(symbolLinesCrdData.Last().Points[0].X - symbolLinesCrdData.Last().Points[1].X, 2) + Math.Pow(symbolLinesCrdData.Last().Points[0].Y - symbolLinesCrdData.Last().Points[1].Y, 2)); CrdArcXYR crd = new CrdArcXYR { EndPosX = backTrackPoint.X, EndPosY = backTrackPoint.Y, R = r, Clockwise = (short)symbolLinesCrdData.Last().Clockwise == (short)0 ? (short)1 : (short)0, Vel = fluidPrm.BacktrackSpeed, Acc = acc, //Machine.Instance.Robot.DefaultPrm.WeightAcc, VelEnd = 0, }; if (Math.Abs(symbolLinesCrdData.Last().EndAngle *fluidPrm.BacktrackDistance / 100) > 180) { crd.R = crd.R * -1; } crdXYZList.Add(crd); } CommandMoveTrc command2 = new CommandMoveTrc( Machine.Instance.Robot.AxisX, Machine.Instance.Robot.AxisY, Machine.Instance.Robot.TrcPrm, crdXYZList) { EnableINP = Machine.Instance.Robot.DefaultPrm.EnableINP }; Machine.Instance.Robot.Fire(command2); ret = Machine.Instance.Robot.WaitCommandReply(command2); } } //结束回走 if (this.RunMode == ValveRunMode.Wet || this.RunMode == ValveRunMode.Dry) { //抬起到回位高度 ret = Machine.Instance.Robot.MoveIncZAndReply(fluidPrm.BackGap, fluidPrm.Vel, acc); } return(ret); }
public override Result FluidSymbolLines(List <CrdSymbolLine> SymbolLinesCrdData, GearValveFluidSymbolLinesPrm fluidPrm, double acc, double offsetX = 0, double offsetY = 0) { throw new NotImplementedException(); }