/// <summary>
/// 判断两线段是否相交
/// </summary>
/// <param name="line1Start"></param>
/// <param name="line1End"></param>
/// <param name="line2Start"></param>
/// <param name="line2End"></param>
/// <returns></returns>
private static bool isColide(PointD line1Start, PointD line1End, PointD line2Start, PointD line2End)
{
VectorD v1 = line1End - line1Start;
VectorD v2 = line2Start - line1Start;
VectorD v3 = line2End - line1Start;
double temp = v1.CrossProduct(v3) * v1.CrossProduct(v2);
return(temp < 0.0 || temp > Math.Exp(-6));
}
private async void Calibration()
{
double r = DataSetting.Default.CentorInCross.DistanceTo(DataSetting.Default.RPointInCross);
double delta = r * Math.Sin(Math.PI / 4);
PointD[] pointResult1 = new PointD[4];
PointD[] pointResult2 = new PointD[4];
//MoveLnXY(PointD pos, double vel, double acc)
PointD p1 = DataSetting.Default.CentorInCross + new PointD(delta, delta);
PointD p2 = DataSetting.Default.CentorInCross + new PointD(-delta, delta);
PointD p3 = DataSetting.Default.CentorInCross + new PointD(-delta, -delta);
PointD p4 = DataSetting.Default.CentorInCross + new PointD(delta, -delta);
await Task.Factory.StartNew(() => {
//针嘴下面
Machine.Instance.Robot.MovePosZ(DataSetting.Default.ZUp);
Machine.Instance.Robot.MoveLnXY(DataSetting.Default.CentorInCross, this.vel, this.acc);
Machine.Instance.Robot.MoveLnXY(p1, this.vel, this.acc);
pointResult1[0] = this.MoveAndCapturePos(p2);
pointResult1[1] = this.MoveAndCapturePos(p3);
pointResult1[2] = this.MoveAndCapturePos(p4);
pointResult1[3] = this.MoveAndCapturePos(p1);
//针嘴上面
Machine.Instance.Robot.MovePosZ(DataSetting.Default.ZDown);
pointResult2[0] = this.MoveAndCapturePos(p2);
pointResult2[1] = this.MoveAndCapturePos(p3);
pointResult2[2] = this.MoveAndCapturePos(p4);
pointResult2[3] = this.MoveAndCapturePos(p1);
Machine.Instance.Robot.MoveSafeZ();
});
VectorD v21 = pointResult2[0] - pointResult2[2];
VectorD v22 = pointResult2[1] - pointResult2[3];
VectorD v11 = pointResult1[0] - pointResult1[2];
VectorD v12 = pointResult1[1] - pointResult1[3];
double dis1 = v11.CrossProduct(v21);
double dis2 = v12.CrossProduct(v22);
double lmt = (double)this.nudLmt.Value;
double diff = Math.Sqrt(dis1 * dis1 + dis2 * dis2);
this.lblInfo.Text = diff.ToString("0.000");
if (diff > lmt)
{
this.lblInfo.ForeColor = Color.Red;
}
this.lblInfo.ForeColor = Color.Green;
}
/// <summary>
/// 计算过渡圆弧方向（顺时针或者逆时针）
/// 0：顺时针，1：逆时针
/// </summary>
/// <returns></returns>
public static int GetArcDirect(PointD start, PointD center, PointD end)
{
//过渡圆弧只有劣弧
int clockwise = 0;
VectorD vStart = start - center;
VectorD vEnd = end - center;
// 使用向量叉乘判断起点到终点的方向（只有劣弧不需要考虑平行的情况）
double temp = vStart.CrossProduct(vEnd);
if (temp > 0)
{
clockwise = 1;
}
return(clockwise);
}
