public static PosXYZ AffineTransform(PosXYZ pos, TransformParams trans)
{
return(new PosXYZ(CoordAlignHelper.AffineTransform(pos.Data(), trans.ToDoubles()))
{
Z = pos.Z
});
}
private int TriggerCamera(PosXYZ pos, int index)
{
if (pos.Description == "barcode")
{
var ret = Camera.TriggerBarcode();
if (!ret)
{
Log($"{Camera} TriggerBarcode Error");
Product.Error = "BARCODEERROR";
}
Log($"{Camera} TriggerBarcode {index} {(ret ? "OK" : "ERROR")} {Camera.GetResult(string.Empty)} {Camera.LastError}");
}
else
{
//only add index on trigger product
index++;
Thread.Sleep(CfgSettings.CaptureDelay);
var ret = Camera.TriggerProduct(index);
if (!ret)
{
Log($"{Camera} TriggerProduct {index} Error");
Product.Error = $"CAPTURE{index}ERROR";
}
Log($"{Camera} TriggerProduct {index} {(ret ? "OK" : "ERROR")} {Camera.GetResult(string.Empty)} {Camera.LastError}");
}
return(index);
}
public double Distance(PosXYZ pos)
{
var offset = new Vector3d(pos.X, pos.Y, pos.Z) - Origin;
var distVector = offset.Cross(Direction);
return(distVector.Length);
}
public static PosXYZ AlignTransform(PosXYZ pos, double[,] trans)
{
return(new PosXYZ(RigidAlign.Transform(pos.Data(), trans))
{
Z = pos.Z
});
}
public static PosXYZ AffineInverseTransform(TransformParams trans, PosXYZ pos)
{
return(new PosXYZ(RigidAlign.AffineInverseTransform(pos.Data(), trans.ToDoubles()))
{
Z = pos.Z
});
}
private void buttonImportAddCurPos_Click(object sender, EventArgs e)
{
var platform = Platforms.FirstOrDefault(p => p.Name == comboBoxCurPlatform.Text);
if (platform == null)
{
return;
}
var data = platform.GetPos(comboBoxCurPosType.Text);
if (data == null)
{
MessageBox.Show($"Get Cur Pos Convert Error: Platform GetPos Fail {comboBoxCurPosType.Text}");
return;
}
if (platform is PlatformXyz)
{
var pos = new PosXYZ(data);
richTextBoxTestPos.Text += $"{pos.X:F2} {pos.Y:F2} {pos.Z:F2}\r\n";
}
else if (platform is PlatformXyzu)
{
var pos = new PosXYZU(data);
richTextBoxTestPos.Text += $"{pos.X:F2} {pos.Y:F2} {pos.Z:F2} {pos.U:F2}\r\n";
}
else
{
MessageBox.Show($"Get Cur Pos Convert Error : {platform.GetType()} Error");
return;
}
}
public double DistanceToLinePlane(PosXYZ pos)
{
var plane = new PlaneParams();
plane.Origin = Origin;
plane.Normal = Direction.Cross(new Vector3d(1, 0, 0));
return(plane.Point2Plane(pos));
}
public void UpdatePos(List <PosXYZ> pos)
{
var center = new PosXYZ(pos.Select(p => p.X).Average(), pos.Select(p => p.Y).Average(), pos.Select(p => p.Z).Average());
_posDisplay = pos.Select(p => new PosXYZ(p.X - center.X, p.Y - center.Y, p.Z - center.Z)
{
Status = p.Status
}).ToList();
}
public void RecordMoveFinish(PosXYZ p)
{
_sw.Stop();
var last = _movements.Last();
last.X = Math.Abs(p.X - last.X);
last.Y = Math.Abs(p.Y - last.Y);
last.Z = Math.Abs(p.Z - last.Z);
last.OffsetX = _sw.ElapsedMilliseconds;
}
protected override int ResetLoop()
{
//load cfg
CfgSettings = Machine.Ins.Settings;
//clear vio
VioTransInp.SetVio(this, false);
VioBarcodeFinish.SetVio(this, false);
VioTransFinish.SetVio(this, false);
VioMeasureFinish.SetVio(this, false);
//load axis pos
Platform.AssertPosTeached("Wait", this);
Platform.AssertPosTeached("GtWork1", this);
Platform.AssertPosTeached("GtWork2", this);
Platform.AssertPosTeached($"Barcode{Project.TypeId}", this);
PosGtWork1 = Platform["GtWork1"] as PosXYZ;
PosGtWork2 = Platform["GtWork2"] as PosXYZ;
if (!DoGTCylinder.SetDo(this, false, ignoreOrWaringOrError: true))
{
ThrowException($"GT Cylinder Reset Fail!");
}
//connect gt controller and barcode controller
try
{
if (BarcodeController.IsOpen)
{
BarcodeController.Close();
}
BarcodeController.Open(BarcodeCOM);
}
catch (Exception e)
{
ThrowException($"Barcode Open Fail:{e.Message}");
}
//home platform
Platform.EnterAuto(this).Servo();
Platform.EnterAuto(this).Home(new[] { 1, 1, 0 }, -1);
Platform.EnterAuto(this).MoveAbs("Wait", checkLimit: false);
//move gt cylinder push pos
if (!DoGTCylinder.SetDo(this, true, ignoreOrWaringOrError: true))
{
//show alarm
ThrowException($"GT Cylinder Set Fail!");
}
return(0);
}
private bool AddPosOffset(PosXYZ pos, out int gt, out PosXYZ gtWork, out PosXYZ newpos)
{
//add sys offset
if (pos.Description == "GT1")
{
gt = 1;
var leftGtSysOffset = CfgSettings.Common.LeftGT1SYSOffset;
var rightGtSysOffset = CfgSettings.Common.RightGT1SYSOffset;
gtWork = PosGtWork1;
if (Station.Id == 1)
{
newpos = pos + leftGtSysOffset;
}
else if (Station.Id == 2)
{
newpos = pos + rightGtSysOffset;
}
else
{
newpos = PosXYZ.Zero;
return(false);
}
}
else if (pos.Description == "GT2")
{
gt = 2;
var leftGtSysOffset = CfgSettings.Common.LeftGT2SYSOffset;
var rightGtSysOffset = CfgSettings.Common.RightGT2SYSOffset;
gtWork = PosGtWork2;
if (Station.Id == 1)
{
newpos = pos + leftGtSysOffset;
}
else if (Station.Id == 2)
{
newpos = pos + rightGtSysOffset;
}
else
{
newpos = PosXYZ.Zero;
return(false);
}
}
else
{
gt = 0;
newpos = PosXYZ.Zero;
gtWork = PosXYZ.Zero;
return(false);
}
return(true);
}
private PosXYZ AddPosOffset(PosXYZ pos)
{
var newpos = PosXYZ.Zero;
if (Station.Id == 1)
{
newpos = pos + Machine.Ins.Settings.Common.LeftGTSYSOffset;
}
else if (Station.Id == 2)
{
newpos = pos + Machine.Ins.Settings.Common.RightGTSYSOffset;
}
return(newpos);
}
public double GetEdge(PosXYZ p1, PosXYZ p2, double jh)
{
var x = Math.Abs(p2.X - p1.X) / XVel;
var y = Math.Abs(p2.Y - p1.Y) / YVel;
if (x < float.Epsilon && y < float.Epsilon)
{
return(0);
}
var z = Math.Abs(p2.Z - p1.Z) + 2 * jh;
z = z / ZVel;
return(x + y + z);
}
private void SelectJumpHeight(ref PosXYZ newPos, PosXYZ pos)
{
//get focus z
var focusBarcode = Platform["FocusBarcode"]?.Data()[2];
var focusInnerZ = Platform["FocusInner"]?.Data()[2];
var focusOuterZ = Platform["FocusOuter"]?.Data()[2];
var focusProfileZ = Platform["FocusProfile"]?.Data()[2];
var focusPedZ = Platform["FocusPed"]?.Data()[2];
//select obj pos z height
//switch (pos.Description)
//{
// case "barcode":
// case "Barcode":
// newPos.Z = focusBarcode ?? 0;
// break;
// case "inner":
// case "Inner":
// newPos.Z = focusInnerZ ?? 0;
// break;
// case "outer":
// case "Outer":
// newPos.Z = focusOuterZ ?? 0;
// break;
// case "profile":
// case "Profile":
// newPos.Z = focusProfileZ ?? 0;
// break;
// case "ped":
// case "Ped":
// newPos.Z = focusPedZ ?? 0;
// break;
// default:
// Log($"POS ERROR : {index} {pos} Not Found Z Focus");
// continue;
//}
//update product height
//if (newPos.Z > 0)
//{
// newPos.Z = newPos.Z - productHeight;
//}
}
public void Test_RigidTransform()
{
var p1 = new PosXYZ(1, 1, 0);
var p2 = new PosXYZ(1, 5, 0);
var p3 = new PosXYZ(1, 10, 0);
Console.WriteLine($"p1:{p1}");
Console.WriteLine($"p2:{p2}");
Console.WriteLine($"p3:{p3}");
var p1r = p1.RotateAt(new PosXYZ(), 2).Translate(new PosXYZ(10, 10, 0));
var p2r = p2.RotateAt(new PosXYZ(), 2).Translate(new PosXYZ(10, 10, 0));
var p3r = p3.RotateAt(new PosXYZ(), 2).Translate(new PosXYZ(10, 10, 0));
Console.WriteLine($"p1r:{p1r}");
Console.WriteLine($"p2r:{p2r}");
Console.WriteLine($"p3r:{p3r}");
var ret = XyzPlarformCalibration.CalcAlignTransform(new List <PosXYZ>()
{
p1, p2, p3
}, new List <PosXYZ>()
{
p1r, p2r, p3r
});
for (int i = 0; i < ret.Item1.GetLength(0); i++)
{
for (int j = 0; j < ret.Item1.GetLength(1); j++)
{
Console.Write(ret.Item1[i, j].ToString("F3"));
Console.Write(",");
}
Console.WriteLine();
}
Console.WriteLine();
}
public override void DoCalib()
{
Platform1?.EnterAuto(this).MoveAbs("Wait");
//上工站点位数据采集
Log($"上平台Align点位标定\n------------------------------------------------------");
List <PosXYZ> upAlignPos = new List <PosXYZ>();
bool isFirst = true;
foreach (var pos in AlignPosUp)
{
if (isFirst)
{
isFirst = false;
Platform1?.EnterAuto(this).Jump(pos, 0);
}
else
{
Platform1?.EnterAuto(this).Jump(pos, JumpHeight1);
}
upAlignPos.Add(pos);
DataList.Add(pos.ToString());
Log($"CurPos {pos.ToString()}", LogLevel.Info);
}
//复位平台1
Platform1?.EnterAuto(this).Jump("Wait", JumpHeight1);
Log($"上平台Align点位标定 完成\n------------------------------------------------------");
OnCalibProgress(50);
Platform2?.EnterAuto(this).MoveAbs("Wait");
//下工站点位 数据采集
Log($"下平台Align点位标定\n------------------------------------------------------");
List <PosXYZ> downAlingPos = new List <PosXYZ>();
isFirst = true;
foreach (var pos in AlignPosDown)
{
if (isFirst)
{
isFirst = false;
Platform2?.EnterAuto(this).Jump(pos, 0);
}
else
{
Platform2?.EnterAuto(this).Jump(pos, JumpHeight2);
}
downAlingPos.Add(pos);
DataList.Add(pos.ToString());
Log($"CurPos {pos.ToString()}", LogLevel.Info);
}
Log($"下平台Align点位标定 完成\n------------------------------------------------------");
//计算下平台GT2偏移
Log($"下平台GT1GT2偏移标定\n------------------------------------------------------");
if (Platform2GtOffsetCalibGT1 != null && Platform2GtOffsetCalibGT2 != null)
{
Platform2?.EnterAuto(this).Jump(Platform2GtOffsetCalibGT1, JumpHeight1);
Platform2?.EnterAuto(this).Jump(Platform2GtOffsetCalibGT2, JumpHeight1);
var gt1 = Platform2GtOffsetCalibGT1 as PosXYZ;
var gt2 = Platform2GtOffsetCalibGT2 as PosXYZ;
OutputPlatform2GtOffset = gt2 - gt1;
OutputPlatform2GtOffset.Z = 0;
}
//复位平台2
Platform2?.EnterAuto(this).Jump("Wait", JumpHeight2);
Log($"下平台GT1GT2偏移标定 完成\n------------------------------------------------------");
//复位治具
PlatformCarrier?.EnterAuto(this).MoveAbs("Wait");
OnCalibProgress(100);
//计算上下平台坐标转换
{
var ret = XyzPlarformCalibration.CalcAffineTransform(
upAlignPos.Select(p => new PosXYZ(p.X, p.Y, 0)).ToList(),
downAlingPos.Select(p => new PosXYZ(p.X, p.Y, 0)).ToList());
Log($"计算上下平台转换矩阵:\r\n"
+ $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6},{ret.Item1[0, 3]:F6}\r\n"
+ $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6},{ret.Item1[1, 3]:F6}\r\n"
+ $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6},{ret.Item1[2, 3]:F6}\r\n"
+ $"{ret.Item1[3, 0]:F6},{ret.Item1[3, 1]:F6},{ret.Item1[3, 2]:F6},{ret.Item1[3, 3]:F6}\r\n", LogLevel.Info);
Log($"计算上下平台转换误差:\r\n{ret.Item2:F2}\r\n", LogLevel.Info);
OutputTransForm = new TransformParams(ret.Item1);
}
Log($"GT1GT2偏移:\r\n{OutputPlatform2GtOffset}\r\n", LogLevel.Info);
}
public void RecordMoveStart(PosXYZ p)
{
sw.Restart();
MoveLength.Add(p);
}
public override void DoCalib()
{
OutputStandardHeight = StandardHeight;
Log("上平台GT高度标定\n----------------------------------------------------------");
{
//GT标定高度
OutputGTCalibPos = Platform1GTCalibPos;
DataList.Add(OutputGTCalibPos.ToString());
//标准平面标定
if (Platform1GTPlaneCalibPos != null)
{
//update z
if (Platform1 != null && GtController != null)
{
bool isFirst = true;
foreach (var calibPos in Platform1GTPlaneCalibPos)
{
//product to up platform
var pos = new PosXYZ(calibPos.Data())
{
Z = Platform1GTCalibPos.Z
};
if (isFirst)
{
isFirst = false;
Platform1?.EnterAuto(this).Jump(Platform1.GetPos("P->UP", pos.Data()), 0);
}
else
{
Platform1?.EnterAuto(this).Jump(Platform1.GetPos("P->UP", pos.Data()), JumpHeight1);
}
Thread.Sleep(1000);
calibPos.Z = GtController.ReadData()[0];
DataList.Add(calibPos.ToString());
}
}
var fitplane = PlaneParams.FitPlane(Platform1GTPlaneCalibPos);
OutputUpStandardPlane = new PlaneParams()
{
Normal = fitplane.Normal, Origin = fitplane.Origin
};
DataList.Add(OutputUpStandardPlane.ToString());
}
}
//复位上平台
Platform1?.EnterAuto(this).Jump("Wait", JumpHeight1);
Log("上平台GT高度标定 完成\n----------------------------------------------------------");
//下GT高度标定
//复位下平台Z轴
if (Platform2 != null)
{
Platform2.EnterAuto(this).Home(2);
Platform2.EnterAuto(this).MoveAbs(2, "Wait", checkLimit: false);
}
Log("下平台GT1高度标定\n----------------------------------------------------------");
{
do_gt2_cy?.SetDo(this, true, ignoreOrWaringOrError: null);
{
//GT1GT2 高度差标定
OutputGT1CalibPos = Platform2GT1CalibPos;
//update z
if (Platform2 != null && GtController != null)
{
Platform2.EnterAuto(this).Jump(Platform2GT1CalibPos, 0);
Thread.Sleep(1000);
OutputGT1CalibPos.OffsetZ = GtController.ReadData()[1];
}
//GT1GT2 高度差标定
OutputGT2CalibPos = Platform2GT2CalibPos;
//update z
if (Platform2 != null && GtController != null)
{
Platform2.EnterAuto(this).Jump(Platform2GT2CalibPos, JumpHeight2);
Thread.Sleep(1000);
OutputGT2CalibPos.OffsetZ = GtController.ReadData()[2];
}
//GT1标准平面标定
if (Platform2GT1PlaneCalibPos != null)
{
//update z
if (Platform2 != null && GtController != null)
{
foreach (var calibPos in Platform2GT1PlaneCalibPos)
{
//product to up platform
var pos = new PosXYZ(calibPos.Data())
{
Z = Platform2GT1CalibPos.Z
};
Platform2.EnterAuto(this).Jump(Platform2.GetPos("P->DOWN1", pos.Data()), JumpHeight2);
Thread.Sleep(1000);
calibPos.Z = GtController.ReadData()[1];
DataList.Add(calibPos.ToString());
}
}
var fitplane = PlaneParams.FitPlane(Platform2GT1PlaneCalibPos);
OutputDownStandardPlane = new PlaneParams()
{
Normal = fitplane.Normal, Origin = fitplane.Origin
};
DataList.Add(OutputDownStandardPlane.ToString());
}
//GT2标准平面标定
if (Platform2GT2PlaneCalibPos != null)
{
//update z
if (Platform2 != null && GtController != null)
{
foreach (var calibPos in Platform2GT2PlaneCalibPos)
{
//product to up platform
var pos = new PosXYZ(calibPos.Data())
{
Z = Platform2GT2CalibPos.Z
};
Platform2.EnterAuto(this).Jump(Platform2.GetPos("P->DOWN2", pos.Data()), JumpHeight2);
Thread.Sleep(1000);
calibPos.Z = GtController.ReadData()[2];
DataList.Add(calibPos.ToString());
}
}
var fitplane = PlaneParams.FitPlane(Platform2GT2PlaneCalibPos);
OutputDownStandardPlane2 = new PlaneParams()
{
Normal = fitplane.Normal, Origin = fitplane.Origin
};
DataList.Add(OutputDownStandardPlane2.ToString());
}
}
do_gt2_cy?.SetDo(this, false, ignoreOrWaringOrError: null);
}
{
//复位下平台
//move platform2 wait
if (Platform2 != null)
{
Platform2.EnterAuto(this).Home(2);
Platform2.EnterAuto(this).MoveAbs("Wait", checkLimit: false);
}
Log("下平台GT1/2高度标定 完成\n----------------------------------------------------------");
}
//output
DataList.Add(OutputGT1CalibPos.ToString());
DataList.Add(OutputGT2CalibPos.ToString());
DataList.Add(OutputStandardHeight.ToString());
DataList.Add(OutputUpStandardPlane.ToString());
DataList.Add(OutputDownStandardPlane.ToString());
}
protected override int RunLoop()
{
//start assert
Platform.AssertAutoMode(this);
Platform.LocateInPos("Wait");
Project.AssertNoNull(this);
//safe height check
var safeHeight = Platform["SafeHeight"]?.Data()[2];
var productHeight = Project.Height;
safeHeight = safeHeight - productHeight;
if (Platform.CurPos[2] > safeHeight)
{
Log($"Platform {Platform.Name} Z SAFE Height Error: {Platform.CurPos[2]:F2} > {safeHeight:F2}", LogLevel.Error);
}
//wait start
VioMeasureStart.WaitVioAndClear(this);
{
//assert
Product.AssertNoNull(this);
//clear client recv buffer
try
{
var msg1 = Camera.GetResult("result", 1);
Log($"RunLoop Start Clear Client Buffer: {msg1}");
}
catch (Exception ex)
{
Log($"RunLoop Start Clear Client Buffer Error: {ex.Message}");
}
var isFirst = true;
PosXYZ lastPos = null;
int index = 0;
//start measure loop
foreach (var pos in Project.CapturePos)
{
var newPos = pos;
if (CfgEnableRelCoordMode)
{
//conver FILE POS TO MOVE POS
newPos = new PosXYZ(Platform.GetPos("MOVE", pos.Data()));
newPos.Name = pos.Name;
newPos.Description = pos.Description;
Log($"EnableRelCoordMode Transform {pos} To {newPos}");
}
if (newPos.Z > safeHeight)
{
Log($"{newPos.Z} > Z limit ERROR", LogLevel.Error);
}
//optimize jump move
{
if (isFirst)
{
isFirst = false;
Platform.Jump(newPos, 0, zLimit: -12);
}
else
{
//select jumpHeight
if (newPos.Z < lastPos.Z)
{
var jumpHeight = -8 + newPos.Z - lastPos.Z;
Platform.Jump(newPos, jumpHeight, zLimit: -12);
}
else
{
Platform.Jump(newPos, 0, zLimit: -12);
}
}
lastPos = newPos;
}
//capture
index = TriggerCamera(pos, index);
if (CfgSettings.QuitOnProductError)
{
if (Product.Status == ProductStatus.ERROR)
{
Log($"Quit {Name} Loop On Error: {Product.Error}");
break;
}
}
}
//process result
try
{
if (string.IsNullOrEmpty(Product.Error))
{
var result = Camera.GetResult("result");
var msgData = result.Split(',');
Product.Status = msgData[0] == "OK" ? ProductStatus.OK : ProductStatus.NG;
if (msgData.Length > 2)
{
var spcItems = new List <Tuple <string, double> >();
//parse msgData to spcs
for (int i = 1; i < msgData.Length; i++)
{
if (string.IsNullOrEmpty(msgData[i]))
{
continue;
}
var spc = msgData[i].Split(':');
if (spc.Length >= 2)
{
try
{
var s = spc[0].Split('_')[1];
var val = double.Parse(spc[1]);
spcItems.Add(new Tuple <string, double>(s, val));
Product.RawData.Add(val);
}
catch (Exception e)
{
Log($"{spc[0]} {spc[1]} SetSpcError:{e.Message}");
}
}
}
//set spc
foreach (var s in spcItems)
{
Product.SetSpcItem(s.Item1, s.Item2);
}
}
Log($"Camera GetResult: {result}");
}
Product.SetSpcItem("STS", Product.Status == ProductStatus.OK ? 0 : 2);
}
catch (Exception ex)
{
Log($"Camera GetResult Exception: {ex.Message}");
}
Platform.MoveAbs("Wait");
}
//measure finish
VioMeasureFinish.SetVio(this);
return(0);
}
protected override int RunLoop()
{
//in case of manual operations
Platform.AssertAutoMode(this);
Platform.LocateInPos("Wait");
//check safe height
var safeHeight = Platform["GtWork"].Data()[2] - Project.Height - 6;
if (Platform.CurPos[2] > safeHeight)
{
Log($"{Name} {Platform.Name} SafeHeightError:{Platform.CurPos[2]:F2}>{safeHeight:F2}", LogLevel.Error);
}
//wait vio start
VioTransFinish.WaitVioAndClear(this);
{
//move wait
Platform.MoveAbs("Wait");
//measure up test positions
bool isFirst = true;
foreach (var pos in Project.UpTestPositions)
{
//transform gt work
var newpos = AddPosOffset(pos);
var gtWorkZ = Platform["GtWork"].Data()[2] - Project.Height;
var gtWork = new PosXYZ(Platform.GetPos("P->UP", newpos.Data()))
{
Z = gtWorkZ
};
Log($"Transform {pos.Name} {pos.Description} {newpos} To {gtWork}");
//move gt work
if (isFirst)
{
isFirst = false;
Platform.Jump(gtWork, 0);
}
else
{
var maxJumpHeight = -Platform.CurPos[2];
if (PlatformJumpHeight < maxJumpHeight)
{
Log($"{Platform.Name} Jump Height {PlatformJumpHeight} > {maxJumpHeight}", LogLevel.Warning);
PlatformJumpHeight = maxJumpHeight + 0.5;
}
Platform.Jump(gtWork, PlatformJumpHeight);
}
//read gt raw
Thread.Sleep(CfgSettings.Common.GtReadDelay);
var gtRaw = GtController?.ReadData();
if (gtRaw != null)
{
Product.RawDataUp.Add(new PosXYZ()
{
Name = pos.Name,
Description = pos.Description,
X = pos.X,
Y = pos.Y,
Z = gtRaw[0],
OffsetX = gtRaw[0],
OffsetZ = gtWorkZ,
});
}
}
//move wait
if (Project.UpTestPositions.Count > 0)
{
Platform.Jump("Wait", PlatformJumpHeight);
}
}
//set vio finish
VioMeasureFinish.SetVio(this);
return(0);
}
/// <summary>
/// 转换当前点gt到产品坐标系
/// </summary>
/// <param name="platform"></param>
/// <param name="calib"></param>
/// <param name="gtPos"></param>
/// <returns></returns>
public static double TransGtRaw(string platform, CalibrationConfig calib, PosXYZ gtPos)
{
var gtWork = gtPos.OffsetZ;
var gtRaw = gtPos.Z;
var gtDesc = gtPos.Description;
var gtWorkX = gtPos.X;
var gtWorkY = gtPos.Y;
var gt = 0;
switch (gtDesc)
{
case "GT":
gt = 0;
break;
case "GT1":
gt = 1;
break;
case "GT2":
gt = 2;
break;
default:
throw new Exception("gt Error");
}
var gtCalibWork = 0d;
var gtCalibRaw = 0d;
var gtStandardHeight = 0d;
var gtDirection = false;
if (platform == "LeftStation")
{
if (gt == 0)
{
//gt convert to calib coord
gtCalibWork = calib.LeftHeightCalibGtPos.Z;
gtCalibRaw = calib.LeftUpStandardPlaneGT.CalcZ(gtWorkX, gtWorkY);
gtStandardHeight = calib.LeftHeightStandard.Z;
gtDirection = false;
}
else if (gt == 1)
{
//gt1 convert to calib coord
gtCalibWork = calib.LeftHeightCalibGt1Pos.Z;
gtCalibRaw = calib.LeftDownStandardPlaneGT1.CalcZ(gtWorkX, gtWorkY);
gtStandardHeight = 0;
gtDirection = true;
}
else if (gt == 2)
{
//gt2 convert to calib coord
gtStandardHeight = 0;
var gtCalibWork2 = calib.LeftHeightCalibGt2Pos.Z;
var gtCalibRaw2 = calib.LeftDownStandardPlaneGT2.CalcZ(gtWorkX, gtWorkY);
var gt1OffsetZ = calib.LeftDownStandardPlaneGT1.CalcZ(gtWorkX, gtWorkY);;
gtDirection = true;
var gt2Raw = GTTransform.TransGT2ToGT1(gtWork, gtRaw, gtCalibWork2, gtCalibRaw2, gtStandardHeight, gtDirection) + gt1OffsetZ;
//gt1 convert to calib coord
gtCalibWork = calib.LeftHeightCalibGt1Pos.Z;
gtCalibRaw = calib.LeftDownStandardPlaneGT1.CalcZ(gtWorkX, gtWorkY);
gtStandardHeight = 0;
gtWork = calib.LeftHeightCalibGt1Pos.Z;
gtRaw = gt2Raw;
return(GTTransform.TransGT2ToGT1(gtWork, gtRaw, gtCalibWork, gtCalibRaw, gtStandardHeight, gtDirection) + calib.LeftGt2ZOffset.OffsetZ);
}
}
else if (platform == "RightStation")
{
if (gt == 0)
{
//gt convert to calib coord
gtCalibWork = calib.RightHeightCalibGtPos.Z;
gtCalibRaw = calib.RightUpStandardPlaneGT.CalcZ(gtWorkX, gtWorkY);
gtStandardHeight = calib.RightHeightStandard.Z;
gtDirection = false;
}
else if (gt == 1)
{
//gt1 convert to calib coord
gtCalibWork = calib.RightHeightCalibGt1Pos.Z;
gtCalibRaw = calib.RightDownStandardPlaneGT1.CalcZ(gtWorkX, gtWorkY);
gtStandardHeight = 0;
gtDirection = true;
}
else if (gt == 2)
{
//single point calib convert
//gtStandardHeight = 0;
//gtDirection = true;
//var gt2Raw = calib.RightHeightCalibGt1Pos.OffsetZ +
// GTTransform.TransGT2ToGT1(gtWork, gtRaw, calib.RightHeightCalibGt2Pos.Z, calib.RightHeightCalibGt2Pos.OffsetZ, gtStandardHeight, gtDirection);
//plane calib convert
//gt2 convert to calib pos
gtStandardHeight = 0;
var gtCalibWork2 = calib.RightHeightCalibGt2Pos.Z;
var gtCalibRaw2 = calib.RightDownStandardPlaneGT2.CalcZ(gtWorkX, gtWorkY);
var gt1OffsetZ = calib.RightDownStandardPlaneGT1.CalcZ(gtWorkX, gtWorkY);;
gtDirection = true;
var gt2Raw = GTTransform.TransGT2ToGT1(gtWork, gtRaw, gtCalibWork2, gtCalibRaw2, gtStandardHeight, gtDirection) + gt1OffsetZ;
//gt1 convert to calib pos
gtCalibWork = calib.RightHeightCalibGt1Pos.Z;
gtCalibRaw = calib.RightDownStandardPlaneGT1.CalcZ(gtWorkX, gtWorkY);
gtStandardHeight = 0;
gtWork = calib.RightHeightCalibGt1Pos.Z;
gtRaw = gt2Raw;
return(GTTransform.TransGT2ToGT1(gtWork, gtRaw, gtCalibWork, gtCalibRaw, gtStandardHeight, gtDirection) + calib.RightGt2ZOffset.OffsetZ);
}
}
else
{
throw new Exception("Platform Error");
}
return(GTTransform.TransGT2ToGT1(gtWork, gtRaw, gtCalibWork, gtCalibRaw, gtStandardHeight, gtDirection));
}
public void RecordMoveStart(PosXYZ p)
{
_sw.Restart();
_movements.Add(p);
}
private void MeasureDownGt()
{
//start measure
if (Project.DownTestPositions.Count == 0)
{
Log($"MeasureGt No Test Points");
return;
}
//run measure loop
bool isFirst = true;
PosXYZ lastPos = PosXYZ.Zero;
foreach (var pos in Project.DownTestPositions)
{
int gt;
PosXYZ gtWorkZ;
PosXYZ newpos;
//add pos offset
if (!AddPosOffset(pos, out gt, out gtWorkZ, out newpos))
{
Log($"AddPosOffset GT Raw Index {gt} Error");
continue;
}
//convert pos to gt pos
var gtPosZ = gtWorkZ.Z - pos.Z;
var gtPos = PosXYZ.Zero;
if (gt == 1)
{
gtPos = new PosXYZ(Platform.GetPos("P->DOWN1", newpos.Data()))
{
Z = gtPosZ
};
}
else if (gt == 2)
{
gtPos = new PosXYZ(Platform.GetPos("P->DOWN2", newpos.Data()))
{
Z = gtPosZ
};
}
else
{
Log($"GT Raw Index {gt} Error", LogLevel.Error);
}
Log($"Transform {pos.Name} {pos.Description} {newpos} To {gtPos}");
//jump gt pos
PlatformJumpPos(isFirst, pos, lastPos, gtPos);
lastPos = pos;
isFirst = false;
//read gt
System.Threading.Thread.Sleep(CfgSettings.Common.GtReadDelay);
var gtRaw = GtController?.ReadData();
if (gtRaw != null)
{
Product.RawDataDown.Add(new PosXYZ()
{
Name = pos.Name,
Description = pos.Description,
X = pos.X,
Y = pos.Y,
Z = gtRaw[gt],
OffsetX = gtRaw[gt],
OffsetZ = gtPosZ,
});
}
else
{
Log($"GtController ReadData Error", LogLevel.Error);
}
}
}
private void PlatformJumpPos(bool isFirst, PosXYZ pos, PosXYZ lastPos, PosXYZ gtPos)
{
if (isFirst)
{
if (pos.Name == "outer" || pos.Name == "i" || pos.Name == "h")
{
if (!DoGTCylinder.SetDo(this, false, ignoreOrWaringOrError: true))
{
ThrowException($"GT Cylinder RESET Fail!");
}
}
Log($"{Platform.Name} {Platform.Description} Jump {pos}");
Platform.Jump(gtPos, 0);
}
else
{
//jump max height
if (pos.Name == "outer" || pos.Name == "down" || pos.Name == "i" || pos.Name == "h")
{
if (pos.Name == "outer" || pos.Name == "i" || pos.Name == "h")
{
if (!DoGTCylinder.SetDo(this, false, ignoreOrWaringOrError: true))
{
ThrowException($"GT Cylinder RESET Fail!");
}
}
else
{
if (lastPos.Name == "outer")
{
if (!DoGTCylinder.SetDo(this, true, ignoreOrWaringOrError: true))
{
ThrowException($"GT Cylinder SET Fail!");
}
}
}
var outerJumpHeight = 0.5 - Platform.CurPos[2];
Log($"{Platform.Name} {Platform.Description} Jump {pos}");
Platform.Jump(gtPos, outerJumpHeight);
}
else
{
//jump normal height
if (lastPos.Name == "outer")
{
if (!DoGTCylinder.SetDo(this, true, ignoreOrWaringOrError: true))
{
ThrowException($"GT Cylinder SET Fail!");
}
}
//calculate jump height
var jumpHeight = 0d;
if (pos.Z > lastPos.Z)
{
//need long dist jump
jumpHeight = lastPos.Z - pos.Z + PlatformJumpHeight;
//check jump height
var lastGtValue = Product.RawDataDown.Last().OffsetX + 0.2;
var a147offset = 2d;
var gtOffset = pos.Description == "GT1" ? 0 : 3;
var minJumpHeight = lastPos.Z - pos.Z - (lastGtValue + a147offset + gtOffset);
if (jumpHeight > minJumpHeight)
{
//not enough jump height to avoid gt1 collision
Log($"not enough jump height {jumpHeight} to avoid gt1 collision {minJumpHeight:F2}", LogLevel.Warning);
jumpHeight = minJumpHeight;
}
}
else
{
jumpHeight = PlatformJumpHeight;
//check jump height
var lastGtValue = Product.RawDataDown.Last().OffsetX + 0.2;
var a147offset = 2d;
var gtOffset = pos.Description == "GT1" ? 0 : 3;
var minJumpHeight = -(lastGtValue + a147offset + gtOffset);
if (jumpHeight > minJumpHeight)
{
//not enough jump height to avoid gt1 collision
Log($"not enough jump height {jumpHeight} to avoid gt1 collision {minJumpHeight:F2}", LogLevel.Warning);
jumpHeight = minJumpHeight;
}
}
Log($"{Platform.Name} {Platform.Description} Jump {pos}");
Platform.Jump(gtPos, jumpHeight);
}
}
}
private void buttonUpdatePosData_Click(object sender, EventArgs e)
{
var newPos = new List <IPlatformPos>();
var data = richTextBoxPosData.Lines;
var line = string.Empty;
if (TestPos.Count > 0 && TestPos.First().GetType() == typeof(PosXYZ))
{
try
{
foreach (var l in data)
{
if (string.IsNullOrEmpty(l))
{
continue;
}
line = l;
newPos.Add(PosXYZ.Create(l));
}
}
catch (Exception ex)
{
MessageBox.Show($"PosXYZ点位数据异常： {line} {ex.Message}");
return;
}
}
else if (TestPos.Count > 0 && TestPos.First().GetType() == typeof(PosXYZU))
{
try
{
foreach (var l in data)
{
if (string.IsNullOrEmpty(l))
{
continue;
}
line = l;
newPos.Add(PosXYZU.Create(l));
}
}
catch (Exception ex)
{
MessageBox.Show($"PosXYZU点位数据异常： {line} {ex.Message}");
return;
}
}
else if (TestPos.Count > 0 && TestPos.First().GetType() == typeof(PosXYZUVW))
{
try
{
foreach (var l in data)
{
if (string.IsNullOrEmpty(l))
{
continue;
}
line = l;
newPos.Add(PosXYZUVW.Create(l));
}
}
catch (Exception ex)
{
MessageBox.Show($"PosXYZUVW点位数据异常： {line} {ex.Message}");
return;
}
}
else
{
return;
}
if (MessageBox.Show($"是否更新点位为：\r\n{string.Join("\r\n", newPos.Select(p => p.ToString()))} ", "更新点位数据", MessageBoxButtons.YesNo) == DialogResult.No)
{
return;
}
try
{
TestPos.Clear();
TestPos.AddRange(newPos);
LoadTestPos();
}
catch (Exception ex)
{
MessageBox.Show($"更新点位数据异常:{ex.Message}");
}
}
public newRightMeasureDown(int id, string name, Station station) : base(id, name, station)
{
DoGTCylinder = Machine.Ins.Find <ICylinderEx>("RGTCylinder");
VioTransInp = Machine.Ins.Find <IVioEx>("RTransInp");
VioBarcodeFinish = Machine.Ins.Find <IVioEx>("RBarcodeFinish");
VioTransFinish = Machine.Ins.Find <IVioEx>("RTransFinishDown");
VioMeasureFinish = Machine.Ins.Find <IVioEx>("RMeasureFinishDown");
Platform = Machine.Ins.Find <PlatformEx>("RightDown");
Platform.SafeChecks.Clear();
Platform.SafeChecks.Add(new DownSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), DoGTCylinder, SafeCheckType.AutoHome));
Platform.SafeChecks.Add(new DownSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), DoGTCylinder, SafeCheckType.Auto));
Platform.SafeChecks.Add(new DownSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), DoGTCylinder, SafeCheckType.ManualHome));
Platform.SafeChecks.Add(new DownSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), DoGTCylinder, SafeCheckType.Manual));
var pToDown1 = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightUpTransform);
pnew = XyzPlarformCalibration.AffineTransform(pnew, Machine.Ins.Settings.Calibration.RightTransform);
return(pnew.Data());
});
var pToDown2 = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightUpTransform);
pnew = XyzPlarformCalibration.AffineTransform(pnew, Machine.Ins.Settings.Calibration.RightTransform);
pnew = pnew + Machine.Ins.Settings.Calibration.RightGtOffset;
return(pnew.Data());
});
var down1ToP = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightTransform, new PosXYZ(d));
pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightUpTransform, pnew);
return(pnew.Data());
});
var down2ToP = new Func <double[], double[]>(d =>
{
var pnew = new PosXYZ(d) - Machine.Ins.Settings.Calibration.RightGtOffset;
pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightTransform, pnew);
pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightUpTransform, pnew);
return(pnew.Data());
});
var upToDown1 = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightTransform);
return(pnew.Data());
});
var down1ToUp = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightTransform, new PosXYZ(d));
return(pnew.Data());
});
var upToDown2 = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightTransform);
pnew = pnew + Machine.Ins.Settings.Calibration.RightGtOffset;
return(pnew.Data());
});
var down2ToUp = new Func <double[], double[]>(d =>
{
var pnew = new PosXYZ(d) - Machine.Ins.Settings.Calibration.RightGtOffset;
pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightTransform, pnew);
return(pnew.Data());
});
Platform.PosConvertFuncs.Add("P->DOWN1", pToDown1);
Platform.PosConvertFuncs.Add("P->DOWN2", pToDown2);
Platform.PosConvertFuncs.Add("DOWN1->P", down1ToP);
Platform.PosConvertFuncs.Add("DOWN2->P", down2ToP);
Platform.PosConvertFuncs.Add("UP->DOWN1", upToDown1);
Platform.PosConvertFuncs.Add("DOWN1->UP", down1ToUp);
Platform.PosConvertFuncs.Add("UP->DOWN2", upToDown2);
Platform.PosConvertFuncs.Add("DOWN2->UP", down2ToUp);
}
/// <summary>
///
/// </summary>
/// <param name="msg"></param>
/// <param name="pos"></param>
public virtual bool Trigger(string msg, PosXYZ pos)
{
return(false);
}
public void TestMethod_Calc_Method20180820()
{
//load settings
var settings = MachineSettings.Load(@".\Config2\Settings.cfg");
//load raw data
List <Thermo1Product> rawDatas = new List <Thermo1Product>();
{
var fprj = FlatnessProject.Load(@".\Config2\A117NoFin_pf_30.fprj");
var file = @".\Config2\20180810 LEFT.csv";
var lines = File.ReadAllLines(file);
foreach (var line in lines)
{
var testData = new Thermo1Product()
{
ProductType = fprj.ProductSettings.ProductName,
SPCItems = fprj.ProductSettings.SPCItems,
};
var strs = line.Split(',');
//raw up
{
var gtZ = 0;// settings.AxisPos.LUpPlatformPos.First(p => p.Name == "GtWork").Z - fprj.ProductSettings.Height;
var strIndex = 13;
for (int i = 0; i < 8; i++)
{
var pos = new PosXYZ(fprj.ProductSettings.UpTestPositions[i].X, fprj.ProductSettings.UpTestPositions[i].Y, double.Parse(strs[i + strIndex]))
{
Name = "up",
Description = "GT",
OffsetX = double.Parse(strs[i + strIndex]),
OffsetZ = gtZ,
};
testData.RawDataUp.Add(pos);
}
}
//raw ped1
{
var gtZ = 0;// settings.AxisPos.LDownPlatformPos.First(p => p.Name == "GtWork1").Z;
var strIndex = 22;
for (int i = 0; i < 5; i++)
{
var pos = new PosXYZ(fprj.ProductSettings.DownTestPositions[i].X, fprj.ProductSettings.DownTestPositions[i].Y, double.Parse(strs[i + strIndex]))
{
Name = "ped1",
Description = "GT1",
OffsetX = double.Parse(strs[i + strIndex]),
OffsetZ = gtZ,
};
testData.RawDataDown.Add(pos);
}
}
//raw inner standoff
{
var gtZ = 0;// settings.AxisPos.LDownPlatformPos.First(p => p.Name == "GtWork2").Z;
var strIndex = 27;
for (int i = 0; i < 4; i++)
{
var pos = new PosXYZ(fprj.ProductSettings.DownTestPositions[i + 5].X, fprj.ProductSettings.DownTestPositions[i + 5].Y, double.Parse(strs[i + strIndex]))
{
Name = "inner",
Description = "GT2",
OffsetX = double.Parse(strs[i + strIndex]),
OffsetZ = gtZ,
};
testData.RawDataDown.Add(pos);
}
}
rawDatas.Add(testData);
}
}
{
var p = rawDatas.Last();
Console.WriteLine($"{string.Join(",", p.RawDataUp.Select(ps => ps.X.ToString("F3")))}");
Console.WriteLine($"{string.Join(",", p.RawDataUp.Select(ps => ps.Y.ToString("F3")))}");
Console.WriteLine($"{string.Join(",", p.RawDataDown.Select(ps => ps.X.ToString("F3")))}");
Console.WriteLine($"{string.Join(",", p.RawDataDown.Select(ps => ps.Y.ToString("F3")))}");
foreach (var data in p.RawDataUp)
{
Console.WriteLine($"pos.Add(new PosXYZ({data.X},{data.Y},{data.Z}) {{ Status = {data.Status.ToString().ToLower()} }}); ");
}
foreach (var data in p.RawDataDown)
{
Console.WriteLine($"pos.Add(new PosXYZ({data.X},{data.Y},{data.Z}) {{ Status = {data.Status.ToString().ToLower()} }}); ");
}
}
}
public bool Trigger(string msg, PosXYZ pos)
{
return(true);
}
public double Point2Plane(PosXYZ pos)
{
var v1 = new Vector3d(pos.X, pos.Y, pos.Z);
return((v1 - Origin).Dot(Normal));
}