private Result SvValveStopSpary(Line line, LineParam param) { Result ret = Result.OK; // 抬高一段距离 Retract Distance if (param.RetractDistance > 0) { Log.Dprint("move up RetractDistance : " + param.RetractDistance); ret = Machine.Instance.Robot.MoveIncZAndReply(param.RetractDistance, param.RetractSpeed, param.RetractAccel); } if (line.RunnableModule.Mode == ModuleMode.MainMode) { ret = Machine.Instance.DualValve.SuckBack(param.SuckBackTime); } else { ret = Machine.Instance.Valve1.SuckBack(param.SuckBackTime); } if (!ret.IsOk) { return(ret); } return(ret); }
public static void DrawArc(PictureBox whereToDraw, LineParam arcInfo, bool istop, bool save) { whereToDraw.Image = (Image)Registry.FixedImage.Clone(); int tension = 25; if (istop) { tension = -25; } Image currentImage = (Image)whereToDraw.Image.Clone(); Graphics myArc = Graphics.FromImage(currentImage); Point[] pts = new Point[4]; pts[0] = arcInfo.Source; pts[1] = new Point(arcInfo.Source.X, arcInfo.Source.Y + tension); pts[2] = new Point(arcInfo.Destination.X, arcInfo.Destination.Y + tension); pts[3] = arcInfo.Destination; myArc.DrawCurve(arcInfo.LineColor, pts, 1.2F); myArc.Dispose(); whereToDraw.Image = (Image)currentImage.Clone(); if (save) { Registry.FixedImage = (Image)whereToDraw.Image.Clone(); } }
private static void ReadLineFromFile(LineParam lineParam) { Utility.PerformIOWithRetries( ctx => { LineParam param = ctx; param.Line = param.Reader.ReadLine(); }, lineParam); }
public static void UnDrawLine(PictureBox whereToDraw, LineParam lineInfo) { whereToDraw.Image = (Image)Registry.FixedImage.Clone(); Image currentImage = (Image)whereToDraw.Image.Clone(); Graphics myLine = Graphics.FromImage(currentImage); myLine.DrawLine(lineInfo.LineColor, lineInfo.Source, lineInfo.Source); myLine.Dispose(); whereToDraw.Image = (Image)currentImage.Clone(); }
public Line(LineCmd lineCmd, CoordinateCorrector coordinateCorrector) { //this.Valve = lineCmd.Valve; this.RunnableModule = lineCmd.RunnableModule; if (this.RunnableModule.Mode == ModuleMode.AssignMode1 || this.RunnableModule.Mode == ModuleMode.MainMode) { this.Valve = ValveType.Valve1; } else if (this.RunnableModule.Mode == ModuleMode.DualFallow) { this.Valve = ValveType.Both; } else { this.Valve = ValveType.Valve2; } PointD start, end; foreach (LineCoordinate line in lineCmd.LineCoordinateList) { start = coordinateCorrector.Correct(lineCmd.RunnableModule, line.Start, Executor.Instance.Program.ExecutantOriginOffset); end = coordinateCorrector.Correct(lineCmd.RunnableModule, line.End, Executor.Instance.Program.ExecutantOriginOffset); LineCoordinate newLine = new LineCoordinate(start, end); newLine.Weight = line.Weight; newLine.LineStyle = line.LineStyle; newLine.Param = lineCmd.RunnableModule.CommandsModule.Program.ProgramSettings.GetLineParam(newLine.LineStyle); newLine.LookOffsetRevs = line.LookOffsetRevs; newLine.LookOffset = line.LookOffset; newLine.Repetition = line.Repetition; lineCoordinateList.Add(newLine); lineCoordinateDic.Add(newLine, this); Log.Dprint("Line start : " + line.Start + ", real : " + start); Log.Dprint("Line end : " + line.End + ", real : " + end); } param = lineCmd.RunnableModule.CommandsModule.Program.ProgramSettings.GetLineParam(lineCmd.LineStyle); isWeightControl = lineCmd.IsWeightControl; wholeWeight = lineCmd.WholeWeight; Program = lineCmd.RunnableModule.CommandsModule.Program; this.lineCmd = lineCmd; if (lineCmd.AssociatedMeasureHeightCmd != null) { curMeasureHeightValue = lineCmd.AssociatedMeasureHeightCmd.RealHtValue; } else { curMeasureHeightValue = this.RunnableModule.MeasuredHt; } this.Tilt = this.lineCmd.Tilt; }
private PointD[] calNormalLinePoints(PointD start, PointD end, LineParam lineParam, double speed, out double intervalSec) { double lineDistance = Math.Sqrt(Math.Pow(end.X - start.X, 2) + Math.Pow(end.Y - start.Y, 2)); double totalTime = lineDistance / speed; // 计算点胶时间 (需要将微秒单位转换成秒) double dispenseTime = Machine.Instance.Valve1.SpraySec; intervalSec = dispenseTime; PointD[] points = null; // Control Mode: 控制模式,一共有四种: // 1. Pos-based spacing: 指定两滴胶水之间的间距长度值 // 2. Time-based spacing: 相邻两滴胶水喷胶开始时间的最小时间间隔,单位:秒,如果该参数值为s,本次喷胶开始时间距离上次喷胶开始时间为d, // 如果d < s,则还需要等待(s-d)秒,否则无需等待。 // 3. Total # of dots: 指定一条线上一共有几个点 // 4. No dispense: 只运动不喷胶 if (lineParam.ControlMode == LineParam.CtrlMode.POS_BASED_SPACING) { points = calFixedTotalOfDots(start, end, (int)(lineDistance / lineParam.Spacing) + 1); intervalSec = totalTime / (points.Length - 1); if (intervalSec < dispenseTime) { intervalSec = dispenseTime; points = calFixedTotalOfDots(start, end, (int)(totalTime / intervalSec) + 1); } } else if (lineParam.ControlMode == LineParam.CtrlMode.TIME_BASED_SPACING) { intervalSec = lineParam.ShotTimeInterval / 1000.0 < dispenseTime ? dispenseTime : lineParam.ShotTimeInterval / 1000.0; points = calFixedTotalOfDots(start, end, (int)(totalTime / intervalSec) + 1); } else if (lineParam.ControlMode == LineParam.CtrlMode.TOTAL_OF_DOTS && lineParam.TotalOfDots > 0) { intervalSec = totalTime / (lineParam.TotalOfDots - 1); if (intervalSec < dispenseTime) { intervalSec = dispenseTime; points = calFixedTotalOfDots(start, end, (int)(totalTime / intervalSec) + 1); } else { points = calFixedTotalOfDots(start, end, lineParam.TotalOfDots); } } else { points = new PointD[0]; } return(points); }
/// <summary> /// 手动打线,供四方位角度校准时使用 /// </summary> /// <returns></returns> public Result ManualDispense(PointD start, PointD end, double posZ, LineParam lineParam, bool isWeight, double weight) { Result result = Result.OK; PointD accStart = new PointD(), decelEnd = new PointD(); double speed = 0, intervalSec = 0; PointD[] points = null; //胶量模式 if (isWeight) { this.GetManualWtLineParam(start, end, lineParam, weight, out accStart, out decelEnd, out speed, out points, out intervalSec); } //速度模式 else { this.GetManualNormalLineParam(start, end, lineParam, out accStart, out decelEnd, out speed, out points, out intervalSec); } //执行打线 // 移动到加速区间起始位置 Log.Dprint("move to position XY : " + accStart); result = Machine.Instance.Robot.MovePosXYAndReply(accStart, FluidProgram.Current.MotionSettings.VelXY, FluidProgram.Current.MotionSettings.AccXY); if (!result.IsOk) { return(result); } // 到指定高度 result = Machine.Instance.Robot.MovePosZAndReply(posZ, lineParam.DownSpeed, lineParam.DownAccel); if (!result.IsOk) { return(result); } //开始走直线出胶 if (Machine.Instance.Valve1.ValveSeries == ValveSeries.喷射阀) { JtValve jtValve = Machine.Instance.Valve1 as JtValve; jtValve.FluidManualLine(accStart, start, end, decelEnd, speed, points, intervalSec, FluidProgram.Current.MotionSettings.WeightAcc); } return(result); }
public void MakeLine(Transform tarTrans, LineParam lineParam, Func <float, float> calcPos) { if (!tarTrans) { return; } float x = tarTrans.position.x + lineParam.StartX; if (x < minX) { minX = x; } x += lineParam.TotalLength; if (x > maxX) { maxX = x; } Vector3 PerPos = tarTrans.position; for (float i = lineParam.StartX; i < lineParam.TotalLength; i = i + delat) { float h = calcPos(i); Vector3 localPos = new Vector3(i, h, 0); Vector3 worldPos = tarTrans.TransformPoint(localPos); h = worldPos.y; if (h > maxY) { maxY = h; } else if (h < minY) { minY = h; } if (i != lineParam.StartX) { Debug.DrawLine(PerPos, worldPos, lineParam.LineColor); print(PerPos + "////" + worldPos); } PerPos = worldPos; } }
/// <summary> /// 计算在手动打线模式下,普通线需要的各种参数 /// </summary> /// <param name="start"></param> /// <param name="end"></param> /// <param name="lineParam"></param> /// <param name="accStartPoint"></param> /// <param name="decelEndPoint"></param> /// <param name="runSpeed"></param> /// <param name="linePoints"></param> /// <param name="fluidIntervalSec"></param> private void GetManualNormalLineParam(PointD start, PointD end, LineParam lineParam, out PointD accStartPoint, out PointD decelEndPoint, out double runSpeed, out PointD[] linePoints, out double fluidIntervalSec) { double accDistance = 0, decelDistance = 0; // 运动速度不能超过设定的最大速度 double speed = lineParam.Speed > FluidProgram.CurrentOrDefault().MotionSettings.WorkMaxVel ? FluidProgram.CurrentOrDefault().MotionSettings.WorkMaxVel : lineParam.Speed; // 计算加速区间距离 if (lineParam.AccelDistanceValueType == LineParam.ValueType.COMPUTE) { accDistance = MathUtils.CalculateDistance(0, speed, Machine.Instance.Robot.AxisX.ConvertAcc2Mm(FluidProgram.CurrentOrDefault().MotionSettings.WeightAcc)); Log.Dprint("Acc Distance COMPUTE speed=" + speed + ", WeightAcc=" + FluidProgram.CurrentOrDefault().MotionSettings.WeightAcc + ", distance=" + accDistance); } else { accDistance = lineParam.AccelDistance; } // 计算减速区间距离 if (lineParam.DecelDistanceValueType == LineParam.ValueType.COMPUTE) { decelDistance = MathUtils.CalculateDistance(speed, 0, -Machine.Instance.Robot.AxisX.ConvertAcc2Mm(FluidProgram.CurrentOrDefault().MotionSettings.WeightAcc)); Log.Dprint("Decel Distance COMPUTE speed=" + speed + ", WeightAcc=" + FluidProgram.CurrentOrDefault().MotionSettings.WeightAcc + ", distance=" + decelDistance); } else { decelDistance = lineParam.DecelDistance; } // 计算加速区间起始位置和减速区间结束位置 PointD accStart = calAccStartPosition(start, end, accDistance); PointD decelEnd = calDecelEndPosition(start, end, decelDistance); // 计算点胶位置 double intervalSec; PointD[] points = calNormalLinePoints(start, end, lineParam, speed, out intervalSec); accStartPoint = accStart; decelEndPoint = decelEnd; runSpeed = speed; linePoints = points; fluidIntervalSec = intervalSec; }
public void makeLine(Transform tarTrans, LineParam param, Func <float, LineParam, Vector3> calcPos) { if (tarTrans == null) { return; } Debug.DrawLine(tarTrans.position, tarTrans.TransformPoint(Vector3.forward * param.TotalLength)); Vector3 PerPos = tarTrans.position; for (float i = 0; i < param.TotalLength; i = i + delat) { Vector3 localPos = calcPos(i, param); Vector3 worldPos = tarTrans.TransformPoint(localPos); if (i > 0) { Debug.DrawLine(PerPos, worldPos, Color.green); } PerPos = worldPos; } }
public Arc(ArcCmd arcCmd, CoordinateCorrector coordinateCorrector) { //this.Valve = arcCmd.Valve; this.RunnableModule = arcCmd.RunnableModule; if (this.RunnableModule.Mode == ModuleMode.AssignMode1 || this.RunnableModule.Mode == ModuleMode.MainMode) { this.Valve = ValveType.Valve1; } else if (this.RunnableModule.Mode == ModuleMode.DualFallow) { this.Valve = ValveType.Both; } else { this.Valve = ValveType.Valve2; } start = coordinateCorrector.Correct(arcCmd.RunnableModule, arcCmd.Start, Executor.Instance.Program.ExecutantOriginOffset); middle = coordinateCorrector.Correct(arcCmd.RunnableModule, arcCmd.Middle, Executor.Instance.Program.ExecutantOriginOffset); end = coordinateCorrector.Correct(arcCmd.RunnableModule, arcCmd.End, Executor.Instance.Program.ExecutantOriginOffset); center = coordinateCorrector.Correct(arcCmd.RunnableModule, arcCmd.Center, Executor.Instance.Program.ExecutantOriginOffset); Log.Dprint("Arc start " + arcCmd.Start + ", real : " + start); Log.Dprint("Arc middle " + arcCmd.Middle + ", real : " + middle); Log.Dprint("Arc end " + arcCmd.End + ", real : " + end); Log.Dprint("Arc center " + arcCmd.Center + ", real : " + center); r = MathUtils.Distance(Center, Start); Degree = arcCmd.Degree; length = Math.Abs(arcCmd.Degree) / 360f * 2 * Math.PI * r; param = arcCmd.RunnableModule.CommandsModule.Program.ProgramSettings.GetLineParam(arcCmd.LineStyle); isWeightControl = arcCmd.IsWeightControl; weight = arcCmd.Weight; Program = arcCmd.RunnableModule.CommandsModule.Program; if (arcCmd.AssociatedMeasureheightCmd != null) { curMeasureHeightValue = arcCmd.AssociatedMeasureheightCmd.RealHtValue; } else { curMeasureHeightValue = this.RunnableModule.MeasuredHt; } }
public Arc(ArcCmd arcCmd) { //this.Valve = arcCmd.Valve; this.RunnableModule = arcCmd.RunnableModule; if (this.RunnableModule.Mode == ModuleMode.AssignMode1 || this.RunnableModule.Mode == ModuleMode.MainMode) { this.Valve = ValveType.Valve1; } else if (this.RunnableModule.Mode == ModuleMode.DualFallow) { this.Valve = ValveType.Both; } else { this.Valve = ValveType.Valve2; } start = arcCmd.Start; middle = arcCmd.Middle; end = arcCmd.End; center = arcCmd.Center; r = MathUtils.Distance(Center, Start); Degree = arcCmd.Degree; length = Math.Abs(arcCmd.Degree) / 360f * 2 * Math.PI * r; param = arcCmd.RunnableModule.CommandsModule.Program.ProgramSettings.GetLineParam(arcCmd.LineStyle); isWeightControl = arcCmd.IsWeightControl; weight = arcCmd.Weight; Program = arcCmd.RunnableModule.CommandsModule.Program; if (arcCmd.AssociatedMeasureheightCmd != null) { curMeasureHeightValue = arcCmd.AssociatedMeasureheightCmd.RealHtValue; } else { curMeasureHeightValue = this.RunnableModule.MeasuredHt; } }
private bool CanParseFile(StreamReader streamReader, string fileName, out int version) { version = -1; // Read the version information from the file LineParam versionParam = new LineParam(); versionParam.Reader = streamReader; try { ReadLineFromFile(versionParam); } catch (Exception e) { this.TraceSource.WriteExceptionAsError( this.LogSourceId, e, "Failed to get version information from file {0}. File will be skipped.", fileName); return(false); } if (null == versionParam.Line) { // End of file reached this.TraceSource.WriteError( this.LogSourceId, "Version information was not present in file {0}.", fileName); return(false); } // Figure out the version number string[] versionLineParts = versionParam.Line.Split(':'); if (versionLineParts.Length != 2) { this.TraceSource.WriteError( this.LogSourceId, "Unable to parse the version information '{0}' in file {1}.", versionParam, fileName); return(false); } if (false == int.TryParse(versionLineParts[1], out version)) { this.TraceSource.WriteError( this.LogSourceId, "Unable to parse version number '{0}' as an integer.", versionLineParts[1]); return(false); } // Verify that the version number is in the range we expect if ((version < 1) || (version > 2)) { this.TraceSource.WriteError( this.LogSourceId, "Unable to parse file {0} because it uses unknown version number {1}.", fileName, version); return(false); } return(true); }
private void ProcessEventsFromFile(string fileName) { this.TraceSource.WriteInfo( this.LogSourceId, "Processing ETW events from file {0}.", fileName); // Open the file ReadEventsFromFileParam readEventsParam = new ReadEventsFromFileParam(); readEventsParam.StreamReader = null; readEventsParam.FileName = fileName; try { Utility.PerformIOWithRetries( this.OpenEtwEventCacheFile, readEventsParam); } catch (Exception e) { this.TraceSource.WriteExceptionAsError( this.LogSourceId, e, "Failed to open file {0} for read.", fileName); return; } if (readEventsParam.FileNotFound) { Debug.Assert(null == readEventsParam.StreamReader, "StreamReader should remain unset if file is not found."); return; } // Read and process events from the file try { // Check the DCA version to make sure we can parse this file int version; if (false == this.CanParseFile(readEventsParam.StreamReader, fileName, out version)) { return; } // Upload the ETW events that we just retrieved this.TraceSource.WriteInfo( this.LogSourceId, "Starting delivery of ETW events from file {0} ....", fileName); LineParam lineParam = new LineParam(); lineParam.Reader = readEventsParam.StreamReader; for (;;) { // Read an event from the file try { ReadLineFromFile(lineParam); } catch (Exception e) { this.TraceSource.WriteExceptionAsError( this.LogSourceId, e, "Failed to read event from file {0}.", fileName); break; } if (null == lineParam.Line) { // End of file reached break; } DecodedEtwEvent etwEventInfo = new DecodedEtwEvent(); string nodeUniqueEventId = null; if (false == this.ParseEventInfo(fileName, version, lineParam.Line, ref etwEventInfo, ref nodeUniqueEventId)) { // Couldn't parse this event, so skip it and continue with the // remaining events. continue; } // Deliver the event to the consumer this.eventSink.OnEtwEventAvailable(etwEventInfo, nodeUniqueEventId); this.perfHelper.EventDeliveredToConsumer(); // If the consumer has asked for the event delivery period to // be aborted, then do so immediately. if (this.eventDeliveryPeriodAborted) { this.TraceSource.WriteInfo( this.LogSourceId, "The event delivery pass is being aborted. Therefore, no more events will be read from file {0}.", fileName); break; } // If we are in the process of stopping, then don't process // any more events if (this.Stopping) { this.TraceSource.WriteInfo( this.LogSourceId, "The consumer is being stopped. Therefore, no more events will be read from file {0}.", fileName); break; } } this.TraceSource.WriteInfo( this.LogSourceId, "Finished delivery of ETW events from file {0}.", fileName); } finally { readEventsParam.StreamReader.Dispose(); } }
/// <summary> /// 螺杆阀移动到线的起点 /// </summary> /// <returns></returns> private Result SvValveMoveToLineStart(Line line, PointD pos, PointD simulPos, double currZ, double targZ, LineParam param) { Result ret = Result.OK; if (currZ > targZ) { // 移动到指定位置 Log.Dprint("move to Line Start position XY : " + pos); if (line.RunnableModule.Mode == ModuleMode.MainMode) { ret = Machine.Instance.Robot.MovePosXYABAndReply(pos, simulPos, FluidProgram.Current.MotionSettings.VelXYAB, FluidProgram.Current.MotionSettings.AccXYAB, (int)Machine.Instance.Setting.CardSelect); } else { ret = Machine.Instance.Robot.MovePosXYAndReply(pos, FluidProgram.Current.MotionSettings.VelXY, FluidProgram.Current.MotionSettings.AccXY); } if (!ret.IsOk) { return(ret); } double z = 0; if (Machine.Instance.Laser.Laserable.Vendor == Drive.Sensors.HeightMeasure.Laser.Vendor.Disable) { z = targZ; } else { z = Converter.NeedleBoard2Z(param.DispenseGap, line.CurMeasureHeightValue); } Log.Dprint("move down to Z : " + z.ToString("0.000000") + ", DispenseGap=" + param.DispenseGap.ToString("0.000000")); ret = Machine.Instance.Robot.MovePosZByToleranceAndReply(z, param.DownSpeed, param.DownAccel); if (!ret.IsOk) { return(ret); } } else { double z = 0; if (Machine.Instance.Laser.Laserable.Vendor == Drive.Sensors.HeightMeasure.Laser.Vendor.Disable) { z = targZ; } else { z = Converter.NeedleBoard2Z(param.DispenseGap, line.CurMeasureHeightValue); } Log.Dprint("move up to Z : " + z.ToString("0.000000") + ", DispenseGap=" + param.DispenseGap.ToString("0.000000")); ret = Machine.Instance.Robot.MovePosZByToleranceAndReply(z, param.DownSpeed, param.DownAccel); if (!ret.IsOk) { return(ret); } Log.Dprint("move to Line Start position XY : " + pos); if (line.RunnableModule.Mode == ModuleMode.MainMode) { ret = Machine.Instance.Robot.MovePosXYABAndReply(pos, simulPos, FluidProgram.Current.MotionSettings.VelXYAB, FluidProgram.Current.MotionSettings.AccXYAB, (int)Machine.Instance.Setting.CardSelect); } else { ret = Machine.Instance.Robot.MovePosXYAndReply(pos, FluidProgram.Current.MotionSettings.VelXY, FluidProgram.Current.MotionSettings.AccXY); } if (!ret.IsOk) { return(ret); } } return(ret); }
/// <summary> /// 计算在手动打线模式下,重量线需要的各种参数 /// </summary> /// <param name="start"></param> /// <param name="end"></param> /// <param name="lineParam"></param> /// <param name="weight"></param> /// <param name="accStartPoint"></param> /// <param name="decelEndPoint"></param> /// <param name="runSpeed"></param> /// <param name="linePoints"></param> /// <param name="fluidIntervalSec"></param> private void GetManualWtLineParam(PointD start, PointD end, LineParam lineParam, double weight, out PointD accStartPoint, out PointD decelEndPoint, out double runSpeed, out PointD[] linePoints, out double fluidIntervalSec) { double accDistance = 0, decelDistance = 0; double lineDistance = Math.Sqrt(Math.Pow(end.X - start.X, 2) + Math.Pow(end.Y - start.Y, 2)); // 计算点胶数量 int shots = (int)((decimal)weight / (decimal)FluidProgram.CurrentOrDefault().RuntimeSettings.SingleDropWeight); // 计算时间 (需要将微秒单位转换成秒) double totalTime = Machine.Instance.Valve1.SpraySec * shots; // 计算速度 double wtSpeed = lineDistance / totalTime; if (lineParam.WtCtrlSpeedValueType != LineParam.ValueType.COMPUTE) { wtSpeed = lineParam.WtCtrlSpeed > wtSpeed ? wtSpeed : lineParam.WtCtrlSpeed; } // 运动速度不能超过设定的最大速度 wtSpeed = wtSpeed > FluidProgram.CurrentOrDefault().MotionSettings.WeightMaxVel ? FluidProgram.CurrentOrDefault().MotionSettings.WeightMaxVel : wtSpeed; Log.Dprint(string.Format("lineDistance={0}, weight={1} totalTime={2}, param.WtCtrlSpeedValueType={3}, param.WtCtrlSpeed={4}, wtSpeed={5}", lineDistance, weight, totalTime, lineParam.WtCtrlSpeedValueType, lineParam.WtCtrlSpeed, wtSpeed)); // 计算加速区间距离 if (lineParam.AccelDistanceValueType == LineParam.ValueType.COMPUTE) { accDistance = MathUtils.CalculateDistance(0, wtSpeed, Machine.Instance.Robot.AxisX.ConvertAcc2Mm(FluidProgram.CurrentOrDefault().MotionSettings.WeightAcc)); Log.Dprint("Acc Distance COMPUTE wtSpeed=" + wtSpeed + ", WeightAcc=" + FluidProgram.CurrentOrDefault().MotionSettings.WeightAcc + ", distance=" + accDistance); } else { accDistance = lineParam.AccelDistance; } // 计算减速区间距离 if (lineParam.DecelDistanceValueType == LineParam.ValueType.COMPUTE) { decelDistance = MathUtils.CalculateDistance(wtSpeed, 0, -Machine.Instance.Robot.AxisX.ConvertAcc2Mm(FluidProgram.CurrentOrDefault().MotionSettings.WeightAcc)); Log.Dprint("Decel Distance COMPUTE wtSpeed=" + wtSpeed + ", WeightAcc=" + FluidProgram.CurrentOrDefault().MotionSettings.WeightAcc + ", distance=" + decelDistance); } else { decelDistance = lineParam.DecelDistance; } // 计算加速区间起始位置和减速区间结束位置 PointD accStart = calAccStartPosition(start, end, accDistance); PointD decelEnd = calDecelEndPosition(start, end, decelDistance); // 计算点胶位置 PointD[] points = calFixedTotalOfDots(start, end, shots); //基于时间控制 totalTime = lineDistance / wtSpeed; double intervalSec = totalTime / (shots - 1); double dispenseSec = Machine.Instance.Valve1.SpraySec; if (intervalSec < dispenseSec) { intervalSec = dispenseSec; points = calFixedTotalOfDots(start, end, (int)(totalTime / intervalSec) + 1); } accStartPoint = accStart; decelEndPoint = decelEnd; runSpeed = wtSpeed; linePoints = points; fluidIntervalSec = intervalSec; }
public override Result Execute() { Result ret = Result.OK; LineParam param = symbolLinesCmd.RunnableModule.CommandsModule.Program.ProgramSettings.GetLineParam(this.oriSymbols[0].type); foreach (var item in oriSymbols) { if (item.symbolType == SymbolType.Arc) { PointD center = MathUtils.CalculateCircleCenter(item.symbolPoints[0], item.symbolPoints[1], item.symbolPoints[2]); item.symbolPoints[1] = center; } } // 添加倒角轨迹 this.finalSymbols = new List <SymbolLine>(); this.finalSymbols.Add((SymbolLine)oriSymbols[0].Clone()); for (int i = 0; i < oriSymbols.Count - 1; i++) { SymbolLine transitionArc = new SymbolLine(); transitionArc.symbolType = SymbolType.Arc; List <PointD> symbol1Points = oriSymbols[i].symbolPoints; List <PointD> symbol2Points = oriSymbols[i + 1].symbolPoints; if (oriSymbols[i].symbolType == SymbolType.Line && oriSymbols[i + 1].symbolType == SymbolType.Line) // 线-线 { // 添加过渡圆弧 transitionArc.symbolPoints = SymbolLineMathTools.GetLineTransitionArc(symbol1Points[0], symbol1Points[1], symbol2Points[1], oriSymbols[i].transitionR); this.finalSymbols.Last().symbolPoints[1] = transitionArc.symbolPoints[0]; oriSymbols[i + 1].symbolPoints[0] = transitionArc.symbolPoints[2]; } else if (oriSymbols[i].symbolType == SymbolType.Line && oriSymbols[i + 1].symbolType == SymbolType.Arc) // 线 - 弧 { // 添加过渡圆弧 transitionArc.symbolPoints = SymbolLineMathTools.GetLineToArcTransitionArcByR(symbol1Points[0], symbol1Points[1], symbol2Points[1], oriSymbols[i].transitionR, oriSymbols[i + 1].clockwise); this.finalSymbols.Last().symbolPoints[1] = transitionArc.symbolPoints[0]; oriSymbols[i + 1].symbolPoints[0] = transitionArc.symbolPoints[2]; } else if (oriSymbols[i].symbolType == SymbolType.Arc && oriSymbols[i + 1].symbolType == SymbolType.Line) // 弧 - 线 { // 添加过渡圆弧 int clockwise = 0; if (oriSymbols[i].clockwise == 0) { clockwise = 1; } transitionArc.symbolPoints = SymbolLineMathTools.GetLineToArcTransitionArcByR(symbol2Points[1], symbol2Points[0], symbol1Points[1], oriSymbols[i].transitionR, clockwise); transitionArc.symbolPoints.Reverse(); this.finalSymbols.Last().symbolPoints[2] = transitionArc.symbolPoints[0]; oriSymbols[i + 1].symbolPoints[0] = transitionArc.symbolPoints[2]; }//弧到弧不做处理 // 计算过渡圆弧方向 transitionArc.clockwise = SymbolLineMathTools.GetArcDirect(transitionArc.symbolPoints[0], transitionArc.symbolPoints[1], transitionArc.symbolPoints[2]); this.finalSymbols.Add(transitionArc); this.finalSymbols.Add((SymbolLine)oriSymbols[i + 1].Clone()); } // 计算每段轨迹起始角度和结束角度 foreach (SymbolLine item in this.finalSymbols) { if (item.symbolType == SymbolType.Line) { item.StartAngle = MathUtils.CalculateArc(item.symbolPoints[0], item.symbolPoints[1]); item.EndAngle = item.StartAngle; } else { double tempAngle = item.clockwise == 0 ? 90f : -90f; item.TrackStartAngle = MathUtils.CalculateArc(item.symbolPoints[1], item.symbolPoints[0]); item.TrackEndAngle = MathUtils.CalculateArc(item.symbolPoints[1], item.symbolPoints[2]); item.StartAngle = MathUtils.CalculateArc(item.symbolPoints[1], item.symbolPoints[0]) - tempAngle; item.EndAngle = MathUtils.CalculateArc(item.symbolPoints[1], item.symbolPoints[2]) - tempAngle; item.EndAngle = -1 * MathUtils.CalculateDegree(item.symbolPoints[0], item.symbolPoints[2], item.symbolPoints[1], item.clockwise); item.TrackSweep = -1 * MathUtils.CalculateDegree(item.symbolPoints[0], item.symbolPoints[2], item.symbolPoints[1], item.clockwise); } } double currZ = Machine.Instance.Robot.PosZ; double firstZ = 0; if (Machine.Instance.Valve1.RunMode != ValveRunMode.Look) { if (Machine.Instance.Laser.Laserable.Vendor == Drive.Sensors.HeightMeasure.Laser.Vendor.Disable) { firstZ = this.Program.RuntimeSettings.BoardZValue + param.DispenseGap; } else { firstZ = Converter.NeedleBoard2Z(param.DispenseGap, curMeasureHeightValue); } } else { firstZ = Machine.Instance.Robot.CalibPrm.SafeZ; } PointD offsetP = new PointD(this.OffsetX, this.OffsetY); // 轨迹动作逻辑 // 胶阀到位置 (XYU三轴同时到位) // 偏移调整 double z = 0; if (Machine.Instance.Valve1.RunMode != Drive.ValveSystem.ValveRunMode.Look) { //到起点,Z轴到点胶位置 if (currZ > firstZ) { // 移动到加速区间起始位置,这里加针嘴上补偿 Log.Dprint("move to position XY : " + this.finalSymbols[0].symbolPoints[0]); ret = Machine.Instance.Robot.MovePosXYRAndReply(this.finalSymbols[0].symbolPoints[0].ToNeedle(Machine.Instance.Valve1.ValveType) + offsetP, this.finalSymbols[0].StartAngle.ToAxisRPos(), this.Program.MotionSettings.VelXY, this.Program.MotionSettings.AccXY); if (!ret.IsOk) { return(ret); } // 下降到指定高度 if (Machine.Instance.Laser.Laserable.Vendor == Drive.Sensors.HeightMeasure.Laser.Vendor.Disable) { z = firstZ; } else { z = Converter.NeedleBoard2Z(param.DispenseGap, curMeasureHeightValue); } Log.Dprint("move down to Z : " + z.ToString("0.000000") + ", DispenseGap=" + param.DispenseGap.ToString("0.000000")); ret = Machine.Instance.Robot.MovePosZAndReply(z, param.DownSpeed, param.DownAccel); if (!ret.IsOk) { return(ret); } } else { // 上升到指定高度 if (Machine.Instance.Laser.Laserable.Vendor == Drive.Sensors.HeightMeasure.Laser.Vendor.Disable) { z = firstZ; } else { z = Converter.NeedleBoard2Z(param.DispenseGap, curMeasureHeightValue); } Log.Dprint("move up to Z : " + z.ToString("0.000000") + ", DispenseGap=" + param.DispenseGap.ToString("0.000000")); ret = Machine.Instance.Robot.MovePosZAndReply(z, param.DownSpeed, param.DownAccel); if (!ret.IsOk) { return(ret); } // 移动到加速区间起始位置,这里加针嘴上补偿 Log.Dprint("move to position XY : " + this.finalSymbols[0].symbolPoints[0]); ret = Machine.Instance.Robot.MovePosXYRAndReply(this.finalSymbols[0].symbolPoints[0].ToNeedle(Machine.Instance.Valve1.ValveType) + offsetP, this.finalSymbols[0].StartAngle.ToAxisRPos(), this.Program.MotionSettings.VelXY, this.Program.MotionSettings.AccXY); if (!ret.IsOk) { return(ret); } } } else { ret = Machine.Instance.Robot.MoveSafeZAndReply(); ret = Machine.Instance.Robot.MovePosXYAndReply(this.finalSymbols[0].symbolPoints[0], this.Program.MotionSettings.VelXY, this.Program.MotionSettings.AccXY); } // XYR轴三轴联动后R轴转点位模式 Machine.Instance.Robot.AxisR.Card.Executor.SetMovePos( Machine.Instance.Robot.AxisR.CardId, Machine.Instance.Robot.AxisR.AxisId, Machine.Instance.Robot.AxisR.ConvertPos2Card(Machine.Instance.Robot.AxisR.Pos), 5, 5, 5); // 使用symbols生成对应数据段对象数组 List <CrdSymbolLine> finalCrdData = new List <CrdSymbolLine>(); foreach (SymbolLine item in this.finalSymbols) { if (item.symbolType == SymbolType.Arc) { CrdSymbolLine temp = new CrdSymbolLine(); temp.Type = (int)item.symbolType; temp.Points = item.symbolPoints; temp.StartAngle = item.StartAngle; temp.EndAngle = item.EndAngle; temp.TrackStartAngle = item.TrackStartAngle; temp.TrackEndAngle = item.TrackEndAngle; temp.TrackSweep = item.TrackSweep; temp.Clockwise = item.clockwise; if (Machine.Instance.Valve1.RunMode == ValveRunMode.Look) { temp.EndZ = Machine.Instance.Robot.CalibPrm.SafeZ; } else { if (Machine.Instance.Laser.Laserable.Vendor == Drive.Sensors.HeightMeasure.Laser.Vendor.Disable) { temp.EndZ = firstZ; } else { if (finalCrdData.Count == 0) // { temp.EndZ = Converter.NeedleBoard2Z(param.DispenseGap, item.MHCmdList.Last().RealHtValue + this.LaserZDelta); } else { temp.EndZ = finalCrdData[finalCrdData.Count - 1].EndZ; } } } finalCrdData.Add(temp); } else { // 根据测高点数拆分长直线 // 首条轨迹有可能有2个测高但不需要拆分 if ((item.MHCount > 1 && finalCrdData.Count != 0) || (item.MHCount > 2 && finalCrdData.Count == 0)) { CrdSymbolLine temp = new CrdSymbolLine(); temp.Points.Add(new PointD()); temp.Points.Add(new PointD()); temp.Type = (int)item.symbolType; temp.Points[0].X = item.symbolPoints[0].X; temp.Points[0].Y = item.symbolPoints[0].Y; //temp.Points[1].X = item.MHCmdList[0].Position.X; //temp.Points[1].Y = item.MHCmdList[0].Position.Y; temp.Points[1].X = item.MHList[0].Position.X; temp.Points[1].Y = item.MHList[0].Position.Y; temp.StartAngle = item.StartAngle; temp.EndAngle = item.EndAngle; temp.TrackStartAngle = item.TrackStartAngle; temp.TrackEndAngle = item.TrackEndAngle; temp.TrackSweep = item.TrackSweep; temp.Clockwise = item.clockwise; if (Machine.Instance.Valve1.RunMode == ValveRunMode.Look) { temp.EndZ = Machine.Instance.Robot.CalibPrm.SafeZ; } else { if (Machine.Instance.Laser.Laserable.Vendor == Drive.Sensors.HeightMeasure.Laser.Vendor.Disable) { temp.EndZ = firstZ; } else { temp.EndZ = Converter.NeedleBoard2Z(param.DispenseGap, item.MHCmdList[0].RealHtValue + this.LaserZDelta); } } finalCrdData.Add(temp); for (int i = 1; i < item.MHCmdList.Count; i++) { CrdSymbolLine temp1 = new CrdSymbolLine(); temp1.Points.Add(new PointD()); temp1.Points.Add(new PointD()); temp1.Type = (int)item.symbolType; //temp1.Points[0].X = item.MHCmdList[i - 1].Position.X; //temp1.Points[0].Y = item.MHCmdList[i - 1].Position.Y; //temp1.Points[1].X = item.MHCmdList[i].Position.X; //temp1.Points[1].Y = item.MHCmdList[i].Position.Y; temp1.Points[0].X = item.MHList[i - 1].Position.X; temp1.Points[0].Y = item.MHList[i - 1].Position.Y; temp1.Points[1].X = item.MHList[i].Position.X; temp1.Points[1].Y = item.MHList[i].Position.Y; temp1.StartAngle = item.StartAngle; temp1.EndAngle = item.EndAngle; temp.TrackStartAngle = item.TrackStartAngle; temp.TrackEndAngle = item.TrackEndAngle; temp.TrackSweep = item.TrackSweep; temp1.Clockwise = item.clockwise; if (Machine.Instance.Valve1.RunMode == ValveRunMode.Look) { temp1.EndZ = Machine.Instance.Robot.CalibPrm.SafeZ; } else { if (Machine.Instance.Laser.Laserable.Vendor == Drive.Sensors.HeightMeasure.Laser.Vendor.Disable) { temp1.EndZ = firstZ; } else { temp1.EndZ = Converter.NeedleBoard2Z(param.DispenseGap, item.MHCmdList[i].RealHtValue + this.LaserZDelta); } } finalCrdData.Add(temp1); } } else { CrdSymbolLine temp = new CrdSymbolLine(); temp.Type = (int)item.symbolType; temp.Points = item.symbolPoints; temp.StartAngle = item.StartAngle; temp.EndAngle = item.EndAngle; temp.TrackStartAngle = item.TrackStartAngle; temp.TrackEndAngle = item.TrackEndAngle; temp.TrackSweep = item.TrackSweep; temp.Clockwise = item.clockwise; if (Machine.Instance.Valve1.RunMode == ValveRunMode.Look) { temp.EndZ = Machine.Instance.Robot.CalibPrm.SafeZ; } else { if (Machine.Instance.Laser.Laserable.Vendor == Drive.Sensors.HeightMeasure.Laser.Vendor.Disable) { temp.EndZ = firstZ; } else if (item.MHCount == 0) //当前直线轨迹没有测高,用前一个测高值 { temp.EndZ = Converter.NeedleBoard2Z(param.DispenseGap, curMeasureHeightValue); } else { temp.EndZ = Converter.NeedleBoard2Z(param.DispenseGap, item.MHCmdList.Last().RealHtValue + this.LaserZDelta); } } finalCrdData.Add(temp); } if (Machine.Instance.Valve1.RunMode != ValveRunMode.Look) { } } } double lastZPos = 0; for (int i = 0; i < finalCrdData.Count; i++) { double temp = finalCrdData[i].EndZ; if (i == 0) { finalCrdData[i].EndZ = finalCrdData[i].EndZ - firstZ; } else { finalCrdData[i].EndZ = finalCrdData[i].EndZ - lastZPos; } lastZPos = temp; } //设置进行点胶需要的各种参数(Edit By 肖旭) GearValveFluidSymbolLinesPrm fluidPrm = new GearValveFluidSymbolLinesPrm { Vel = this.symbolLinesCmd.LineParam.Speed, StopSprayDistance = this.symbolLinesCmd.LineParam.ShutOffDistance, StartSprayDelay = this.symbolLinesCmd.LineParam.PreMoveDelay, EndPosDelay = this.symbolLinesCmd.LineParam.Dwell, PressDistance = this.symbolLinesCmd.LineParam.PressDistance, PressVel = this.symbolLinesCmd.LineParam.PressSpeed, PressAcc = this.symbolLinesCmd.LineParam.PressAccel, PressTime = this.symbolLinesCmd.LineParam.PressTime, RaiseDistance = this.symbolLinesCmd.LineParam.RaiseDistance, RaiseVel = this.symbolLinesCmd.LineParam.RaiseSpeed, RaiseAcc = this.symbolLinesCmd.LineParam.RaiseAccel, ArcSpeed = this.symbolLinesCmd.ArcSpeed, BacktrackEndPos = this.symbolLinesCmd.LineParam.BacktrackEndGap + z, BacktrackDistance = this.symbolLinesCmd.LineParam.BacktrackDistance, BacktrackGap = this.symbolLinesCmd.LineParam.BacktrackGap, BacktrackSpeed = this.symbolLinesCmd.LineParam.BacktrackSpeed, BacktrackEndGap = this.symbolLinesCmd.LineParam.BacktrackEndGap, BackGap = this.symbolLinesCmd.LineParam.BackGap }; // 将插补数据传递到valve对象中 ret = Machine.Instance.Valve1.FluidSymbolLines(finalCrdData, fluidPrm, FluidProgram.Current.MotionSettings.WeightAcc, this.OffsetX, this.OffsetY ); return(ret); }