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