/// <summary>
/// 对于一条线段上固定点胶数的模式，计算所有点胶位置
/// </summary>
/// <param name="line"></param>
/// <param name="totalOfDots"></param>
/// <returns></returns>
private PointD[] calFixedTotalOfDots(LineCoordinate line, int totalOfDots)
{
PointD[] points = null;
if (totalOfDots <= 0)
{
points = new PointD[0];
return(points);
}
else
{
points = new PointD[totalOfDots];
if (totalOfDots == 1)
{
points[0] = new PointD((line.End.X + line.Start.X) / 2, (line.End.Y + line.Start.Y) / 2);
return(points);
}
else
{
double dx = (line.End.X - line.Start.X) / (totalOfDots - 1);
double dy = (line.End.Y - line.Start.Y) / (totalOfDots - 1);
for (int i = 0; i < totalOfDots; i++)
{
points[i] = new PointD(line.Start.X + dx * i, line.Start.Y + dy * i);
}
//根据首尾重复情况去掉首尾点
PointD[] dispensPoints;
if (line.Repetition.TotalRepeat)
{
dispensPoints = new PointD[points.Length - 2];
for (int i = 0; i < dispensPoints.Length; i++)
{
dispensPoints[i] = points[i + 1];
}
}
else if (!line.Repetition.TotalRepeat && line.Repetition.StartIsRepeat)
{
dispensPoints = new PointD[points.Length - 1];
for (int i = 0; i < dispensPoints.Length; i++)
{
dispensPoints[i] = points[i + 1];
}
}
else if (!line.Repetition.TotalRepeat && line.Repetition.EndIsRepeat)
{
dispensPoints = new PointD[points.Length - 1];
for (int i = 0; i < dispensPoints.Length; i++)
{
dispensPoints[i] = points[i];
}
}
else
{
dispensPoints = points;
}
return(dispensPoints);
}
}
}
private void OnMouseMoveCommandExecuted(MouseEventArgs args)
{
if (!(args.Source is FrameworkElement element))
{
return;
}
switch (element.Name)
{
case "Triangle":
{
if (isDragDropForPathTriangle)
{
TriangleCoordinate.TriangleMove(args.GetPosition(null), pointOfMouseDown);
pointOfMouseDown = args.GetPosition(null);
}
break;
}
case "Line":
{
if (isDragDropForPathLine)
{
LineCoordinate.LineMove(args.GetPosition(null), pointOfMouseDown);
pointOfMouseDown = args.GetPosition(null);
}
break;
}
}
}
private void button3_Click(object sender, EventArgs e)
{
line = null;
isFirst = true;
showAcc = false;
showDecel = false;
panel1.Invalidate();
}
private void btnOk_Click(object sender, System.EventArgs e)
{
if (lineCoordinateCache.Count <= 0)
{
//MessageBox.Show("Line points is empty.");
MessageBox.Show("线轨迹点个数不可以为0.");
return;
}
lineCmdLine.LineCoordinateList.Clear();
lineCmdLine.LineCoordinateList.AddRange(lineCoordinateCache);
lineCmdLine.IsWeightControl = cbWeightControl.Checked;
if (isCreating)
{
MsgCenter.Broadcast(Constants.MSG_FINISH_ADDING_CMD_LINE, this, lineCmdLine);
}
else
{
MsgCenter.Broadcast(Constants.MSG_FINISH_EDITING_CMD_LINE, this, lineCmdLine);
}
int count = lineCmdLine.LineCoordinateList.Count;
if (count > 0)
{
LineCoordinate lineCoordinate = lineCmdLine.LineCoordinateList[count - 1];
Properties.Settings.Default.LineStartX = lineCoordinate.Start.X;
Properties.Settings.Default.LineStartY = lineCoordinate.Start.Y;
Properties.Settings.Default.LineEndX = lineCoordinate.End.X;
Properties.Settings.Default.LineEndY = lineCoordinate.End.Y;
Properties.Settings.Default.LineStyle = (int)lineCmdLine.LineStyle;
Properties.Settings.Default.LineIsWt = lineCmdLine.IsWeightControl;
Properties.Settings.Default.LineWt = lineCmdLine.WholeWeight;
}
if (Machine.Instance.Valve1.RunMode == ValveRunMode.AdjustLine)
{
Line.WaitMsg.Set();
}
if (!this.isCreating)
{
Close();
if (this.lineCmdLine != null && this.lineCmdLineBackUp != null)
{
CompareObj.CompareField(this.lineCmdLine, this.lineCmdLineBackUp, null, this.GetType().Name, true);
CompareObj.CompareProperty(this.lineCmdLine, this.lineCmdLineBackUp, null, this.GetType().Name, true);
if (this.lineCmdLine.LineCoordinateList.Count == this.lineCmdLineBackUp.LineCoordinateList.Count)
{
for (int i = 0; i < this.lineCmdLine.LineCoordinateList.Count; i++)
{
string pathRoot = this.GetType().Name + "\\lineCmdLine\\" + "LineCoordinateList: " + i.ToString();
CompareObj.CompareField(this.lineCmdLine.LineCoordinateList[i], this.lineCmdLineBackUp.LineCoordinateList[i], null, pathRoot, true);
}
}
}
}
}
/// <summary>
/// 根据减速区间，计算减速结束位置
/// </summary>
/// <param name="line"></param>
/// <param name="decelDistance"></param>
/// <returns></returns>
private PointD calDecelEndPosition(LineCoordinate line, double decelDistance)
{
PointD position = new PointD();
double lineDistance = MathUtils.Distance(line.Start, line.End);
double cos = (line.End.X - line.Start.X) / lineDistance;
double sin = (line.End.Y - line.Start.Y) / lineDistance;
position.X = line.End.X + decelDistance * cos;
position.Y = line.End.Y + decelDistance * sin;
return(position);
}
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 void OnKeyDownCommandExecuted()
{
if (LineCoordinate.LineEndPointY != null)
{
LineCoordinate.RestorationLine();
LineCoordinate.AddPointToCollection();
}
if (TriangleCoordinate.TriangleThirdPointY == null)
{
return;
}
TriangleCoordinate.RestorationTriangle();
TriangleCoordinate.AddPointToCollection();
}
/// <summary>
/// 根据加速区间，计算加速起始位置
/// </summary>
/// <param name="line"></param>
/// <param name="accDistance"></param>
/// <returns></returns>
private PointD calAccStartPosition(LineCoordinate line, double accDistance)
{
VectorD v = (line.Start - line.End).Normalize() * accDistance;
return(line.Start + v);
//PointD position = new PointD();
//double lineDistance = MathUtils.Distance(line.Start, line.End);
//double cos = (line.End.X - line.Start.X) / lineDistance;
//double sin = (line.End.Y - line.Start.Y) / lineDistance;
//position.X = line.Start.X - accDistance * cos;
//position.Y = line.Start.Y - accDistance * sin;
//return position;
}
/// <summary>
/// 根据减速区间，计算减速结束位置
/// </summary>
/// <param name="line"></param>
/// <param name="decelDistance"></param>
/// <returns></returns>
private PointD calDecelEndPosition(LineCoordinate line, double decelDistance)
{
VectorD v = (line.End - line.Start).Normalize() * decelDistance;
return(line.End + v);
//PointD position = new PointD();
//double lineDistance = MathUtils.Distance(line.Start, line.End);
//double cos = (line.End.X - line.Start.X) / lineDistance;
//double sin = (line.End.Y - line.Start.Y) / lineDistance;
//position.X = line.End.X + decelDistance * cos;
//position.Y = line.End.Y + decelDistance * sin;
//return position;
}
/// <summary>
/// 计算普通线段上每个点胶位置
/// </summary>
/// <param name="line"></param>
/// <returns></returns>
private PointD[] calNormalLinePoints(LineCoordinate line, double speed, out double intervalSec)
{
double totalTime = line.CalculateDistance() / 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 (line.Param.ControlMode == LineParam.CtrlMode.POS_BASED_SPACING)
{
points = calFixedTotalOfDots(line, (int)(line.CalculateDistance() / line.Param.Spacing) + 1);
intervalSec = totalTime / (points.Length - 1);
if (intervalSec < dispenseTime)
{
intervalSec = dispenseTime;
points = calFixedTotalOfDots(line, (int)(totalTime / intervalSec) + 1);
}
}
else if (line.Param.ControlMode == LineParam.CtrlMode.TIME_BASED_SPACING)
{
intervalSec = line.Param.ShotTimeInterval / 1000.0 < dispenseTime ? dispenseTime : line.Param.ShotTimeInterval / 1000.0;
points = calFixedTotalOfDots(line, (int)(totalTime / intervalSec) + 1);
}
else if (line.Param.ControlMode == LineParam.CtrlMode.TOTAL_OF_DOTS && line.Param.TotalOfDots > 0)
{
intervalSec = totalTime / (line.Param.TotalOfDots - 1);
if (intervalSec < dispenseTime)
{
intervalSec = dispenseTime;
points = calFixedTotalOfDots(line, (int)(totalTime / intervalSec) + 1);
}
else
{
points = calFixedTotalOfDots(line, line.Param.TotalOfDots);
}
}
else
{
points = new PointD[0];
}
return(points);
}
private double CalculateVel(LineCoordinate line, double lineDistance, double totalTime)
{
// 计算速度
double wtSpeed = lineDistance / totalTime;
if (line.Param.WtCtrlSpeedValueType != LineParam.ValueType.COMPUTE)
{
wtSpeed = line.Param.WtCtrlSpeed > wtSpeed ? wtSpeed : line.Param.WtCtrlSpeed;
}
// 运动速度不能超过设定的最大速度
wtSpeed = wtSpeed > FluidProgram.Current.MotionSettings.WeightMaxVel ? FluidProgram.Current.MotionSettings.WeightMaxVel : wtSpeed;
Log.Dprint(string.Format("lineDistance={0}, weight={1} totalTime={2}, param.WtCtrlSpeedValueType={3}, param.WtCtrlSpeed={4}, wtSpeed={5}",
lineDistance, line.Weight, totalTime, line.Param.WtCtrlSpeedValueType, line.Param.WtCtrlSpeed, wtSpeed));
return(wtSpeed);
}
public LineCmd(RunnableModule runnableModule, LineCmdLine lineCmdLine, MeasureHeightCmd mhCmd) : base(runnableModule, lineCmdLine)
{
this.Valve = lineCmdLine.Valve;
var structure = runnableModule.CommandsModule.program.ModuleStructure;
PointD start, end;
this.lineCmdLine = lineCmdLine;
if (!runnableModule.CommandsModule.IsReversePattern)
{
foreach (LineCoordinate line in lineCmdLine.LineCoordinateList)
{
start = structure.ToMachine(runnableModule, line.Start);
end = structure.ToMachine(runnableModule, line.End);
LineCoordinate newLine = new LineCoordinate(start, end);
newLine.Weight = line.Weight;
newLine.LineStyle = line.LineStyle;
newLine.LookOffset = line.LookOffset;//偏移量
lineCoordinateList.Add(newLine);
}
}
else
{
List <LineCoordinate> lineCoordinateRevs = new List <LineCoordinate>();
lineCoordinateRevs.AddRange(lineCmdLine.LineCoordinateList);
lineCoordinateRevs.Reverse();
foreach (LineCoordinate line in lineCoordinateRevs)
{
start = structure.ToMachine(runnableModule, line.End);
end = structure.ToMachine(runnableModule, line.Start);
LineCoordinate newLine = new LineCoordinate(start, end);
newLine.Weight = line.Weight;
newLine.LineStyle = line.LineStyle;
newLine.LookOffsetRevs = line.LookOffsetRevs;//偏移量
lineCoordinateList.Add(newLine);
}
}
LineStyle = lineCmdLine.LineStyle;
IsWeightControl = lineCmdLine.IsWeightControl;
wholeWeight = lineCmdLine.WholeWeight;
this.associatedMeasureHeightCmd = mhCmd;
this.CheckRepeat();
}
private void panel1_MouseClick(object sender, MouseEventArgs e)
{
if (line == null)
{
if (isFirst)
{
start.X = e.X;
start.Y = e.Y;
isFirst = false;
}
else
{
end.X = e.X;
end.Y = e.Y;
line = new LineCoordinate(start, end);
}
}
panel1.Invalidate();
}
/// <summary>
/// 计算在所有模式下，执行普通线或速度模式时线命令所需要的参数
/// </summary>
private void CalculateNormalLineParam(LineCoordinate line, Line directiveLine, out PointD accStartPoint, out PointD decelEndPoint, out double runSpeed, out PointD[] linePoints, out double fluidIntervalSec)
{
double accDistance = 0, decelDistance = 0;
// 运动速度不能超过设定的最大速度
double speed = line.Param.Speed > FluidProgram.Current.MotionSettings.WorkMaxVel ? FluidProgram.Current.MotionSettings.WorkMaxVel : line.Param.Speed;
// 计算加速区间距离
if (line.Param.AccelDistanceValueType == LineParam.ValueType.COMPUTE)
{
accDistance = MathUtils.CalculateDistance(0, speed, Machine.Instance.Robot.AxisX.ConvertAcc2Mm(FluidProgram.Current.MotionSettings.WeightAcc));
Log.Dprint("Acc Distance COMPUTE speed=" + speed + ", WeightAcc=" + FluidProgram.Current.MotionSettings.WeightAcc
+ ", distance=" + accDistance);
}
else
{
accDistance = line.Param.AccelDistance;
}
// 计算减速区间距离
if (line.Param.DecelDistanceValueType == LineParam.ValueType.COMPUTE)
{
decelDistance = MathUtils.CalculateDistance(speed, 0, -Machine.Instance.Robot.AxisX.ConvertAcc2Mm(FluidProgram.Current.MotionSettings.WeightAcc));
Log.Dprint("Decel Distance COMPUTE speed=" + speed + ", WeightAcc=" + FluidProgram.Current.MotionSettings.WeightAcc
+ ", distance=" + decelDistance);
}
else
{
decelDistance = line.Param.DecelDistance;
}
// 计算加速区间起始位置和减速区间结束位置
PointD accStart = calAccStartPosition(line, accDistance);
PointD decelEnd = calDecelEndPosition(line, decelDistance);
// 计算点胶位置
double intervalSec;
PointD[] points = calNormalLinePoints(line, speed, out intervalSec);
accStartPoint = accStart;
decelEndPoint = decelEnd;
runSpeed = speed;
linePoints = points;
fluidIntervalSec = intervalSec;
}
private void btnAdd_Click(object sender, EventArgs e)
{
if (!tbStartX.IsValid || !tbStartY.IsValid || !tbEndX.IsValid || !tbEndY.IsValid)
{
//MessageBox.Show("Please input valid values.");
MessageBox.Show("请输入正确的值.");
return;
}
if (tbStartX.Value == tbEndX.Value && tbStartY.Value == tbEndY.Value)
{
//MessageBox.Show("Start point cannot be same with end point.");
MessageBox.Show("请输入正确的值.");
return;
}
LineCoordinate coordinate = new LineCoordinate(tbStartX.Value, tbStartY.Value, tbEndX.Value, tbEndY.Value);
lineCoordinateCache.Add(coordinate);
listBoxPoints.Items.Add(string.Format("{0}:{1},{2}mg,{3}", listBoxPoints.Items.Count, coordinate.LineStyle, coordinate.Weight, coordinate));
listBoxPoints.SelectedIndex = listBoxPoints.Items.Count - 1;
}
private void listBoxLines_SelectedIndexChanged(object sender, EventArgs e)
{
if (this.listBoxLines.SelectedIndex < 0)
{
return;
}
LineCoordinate lineCoordinate = lineCoordinateCache[this.listBoxLines.SelectedIndex];
//系统坐标->机械坐标
PointD start = pattern.MachineRel(lineCoordinate.Start);
PointD end = pattern.MachineRel(lineCoordinate.End);
this.tbPointX.Text = start.X.ToString("0.000");
this.tbPointY.Text = start.Y.ToString("0.000");
if (this.pattern.IsReversePattern)
{
this.offset = lineCoordinate.LookOffsetRevs;
}
else
{
this.offset = lineCoordinate.LookOffset;
}
this.nudOffset.Value = (decimal)this.offset;
this.indexOfLineCoordinate = listBoxLines.SelectedIndex;
}
private void btnAdd_Click(object sender, EventArgs e)
{
tbPointX.Text = (Machine.Instance.Robot.PosX - origin.X).ToString("0.000");
tbPointY.Text = (Machine.Instance.Robot.PosY - origin.Y).ToString("0.000");
if (!tbPointX.IsValid || !tbPointY.IsValid)
{
//MessageBox.Show("Please input valid values.");
MessageBox.Show("请输入正确的值.");
return;
}
//机械坐标->系统坐标
PointD newPoint = pattern.SystemRel(tbPointX.Value, tbPointY.Value);
PointD lastPoint = null;
if (this.linePointsCache.Count > 0)
{
lastPoint = this.linePointsCache[this.linePointsCache.Count - 1];
if (newPoint == lastPoint)
{
//MessageBox.Show("Start point cannot be same with end point.");
MessageBox.Show("起始点和终点不可以相同.");
return;
}
LineCoordinate line = new LineCoordinate(lastPoint, newPoint);
lineCoordinateCache.Add(line);
//系统坐标->机械坐标
listBoxLines.Items.Add(string.Format("{0}:{1},{2}mg,{3}{4}", listBoxLines.Items.Count, line.LineStyle, line.Weight,
pattern.MachineRel(line.Start), pattern.MachineRel(line.End)));
}
linePointsCache.Add(newPoint);
//系统坐标->机械坐标
listBoxPoints.Items.Add(string.Format("{0}:{1}", listBoxPoints.Items.Count,
pattern.MachineRel(newPoint)));
listBoxPoints.SelectedIndex = listBoxPoints.Items.Count - 1;
}
private void listBoxPoints_SelectedIndexChanged(object sender, EventArgs e)
{
if (listBoxPoints.SelectedIndex < 0)
{
return;
}
LineCoordinate lineCoordinate = lineCoordinateCache[listBoxPoints.SelectedIndex];
tbStartX.Text = lineCoordinate.Start.X.ToString("0.000");
tbStartY.Text = lineCoordinate.Start.Y.ToString("0.000");
tbEndX.Text = lineCoordinate.End.X.ToString("0.000");
tbEndY.Text = lineCoordinate.End.Y.ToString("0.000");
this.indexOfLineCoordinate = listBoxPoints.SelectedIndex;
if (this.pattern.IsReversePattern)
{
this.offset = lineCoordinate.LookOffsetRevs;
}
else
{
this.offset = lineCoordinate.LookOffset;
}
this.nudOffset.Value = (decimal)this.offset;
}
public EditLineMetro(Pattern pattern, LineCmdLine lineCmdLine)
{
InitializeComponent();
//this.ReadLanguageResources();
this.pattern = pattern;
this.origin = pattern.GetOriginPos();
if (lineCmdLine == null)
{
isCreating = true;
this.lineCmdLine = new LineCmdLine()
{
LineMethod = LineMethod.Single
};
this.lineCoordinate = new LineCoordinate(new PointD(), new PointD());
this.lineCoordinate.Start.X = Properties.Settings.Default.LineStartX;
this.lineCoordinate.Start.Y = Properties.Settings.Default.LineStartY;
this.lineCoordinate.End.X = Properties.Settings.Default.LineEndX;
this.lineCoordinate.End.Y = Properties.Settings.Default.LineEndY;
this.lineCmdLine.LineStyle = (LineStyle)Properties.Settings.Default.LineStyle;
this.lineCmdLine.IsWeightControl = Properties.Settings.Default.LineIsWt;
this.lineCmdLine.WholeWeight = Properties.Settings.Default.LineWt;
}
else
{
isCreating = false;
this.lineCmdLine = lineCmdLine;
this.lineCoordinate = this.lineCmdLine.LineCoordinateList[0];
}
PointD startMachine = this.pattern.MachineRel(this.lineCoordinate.Start);
PointD endMachine = this.pattern.MachineRel(this.lineCoordinate.End);
this.tbStartX.Text = startMachine.X.ToString("0.000");
this.tbStartY.Text = startMachine.Y.ToString("0.000");
this.tbEndX.Text = endMachine.X.ToString("0.000");
this.tbEndY.Text = endMachine.Y.ToString("0.000");
this.offset = this.lineCoordinate.LookOffset;
this.nudOffset.Value = (decimal)this.lineCoordinate.LookOffset;
this.nudOffset.Enabled = false;
if (Machine.Instance.Valve1.RunMode == ValveRunMode.AdjustLine)
{
this.disableUI();
this.nudOffset.Enabled = true;
}
for (int i = 0; i < FluidProgram.Current.ProgramSettings.LineParamList.Count; i++)
{
comboBoxLineType.Items.Add("Type " + (i + 1));
}
comboBoxLineType.SelectedIndex = (int)this.lineCmdLine.LineStyle;
cbWeightControl.Checked = this.lineCmdLine.IsWeightControl;
if (this.lineCmdLine.IsWeightControl)
{
tbWeight.Text = this.lineCmdLine.WholeWeight.ToString("0.000");
}
this.cbInspectionKey.Items.Add(InspectionKey.Line1);
this.cbInspectionKey.Items.Add(InspectionKey.Line2);
this.cbInspectionKey.Items.Add(InspectionKey.Line3);
this.cbInspectionKey.Items.Add(InspectionKey.Line4);
this.cbInspectionKey.SelectedIndex = 0;
if (this.pattern.IsReversePattern)
{
this.nudOffset.Value = (decimal)this.lineCoordinate.LookOffsetRevs;
}
else
{
this.nudOffset.Value = (decimal)this.lineCoordinate.LookOffset;
}
if (this.lineCmdLine != null)
{
this.lineCmdLineBackUp = (LineCmdLine)this.lineCmdLine.Clone();
}
}
/// <summary>
/// 执行单条线段命令，连续插补模式，将每条线压入插补队列
/// </summary>
/// <param name="line"></param>
/// <param name="accStart">加速区间距离</param>
/// <param name="decelEnd">减速区间距离</param>
/// <param name="speed">线段区间的运动速度</param>
/// <param name="points">每一滴胶水的点胶位置</param>
/// <param name="intervalSec">每一滴胶水的打胶时间，单位秒</param>
/// <returns></returns>
private Result executeSingleLine(Line directiveLine, LineCoordinate line, PointD accStart, PointD decelEnd, double speed, PointD[] points, double intervalSec)
{
double lineOffset = line.Param.Offset;
// 偏移调整
if (Machine.Instance.Valve1.RunMode == ValveRunMode.Dry ||
Machine.Instance.Valve1.RunMode == ValveRunMode.Wet ||
Machine.Instance.Valve1.RunMode == ValveRunMode.AdjustLine)
{
if (directiveLine.LineCmd.RunnableModule.CommandsModule.IsReversePattern)
{
lineOffset += line.LookOffsetRevs;
}
else
{
lineOffset += line.LookOffset;
}
}
VectorD v = (line.End - line.Start).Normalize() * lineOffset;
//相机位置
PointD cameraLineStart = line.Start + v;
PointD cameraLineEnd = line.End + v;
PointD cameraAccStart = accStart + v;
PointD cameraDecEnd = decelEnd + v;
PointD[] cameraPoints = new PointD[points.Length];
PointD[] cameraPoints2 = new PointD[points.Length];//副阀的点位
for (int i = 0; i < points.Length; i++)
{
cameraPoints[i] = points[i] + v;
}
//胶枪点胶起点
PointD valveAccStart = cameraAccStart.ToNeedle(directiveLine.Valve, directiveLine.Tilt);
//副阀点胶起点位置(默认值为设定间距)
PointD simulStart = new PointD(directiveLine.Program.RuntimeSettings.SimulDistence, 0) /*-胶阀原点间距？*/;
///生成副阀相关参数(起点、插补点位)
if (directiveLine.RunnableModule.Mode == ModuleMode.MainMode)
{
//副阀插补坐标绝对值(X方向实际坐标取负值) = 主阀机械坐标-副阀机械坐标-双阀原点间距（理论情况-不考虑坐标系不平行）
VectorD SimulModuleOffset = Machine.Instance.Robot.CalibPrm.NeedleCamera2 - Machine.Instance.Robot.CalibPrm.NeedleCamera1;
for (int i = 0; i < points.Length; i++)
{
cameraPoints2[i] = cameraPoints[i] - directiveLine.RunnableModule.SimulTransformer.Transform(cameraPoints[i]).ToVector() - SimulModuleOffset;
//乘以系数得到AB轴的移动位移
cameraPoints2[i].X = -Math.Abs(cameraPoints2[i].X) / Machine.Instance.Robot.CalibPrm.HorizontalRatio;
cameraPoints2[i].Y = -cameraPoints2[i].Y / Machine.Instance.Robot.CalibPrm.VerticalRatio;
}
//副阀在加速段直接使用实际点胶起始位置
simulStart = cameraPoints2[0];
}
Result ret = Result.OK;
double targZ = 0;
if (Machine.Instance.Laser.Laserable.Vendor == Drive.Sensors.HeightMeasure.Laser.Vendor.Disable)
{
targZ = directiveLine.Program.RuntimeSettings.BoardZValue + line.Param.DispenseGap;
}
else
{
targZ = Converter.NeedleBoard2Z(line.Param.DispenseGap, directiveLine.CurMeasureHeightValue, directiveLine.Tilt);
}
if (Machine.Instance.Valve1.RunMode == Drive.ValveSystem.ValveRunMode.Look ||
Machine.Instance.Valve1.RunMode == Drive.ValveSystem.ValveRunMode.AdjustLine ||
Machine.Instance.Valve1.RunMode == Drive.ValveSystem.ValveRunMode.InspectRect)
{
ret = Machine.Instance.Robot.BufMoveSafeZ();
if (!ret.IsOk)
{
return(ret);
}
if (Machine.Instance.Valve1.RunMode == Drive.ValveSystem.ValveRunMode.Look)
{
ret = Machine.Instance.Robot.BufMoveLnXY(cameraLineStart,
FluidProgram.Current.MotionSettings.VelXY,
FluidProgram.Current.MotionSettings.AccXY);
if (!ret.IsOk)
{
return(ret);
}
ret = Machine.Instance.Robot.BufMoveLnXY(cameraLineEnd,
line.Param.Speed,
FluidProgram.Current.MotionSettings.AccXY);
if (!ret.IsOk)
{
return(ret);
}
Log.Print("current position in LookMode: " + cameraAccStart.ToString() + " || the LookOffset:" + line.LookOffset);
}
else if (Machine.Instance.Valve1.RunMode == Drive.ValveSystem.ValveRunMode.AdjustLine)
{
ret = Machine.Instance.Robot.MovePosXYAndReply(cameraLineStart,
FluidProgram.Current.MotionSettings.VelXY,
FluidProgram.Current.MotionSettings.AccXY);
if (!ret.IsOk)
{
return(ret);
}
LineCmdLine lineCmdline = directiveLine.LineCmd.lineCmdLine;
if (lineCmdline == null || line == null)
{
return(ret);
}
Machine.Instance.IsProducting = false;
Line.WaitMsg.Reset();
switch (lineCmdline.LineMethod)
{
case LineMethod.Single:
MsgCenter.Broadcast(MsgType.MSG_LINEEDITLOOK_SHOW, directiveLine, lineCmdline);
break;
case LineMethod.Multi:
case LineMethod.Poly:
MsgCenter.Broadcast(MsgType.MSG_LINEEDITLOOK_SHOW, directiveLine, lineCmdline, directiveLine.LineCoordinates.IndexOf(line));
break;
default:
break;
}
Line.WaitMsg.WaitOne();
Machine.Instance.IsProducting = true;
}
else if (Machine.Instance.Valve1.RunMode == Drive.ValveSystem.ValveRunMode.InspectRect)
{
ret = Machine.Instance.Robot.MovePosXYAndReply(cameraLineStart,
FluidProgram.Current.MotionSettings.VelXY,
FluidProgram.Current.MotionSettings.AccXY);
if (!ret.IsOk)
{
return(ret);
}
LineCmdLine lineCmdline = directiveLine.LineCmd.lineCmdLine;
if (lineCmdline == null || line == null)
{
return(ret);
}
if (lineCmdline.LineMethod != LineMethod.Single)
{
return(ret);
}
InspectionLine inspectionLine = InspectionMgr.Instance.FindBy((int)lineCmdline.inspectionKey) as InspectionLine;
if (inspectionLine != null)
{
Thread.Sleep(inspectionLine.SettlingTime);
double width1, width2;
Machine.Instance.CaptureAndInspect(inspectionLine);
width1 = inspectionLine.PhyWidth1;
width2 = inspectionLine.PhyWidth2;
Log.Dprint(inspectionLine.CurrResultStr);
string resline = string.Format("{0},{1},{2},{3}", Math.Round(cameraLineStart.X, 3), Math.Round(cameraLineStart.Y, 3), Math.Round(width1, 3), Math.Round(width2, 3));
CsvUtil.WriteLine(directiveLine.Program.RuntimeSettings.FilePathInspectRect, resline);
Thread.Sleep(inspectionLine.DwellTime);
}
}
return(ret);
}
// 倾斜到位
Log.Dprint("change tilt status : " + directiveLine.Tilt.ToString());
ret = Machine.Instance.Valve1.BufChangeValveTiltStatus(directiveLine.Tilt, FluidProgram.Current.MotionSettings.VelU, FluidProgram.Current.MotionSettings.AccU);
if (!ret.IsOk)
{
return(ret);
}
double currZ = Machine.Instance.Robot.PosZ;
//到起点，Z轴到点胶位置
if (currZ > targZ)
{
// 移动到加速区间起始位置
Log.Dprint("move to position XY : " + valveAccStart);
if (directiveLine.RunnableModule.Mode == ModuleMode.MainMode)
{
ret = Machine.Instance.Robot.MovePosXYABAndReply(valveAccStart, simulStart,
FluidProgram.Current.MotionSettings.VelXYAB,
FluidProgram.Current.MotionSettings.AccXYAB,
(int)Machine.Instance.Setting.CardSelect);
}
else
{
ret = Machine.Instance.Robot.BufMoveLnXY(valveAccStart,
FluidProgram.Current.MotionSettings.VelXY,
FluidProgram.Current.MotionSettings.AccXY);
}
if (!ret.IsOk)
{
return(ret);
}
Log.Dprint("move down to Z : " + targZ.ToString("0.000000") + ", DispenseGap=" + line.Param.DispenseGap.ToString("0.000000"));
ret = Machine.Instance.Robot.BufMovePosZ(targZ, line.Param.DownSpeed, line.Param.DownAccel);
if (!ret.IsOk)
{
return(ret);
}
}
else
{
Log.Dprint("move up to Z : " + targZ.ToString("0.000000") + ", DispenseGap=" + line.Param.DispenseGap.ToString("0.000000"));
ret = Machine.Instance.Robot.BufMovePosZ(targZ, line.Param.DownSpeed, line.Param.DownAccel);
if (!ret.IsOk)
{
return(ret);
}
// 移动到加速区间起始位置
Log.Dprint("move to position XY : " + valveAccStart);
if (directiveLine.RunnableModule.Mode == ModuleMode.MainMode)
{
ret = Machine.Instance.Robot.MovePosXYABAndReply(valveAccStart, simulStart,
FluidProgram.Current.MotionSettings.VelXYAB,
FluidProgram.Current.MotionSettings.AccXYAB,
(int)Machine.Instance.Setting.CardSelect);
}
else
{
ret = Machine.Instance.Robot.BufMoveLnXY(valveAccStart,
FluidProgram.Current.MotionSettings.VelXY,
FluidProgram.Current.MotionSettings.AccXY);
}
if (!ret.IsOk)
{
return(ret);
}
}
// 移动： 加速区间--线段区间--减速区间
Log.Dprint("fluid line, accStart=" + cameraAccStart + " start=" + cameraLineStart + ", end=" + cameraLineEnd + ", decelEnd=" + cameraDecEnd + ", spped=" + speed);
//printPoints(newPoints);
if (directiveLine.RunnableModule.Mode == ModuleMode.MainMode)
{
ret = Machine.Instance.DualValve.FluidLine(cameraAccStart, cameraLineStart, cameraLineEnd, cameraDecEnd, speed, cameraPoints, intervalSec, simulStart, cameraPoints2, FluidProgram.Current.MotionSettings.WeightAcc);
GlueManagerMgr.Instance.UpdateGlueRemainWeight(0, points.Length * directiveLine.Program.RuntimeSettings.SingleDropWeight);
GlueManagerMgr.Instance.UpdateGlueRemainWeight(1, points.Length * directiveLine.Program.RuntimeSettings.SingleDropWeight);
}
else if (directiveLine.RunnableModule.Mode == ModuleMode.DualFallow)
{
ret = Machine.Instance.DualValve.FluidLine(cameraAccStart, cameraLineStart, cameraLineEnd, cameraDecEnd, speed, cameraPoints, intervalSec, FluidProgram.Current.MotionSettings.WeightAcc);
GlueManagerMgr.Instance.UpdateGlueRemainWeight(0, points.Length * directiveLine.Program.RuntimeSettings.SingleDropWeight);
GlueManagerMgr.Instance.UpdateGlueRemainWeight(1, points.Length * directiveLine.Program.RuntimeSettings.SingleDropWeight);
}
else if (directiveLine.RunnableModule.Mode == ModuleMode.AssignMode2)
{
ret = Machine.Instance.Valve2.FluidLine(cameraAccStart, cameraLineStart, cameraLineEnd, cameraDecEnd, speed, cameraPoints, intervalSec, FluidProgram.Current.MotionSettings.WeightAcc);
GlueManagerMgr.Instance.UpdateGlueRemainWeight(1, points.Length * directiveLine.Program.RuntimeSettings.SingleDropWeight);
}
else
{
ret = Machine.Instance.Valve1.BufFluidLine(cameraAccStart, cameraLineStart, cameraLineEnd, cameraDecEnd, speed, cameraPoints, intervalSec, FluidProgram.Current.MotionSettings.WeightAcc);
GlueManagerMgr.Instance.UpdateGlueRemainWeight(0, points.Length * directiveLine.Program.RuntimeSettings.SingleDropWeight);
}
if (!ret.IsOk)
{
return(ret);
}
if (Machine.Instance.Valve1.RunMode == Drive.ValveSystem.ValveRunMode.Wet ||
Machine.Instance.Valve1.RunMode == Drive.ValveSystem.ValveRunMode.Dry)
{
Log.Dprint("RetractDistance : " + line.Param.RetractDistance);
// 减速区间运动完成后，x y 轴速度为0， 此时需要抬升一段高度
if (line.Param.RetractDistance > 0)
{
Log.Dprint("move up RetractDistance : " + line.Param.RetractDistance);
ret = Machine.Instance.Robot.BufMoveIncZ(line.Param.RetractDistance, line.Param.RetractSpeed, line.Param.RetractAccel);
if (!ret.IsOk)
{
return(ret);
}
}
}
return(ret);
}
/// <summary>
/// 计算螺杆阀运行非Poly线时所需的各个参数
/// </summary>
/// <param name="primaryPoints"></param>
/// <param name="simulPoints"></param>
/// <param name="lineParam"></param>
/// <returns></returns>
private SvValveFludLineParam GetSvValveParam(Line line, PointD startPos, PointD endPos, LineCoordinate lineCoordinate)
{
double startPosDelay = lineCoordinate.Param.PreMoveDelay;
PointD endPoint = endPos;
PointD[] transPoints = new PointD[0];//非Poly线为空
//计算线段的角度
double angleOfLine = Math.Atan2(endPos.Y - startPos.Y, endPos.X - startPos.X) * 180 / Math.PI;
//线段总长度
double sumDistance = Math.Sqrt(Math.Pow(endPos.Y - startPos.Y, 2) + Math.Pow(endPos.X - startPos.X, 2));
//计算以起点为基准的关胶距离
double shutOffDistance = Math.Abs(sumDistance - lineCoordinate.Param.ShutOffDistance);
PointD stopSprayPos = new PointD();
stopSprayPos.X = Math.Round(startPos.X + shutOffDistance * Math.Cos(angleOfLine * 2 * Math.PI / 360), 3);
stopSprayPos.Y = Math.Round(startPos.Y + shutOffDistance * Math.Sin(angleOfLine * 2 * Math.PI / 360), 3);
PointD backTrackPos = new PointD();
//计算以起点为基准的到回走时的终点距离
double backTrackDistance = sumDistance * (1 - lineCoordinate.Param.BacktrackDistance * 0.01);
backTrackPos.X = Math.Round(startPos.X + backTrackDistance * Math.Cos(angleOfLine * 2 * Math.PI / 360), 3);
backTrackPos.Y = Math.Round(startPos.Y + backTrackDistance * Math.Sin(angleOfLine * 2 * Math.PI / 360), 3);
PointD[] backTransPoints = new PointD[0];
//计算线速度，分重量线和普通线
double[] vel = new double[1];
if (line.LineCmd.IsWeightControl)
{
double lineWeight = lineCoordinate.Weight;
double flowVel;
FluidProgram.Current.RuntimeSettings.VavelSpeedDic.TryGetValue(FluidProgram.Current.RuntimeSettings.SvOrGearValveCurrSpeed, out flowVel);
double time = lineWeight * flowVel;
vel[0] = sumDistance / time;
}
else
{
vel[0] = lineCoordinate.Param.Speed;
}
double backTrackDelay = lineCoordinate.Param.Dwell;
double backTrackGap = lineCoordinate.Param.BacktrackGap;
double backTrackVel = lineCoordinate.Param.BacktrackSpeed;
return(new SvValveFludLineParam()
{
startPosDelay = startPosDelay,
endPos = endPos,
transPoints = transPoints,
stopSprayPos = stopSprayPos,
backTrackPos = backTrackPos,
backTransPoints = backTransPoints,
vels = vel,
backTrackDelay = backTrackDelay,
backTrackGap = backTrackGap,
backTrackVel = backTrackVel
});
}
/// <summary>
/// 计算在所有模式下，执行重量线或胶量模式时线命令所需要的参数
/// </summary>
private void CalculateWtLineParam(LineCoordinate line, Line directiveLine, out PointD accStartPoint, out PointD decelEndPoint, out double runSpeed, out PointD[] linePoints, out double fluidIntervalSec)
{
double accDistance = 0, decelDistance = 0;
double lineDistance = line.CalculateDistance();
// 计算点胶数量
int shots = 0;
if (directiveLine.LineCmd.lineCmdLine.IsWholeWtMode && directiveLine.LineCmd.lineCmdLine.LineMethod != LineMethod.Single)
{
shots = line.Dots;
}
else
{
shots = (int)((decimal)line.Weight / (decimal)directiveLine.Program.RuntimeSettings.SingleDropWeight);
}
// 计算时间 (需要将微秒单位转换成秒）
double totalTime = Machine.Instance.Valve1.SpraySec * shots;
// 计算速度
double wtSpeed = lineDistance / totalTime;
if (line.Param.WtCtrlSpeedValueType != LineParam.ValueType.COMPUTE)
{
wtSpeed = line.Param.WtCtrlSpeed > wtSpeed ? wtSpeed : line.Param.WtCtrlSpeed;
}
// 运动速度不能超过设定的最大速度
wtSpeed = wtSpeed > FluidProgram.Current.MotionSettings.WeightMaxVel ? FluidProgram.Current.MotionSettings.WeightMaxVel : wtSpeed;
Log.Dprint(string.Format("lineDistance={0}, weight={1} totalTime={2}, param.WtCtrlSpeedValueType={3}, param.WtCtrlSpeed={4}, wtSpeed={5}",
lineDistance, line.Weight, totalTime, line.Param.WtCtrlSpeedValueType, line.Param.WtCtrlSpeed, wtSpeed));
// 计算加速区间距离
if (line.Param.AccelDistanceValueType == LineParam.ValueType.COMPUTE)
{
accDistance = MathUtils.CalculateDistance(0, wtSpeed, Machine.Instance.Robot.AxisX.ConvertAcc2Mm(FluidProgram.Current.MotionSettings.WeightAcc));
Log.Dprint("Acc Distance COMPUTE wtSpeed=" + wtSpeed + ", WeightAcc=" + FluidProgram.Current.MotionSettings.WeightAcc
+ ", distance=" + accDistance);
}
else
{
accDistance = line.Param.AccelDistance;
}
// 计算减速区间距离
if (line.Param.DecelDistanceValueType == LineParam.ValueType.COMPUTE)
{
decelDistance = MathUtils.CalculateDistance(wtSpeed, 0, -Machine.Instance.Robot.AxisX.ConvertAcc2Mm(FluidProgram.Current.MotionSettings.WeightAcc));
Log.Dprint("Decel Distance COMPUTE wtSpeed=" + wtSpeed + ", WeightAcc=" + FluidProgram.Current.MotionSettings.WeightAcc
+ ", distance=" + decelDistance);
}
else
{
decelDistance = line.Param.DecelDistance;
}
// 计算加速区间起始位置和减速区间结束位置
PointD accStart = calAccStartPosition(line, accDistance);
PointD decelEnd = calDecelEndPosition(line, decelDistance);
// 计算点胶位置
PointD[] points = calFixedTotalOfDots(line, shots);
//基于时间控制
totalTime = lineDistance / wtSpeed;
double intervalSec = totalTime / (shots - 1);
double dispenseSec = Machine.Instance.Valve1.SpraySec;
if (intervalSec < dispenseSec)
{
intervalSec = dispenseSec;
points = calFixedTotalOfDots(line, (int)(totalTime / intervalSec) + 1);
}
accStartPoint = accStart;
decelEndPoint = decelEnd;
runSpeed = wtSpeed;
linePoints = points;
fluidIntervalSec = intervalSec;
}
