public CaliParam()
{
this.camLightPara = new CameraLightPara();
this.localPara = new LocalPara();
this.AngleRange = 10.0;
this.StartCaliPt = new Point3Db();
this.EndCaliPt = new Point3Db();
this.IsMoveX = false;
this.IsMoveY = false;
this.caliModel = CaliModelEnum.HandEyeCali;
this.HomMat = new MyHomMat2D();
this.ListRectRegion = new List <RectangleF>();
this.ListPt2D = new List <Point2Db>();
this.cam = CameraEnum.Cam0;
this.describe = "未定义";
this.CoordiCam = CoordiCamHandEyeMatEnum.Coordi0Cam0;
}
/// <summary>
/// 扎针定位
/// </summary>
/// <param name="PlcItem"></param>
/// <param name="Obj"></param>
public void DoProject(LD.Config.PlcDataItem PlcItem, Object Obj)
{
LD.Common.PlcDevice plcDevice = PlcItem.PlcDevice;
if (plcDevice == LD.Common.PlcDevice.V_01_TriggerGrab) //
{
NowVisionProcess = new DoVisionProcess();
//起始信号清零
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.V_01_TriggerGrab, 0);
FileLib.Logger.Pop("接受到PLC信号，开始扎针对位", false, "运行日志");
short ClearValue = 1;
object obj = new object();
while (true)
{
obj = LD.Logic.PlcHandle.Instance.ReadValue(LD.Common.PlcDevice.V_01_TriggerGrab);
ClearValue = (short)obj;
if (ClearValue == 0)
{
break;
}
else
{
System.Threading.Thread.Sleep(10);
}
}
System.Threading.Thread.Sleep(1);
//读取机台编号
int StageNum = (int)LD.Logic.PlcHandle.Instance.ReadValue(LD.Common.PlcDevice.ArtTestStageNumRead);
FileLib.Logger.Pop(" 读取机台编号：" + StageNum.ToString(), false, "运行日志");
System.Threading.Thread.Sleep(1);
//读取拍照位编号
short GrabNum = (short)LD.Logic.PlcHandle.Instance.ReadValue(LD.Common.PlcDevice.ArtTestGrabNum);
System.Threading.Thread.Sleep(1);
//获取视觉示教的定位参数
NowProjectPara = ProjectParaManager.Instance.GetProjectPara(StageNum);
//获取当前拍照点的视觉坐标
NowVisionPara = NowProjectPara.GetVisionPara(GrabNum);
NowVisionProcess.SetVisionPara(NowVisionPara);
NowVisionProcess.Do();
if (GrabNum == 0)
{
FirstLocalResult = NowVisionProcess.MyLocalResult;
FileLib.Logger.Pop(" 左边拍照定位完成，开始右边拍照定位：", false, "运行日志");
}
else
{
SecondLocalResult = NowVisionProcess.MyLocalResult;
//示教点到旋转中心的坐标
Point2Db TeachPt2d1 = new Point2Db(FirstLocalResult.TeachPosToRot.Col, FirstLocalResult.TeachPosToRot.Row);
Point2Db TeachPt2d2 = new Point2Db(SecondLocalResult.TeachPosToRot.Col, SecondLocalResult.TeachPosToRot.Row);
FileLib.Logger.Pop(" 示教产品到旋转中心的坐标(左右两点)：" + TeachPt2d1.Col.ToString("f3") + " " + TeachPt2d1.Row.ToString("f3") + " "
+ TeachPt2d2.Col.ToString("f3") + " " + TeachPt2d2.Row.ToString("f3"), false, "运行日志");
//当前产品到旋转中心的坐标
Point2Db NowPosPt2d1 = new Point2Db(FirstLocalResult.PosToRot.Col, FirstLocalResult.PosToRot.Row);
Point2Db NowPosPt2d2 = new Point2Db(SecondLocalResult.PosToRot.Col, SecondLocalResult.PosToRot.Row);
FileLib.Logger.Pop(" 当前产品到旋转中心的坐标(左右两点)：" + NowPosPt2d1.Col.ToString("f3") + " " + NowPosPt2d1.Row.ToString("f3") + " "
+ NowPosPt2d2.Col.ToString("f3") + " " + NowPosPt2d2.Row.ToString("f3"), false, "运行日志");
List <Point2Db> TeachPtList = new List <Point2Db>();
List <Point2Db> NowPtList = new List <Point2Db>();
//当前产品移动到产品示教时的位置，先计算出偏移矩阵
TeachPtList.Add(TeachPt2d1);
TeachPtList.Add(TeachPt2d2);
NowPtList.Add(NowPosPt2d1);
NowPtList.Add(NowPosPt2d2);
MyHomMat2D NowHomat = new MyHomMat2D();
bool IsTrue = false;
MyVisionBase.VectorToRigidHomMat(NowPtList, TeachPtList, out NowHomat, out IsTrue);
double AddX = 0, AddY = 0, AddTheta = 0;
MyVisionBase.CalculateThreeTapePos(NowPosPt2d1, NowPosPt2d2, TeachPt2d1, TeachPt2d2, out AddX, out AddY, out AddTheta);
FileLib.Logger.Pop(" 计算出的偏移补偿量：" + AddX.ToString("f3") + " " + AddY.ToString("f3") + " " + AddTheta.ToString("f3"), false, "运行日志");
///再根据偏移矩阵计算出偏移量
double AddX1 = 0, AddY1 = 0, AddTheta1 = 0;
MyVisionBase.HomMat2dToAffinePara(NowHomat, out AddTheta1, out AddY1, out AddX1);
ViewControl view1 = DisplaySystem.GetViewControl(CameraTest.UpCam1);
view1.SetString(500, 500, "red", " AddX： " + AddX.ToString("f4") + " mm ");
view1.SetString(500, 600, "red", " AddY： " + AddY.ToString("f4") + " mm ");
view1.SetString(500, 700, "red", " AddTheta： " + AddTheta.ToString("f4") + " 度 ");
///
double addx0 = -AddX * 1000;
double addy0 = AddY * 1000;
double addtheta0 = AddTheta * 1000;
int addx = (int)addx0;
int addy = (int)addy0;
int addtheta = (int)addtheta0;
NowVisionPara = NowProjectPara.GetVisionPara(0);
int offsetx = (int)(NowVisionPara.localPara.localSetting.Offset_x * 1000);
int offsety = (int)(NowVisionPara.localPara.localSetting.Offset_y * 1000);
int offsettheta = (int)(NowVisionPara.localPara.localSetting.Offset_theta * 1000);
int newAddX = addx + offsetx;
int NewAddY = addy + offsety;
int NewAddTheta = -addtheta + offsettheta;
if (StageNum == 0 || StageNum == 2)//左边两个平台X轴为正常方向，后面两个平台为反方向
{
newAddX = addx + offsetx;
}
else
{
newAddX = -addx + offsetx;
}
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestAddX, newAddX);
System.Threading.Thread.Sleep(5);
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestAddY, NewAddY);
System.Threading.Thread.Sleep(5);
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestAddTheta, NewAddTheta);//图像坐标系为逆时针，机械坐标系为顺时针
System.Threading.Thread.Sleep(5);
int ReadAddX = 0;
int ReadAddY = 0;
int ReadAddTheta = 0;
object ObjRead = 0;
int WriteCount = 0;
while (true)
{
ObjRead = LD.Logic.PlcHandle.Instance.ReadValue(LD.Common.PlcDevice.ArtTestAddX);
System.Threading.Thread.Sleep(5);
ReadAddX = (int)ObjRead;
ObjRead = LD.Logic.PlcHandle.Instance.ReadValue(LD.Common.PlcDevice.ArtTestAddY);
System.Threading.Thread.Sleep(5);
ReadAddY = (int)ObjRead;
ObjRead = LD.Logic.PlcHandle.Instance.ReadValue(LD.Common.PlcDevice.ArtTestAddTheta);
System.Threading.Thread.Sleep(5);
ReadAddTheta = (int)ObjRead;
if (ReadAddX == newAddX && ReadAddY == NewAddY && ReadAddTheta == NewAddTheta)
{
FileLib.Logger.Pop(" 补偿值成功写入", false, "运行日志");
break;
}
else
{
WriteCount++;
if (WriteCount % 10 == 0)
{
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestAddX, newAddX);
System.Threading.Thread.Sleep(5);
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestAddY, NewAddY);
System.Threading.Thread.Sleep(5);
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestAddTheta, NewAddTheta);//图像坐标系为逆时针，机械坐标系为顺时针
System.Threading.Thread.Sleep(5);
}
if (WriteCount > 300)
{
break;
}
}
}
if (SecondLocalResult.IsLocalOk)
{
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestCamLocalResult, 1); //告诉PLC定位结果OK
FileLib.Logger.Pop(" 告诉PLC补偿量已经发给PLC：", false, "运行日志");
}
}
}
}
public override void Do()
{
//1.打开光源
foreach (LightPara item in MyVisionPara.camLightPara.lightPara)
{
LightCtrlManager.Instance.SetLightValue(item);
}
//2.采图
if (GrabImg != null)
{
GrabImg.Dispose();
}
CameraCtrl.Instance.SetExpos(MyVisionPara.camLightPara.CamName, MyVisionPara.camLightPara.Exposure);
if (CameraCtrl.Instance.GrabImg(MyVisionPara.camLightPara.CamName, out GrabImg))
{
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.FOF_Grabed_ok, 1); //拍照OK
}
else
{
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.FOF_Grabed_ok, 2); //拍照NG
}
ViewControl view1 = DisplaySystem.GetViewControl(CameraTest.UpCam1);
if (MyVisionPara.camLightPara.CamName == CameraEnum.Cam0)
{
view1 = DisplaySystem.GetViewControl(CameraTest.UpCam1);
}
if (MyVisionPara.camLightPara.CamName == CameraEnum.Cam1)
{
view1 = DisplaySystem.GetViewControl(CameraTest.UpCam2);
}
if (MyVisionPara.camLightPara.CamName == CameraEnum.Cam2)
{
view1 = DisplaySystem.GetViewControl(CameraTest.UpCam3);
}
view1.ResetView();
view1.Refresh();
view1.AddViewImage(GrabImg);
view1.Repaint();
MyVisionBase.SaveImg(GrabImg, "扎针对位图片");
//3.关闭光源
LightCtrlManager.Instance.SetAllLightTo0();
//4.0设置定位模式
MyLocal.SetLocalModel(MyVisionPara.localPara.localSetting.localModel);
//5.0设置定位参数
MyLocal.SetParam(GrabImg, MyVisionPara.localPara);
//6.执行定位
try
{
MyLocal.doLocal();
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.FOF_Grabed_result, 1); //告诉PLC定位结果OK
}
catch (Exception e0)
{
Logger.PopError1(e0, false, "视觉日志");
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.FOF_Grabed_result, 2); //告诉PLC定位结果OK
}
//7.告诉PLC定位结果完成
MyLocalResult = MyLocal.GetResult();
view1.AddViewObject(MyLocalResult.ShowContour);
//结合标定坐标计算出产品的实际位置
MyHomMat2D HomMat = new MyHomMat2D();
HomMat = CaliParaManager.Instance.GetHomMAT(MyVisionPara.localPara.localSetting.CoordiCam); //获取示教的标定矩阵
HalconDotNet.HTuple HHomMat = new HalconDotNet.HTuple();
//标定矩阵的转换
MyVisionBase.MyHomMatToHalcon(HomMat, out HHomMat);
//图像坐标系的原点由左上角变到左下角
MyVisionBase.AdjImgRow(GrabImg, ref MyLocalResult.row);
St_VectorAngle PixelVector = new St_VectorAngle(MyLocalResult.row, MyLocalResult.col, MyLocalResult.angle);
CaliValue CaliPara = CaliParaManager.Instance.GetCaliValue(MyVisionPara.localPara.localSetting.CoordiCam);
//将像素坐标转换到标定坐标
//MyLocalResult.AffineTransResult()
HalconDotNet.HTuple HomMatCali = new HalconDotNet.HTuple();
MyVisionBase.MyHomMatToHalcon(CaliPara.HomMat, out HomMatCali);
MyVisionBase.AffineTransPt(MyLocalResult.col, MyLocalResult.row, HomMatCali, out MyLocalResult.x, out MyLocalResult.y);
//MyVisionBase.AffineTransPt()
//获取当前坐标
//double X = 0, Y = 0, Z = 0, Theta = 0;
//if (CaliParaManager.Instance.GetCaliMode(MyVisionPara.localPara.localSetting.TeachCoordi) == CaliModelEnum.HandEyeCali)
//{
// MotionManager.Instance.SetCoordi(MyVisionPara.localPara.localSetting.TeachCoordi);
// MotionManager.Instance.GetCoordiPos(out X, out Y, out Z, out Theta);
//}
//VectorAngle GrabPos = new VectorAngle(X, Y, Theta);
////9 计算出当前产品距离旋转中心的坐标
//EyeToHandPos.TransEyeToHandPos(PixelVector, CaliPara, GrabPos, out PosToRot);
////10计算出示教产品到旋转中心的坐标
//St_VectorAngle TeachPixelPos = new St_VectorAngle(MyLocalResult.TeachRow, MyLocalResult.TeachCol, MyLocalResult.TeachAngle);
//VectorAngle TeachGrabPos = new VectorAngle(MyLocalResult.TeachX, MyLocalResult.TeachY, MyLocalResult.TeachTheta);
//MyVisionBase.AdjImgRow(GrabImg, ref TeachPixelPos.Row);
//EyeToHandPos.TransEyeToHandPos(TeachPixelPos, CaliPara, TeachGrabPos, out TeachPosToRot);
//view1.Repaint();
//view1.SetString(100, 100, "red", "PosToRot: " + " X： " + PosToRot.Col.ToString("f3") + " Y： " +
// PosToRot.Row.ToString("f3") + " Theta： " + PosToRot.Angle.ToString("f3"));
//MyLocalResult.PosToRot = PosToRot;
//MyLocalResult.TeachPosToRot = TeachPosToRot;
}
/// <summary>
/// 计算直线1的夹角
/// </summary>
/// <param name="Row1"></param>
/// <param name="Col1"></param>
/// <param name="Row2"></param>
/// <param name="Col2"></param>
/// <param name="CurHandEyeMat"></param>
/// <param name="Th"></param>
/// <returns></returns>
public static bool CalculateLineTh(double Row1, double Col1, double Row2, double Col2, MyHomMat2D CurHandEyeMat, out double Th)
{
Th = 0;
try {
//调整原点位置
Row1 = 2448 - Row1;
Row2 = 2448 - Row2;
Point2Db Pt1 = new Point2Db(true), Pt2 = new Point2Db(true);
Pt1.Row = Row1;
Pt1.Col = Col1;
Pt2.Row = Row2;
Pt2.Col = Col2;
Point2Db WorldPt1 = new Point2Db(true), WorldPt2 = new Point2Db(true);
MyVisionBase.AffineTransPoint2D(Pt1, CurHandEyeMat, out WorldPt1);
MyVisionBase.AffineTransPoint2D(Pt2, CurHandEyeMat, out WorldPt2);
HTuple AngleX = 0;
if (WorldPt1.Col < WorldPt2.Col)
{
HOperatorSet.AngleLl(0, 0, 1, 0, WorldPt1.Col, WorldPt1.Row, WorldPt2.Col, WorldPt2.Row, out AngleX);
Th = AngleX.D;
}
else
{
HOperatorSet.AngleLl(0, 0, 1, 0, WorldPt2.Col, WorldPt2.Row, WorldPt1.Col, WorldPt1.Row, out AngleX);
Th = AngleX.D;
}
return(true);
}
catch (Exception e0) {
Logger.PopError(e0.Message + e0.Source + e0.StackTrace);
return(false);
}
}
public virtual void Do()
{
//1.打开光源
FileLib.Logger.Pop(" 打开光源：", false, "运行日志");
foreach (LightPara item in MyVisionPara.camLightPara.lightPara)
{
LightCtrlManager.Instance.SetLightValue(item);
}
//2.采图
if (GrabImg != null)
{
GrabImg.Dispose();
}
FileLib.Logger.Pop(" 开始采图：", false, "运行日志");
CameraCtrl.Instance.SetExpos(MyVisionPara.camLightPara.CamName, MyVisionPara.camLightPara.Exposure);
//10.获取当前的机械坐标
double X = 0, Y = 0, Z = 0, Theta = 0;
if (CaliParaManager.Instance.GetCaliMode(MyVisionPara.localPara.localSetting.CoordiCam) == CaliModelEnum.HandEyeCali)
{
MotionManager.Instance.SetCoordi(MyVisionPara.localPara.localSetting.TeachCoordi);
FileLib.Logger.Pop("当前的坐标系为：" + MyVisionPara.localPara.localSetting.TeachCoordi.ToString(), false, "运行日志");
MotionManager.Instance.GetCoordiPos(out X, out Y, out Z, out Theta);
FileLib.Logger.Pop("当前的机械坐标", false, "运行日志");
FileLib.Logger.Pop("X：" + X.ToString("f4") + " Y：" + Y.ToString("f4") +
" Z：" + Z.ToString("f4") + " Theta：" + Theta.ToString("f4"), false, "运行日志");
}
if (CameraCtrl.Instance.GrabImg(MyVisionPara.camLightPara.CamName, out GrabImg))
{
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestCameraGrabed, 1); //拍照OK
FileLib.Logger.Pop(" 采图OK：", false, "运行日志");
}
else
{
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestCameraGrabed, 2); //拍照NG
FileLib.Logger.Pop(" 采图NG：", false, "运行日志");
}
ViewControl view1 = DisplaySystem.GetViewControl(CameraTest.UpCam1);
if (MyVisionPara.camLightPara.CamName == CameraEnum.Cam0)
{
view1 = DisplaySystem.GetViewControl(CameraTest.UpCam1);
}
if (MyVisionPara.camLightPara.CamName == CameraEnum.Cam1)
{
view1 = DisplaySystem.GetViewControl(CameraTest.UpCam2);
}
if (MyVisionPara.camLightPara.CamName == CameraEnum.Cam2)
{
view1 = DisplaySystem.GetViewControl(CameraTest.UpCam3);
}
view1.ResetView();
view1.Refresh();
view1.AddViewImage(GrabImg);
view1.Repaint();
MyVisionBase.SaveImg(GrabImg, "扎针对位图片");
//3.关闭光源
LightCtrlManager.Instance.SetAllLightTo0();
FileLib.Logger.Pop(" 关闭光源：", false, "运行日志");
//4.0设置定位模式
MyLocal.SetLocalModel(MyVisionPara.localPara.localSetting.localModel);
//5.0设置定位参数
MyLocal.SetParam(GrabImg, MyVisionPara.localPara);
//6.执行定位
try
{
MyLocal.doLocal();
//7.告诉PLC定位结果完成
if (MyVisionPara.ProjectVisionItem == ProjectVisionEnum.ProjectVision1)
{
FileLib.Logger.Pop(" 扎针右侧视觉定位完成，先不发给PLC信号，等计算出偏移量后，将信号发给PLC：", false, "运行日志");
}
else
{
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestCamLocalResult, 1); //告诉PLC定位结果OK
FileLib.Logger.Pop(" 告诉PLC拍照结果OK（告诉PLC定位结果OK）：", false, "运行日志");
}
}
catch (Exception e0)
{
Logger.PopError1(e0, false, "视觉错误日志");
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestCamLocalResult, 2); //告诉PLC定位结果NG
FileLib.Logger.Pop(" 告诉PLC拍照结果OK（告诉PLC定位结果NG）：", false, "运行日志");
MyLocalResult.IsLocalOk = false;
}
MyLocalResult = MyLocal.GetResult();
view1.AddViewObject(MyLocalResult.ShowContour);
//结合标定坐标计算出产品的实际位置
//8.获取标定矩阵
MyHomMat2D HomMat = new MyHomMat2D();
HomMat = CaliParaManager.Instance.GetHomMAT(MyVisionPara.localPara.localSetting.CoordiCam); //获取示教的标定矩阵
FileLib.Logger.Pop(" 获取示教的标定矩阵：" + MyVisionPara.localPara.localSetting.CoordiCam.ToString(), false, "运行日志");
FileLib.Logger.Pop(HomMat.ToString(), false, "运行日志");
HalconDotNet.HTuple HHomMat = new HalconDotNet.HTuple();
//标定矩阵的转换
MyVisionBase.MyHomMatToHalcon(HomMat, out HHomMat);
//9.图像坐标系的原点由左上角变到左下角
MyVisionBase.AdjImgRow(GrabImg, ref MyLocalResult.row);
St_VectorAngle PixelVector = new St_VectorAngle(MyLocalResult.row, MyLocalResult.col, MyLocalResult.angle);
FileLib.Logger.Pop("当前的像素坐标", false, "运行日志");
FileLib.Logger.Pop("Col：" + MyLocalResult.col.ToString("f4") + " Row：" + MyLocalResult.row.ToString("f4") +
" Theta：" + MyLocalResult.angle.ToString("f4"), false, "运行日志");
CaliValue CaliPara = CaliParaManager.Instance.GetCaliValue(MyVisionPara.localPara.localSetting.CoordiCam);
VectorAngle GrabPos = new VectorAngle(X, Y, Theta);
//9 计算出当前产品距离旋转中心的坐标
EyeToHandPos.TransEyeToHandPos(PixelVector, CaliPara, GrabPos, out PosToRot);
//10计算出示教产品到旋转中心的坐标
St_VectorAngle TeachPixelPos = new St_VectorAngle(MyLocalResult.TeachRow, MyLocalResult.TeachCol, MyLocalResult.TeachAngle);
VectorAngle TeachGrabPos = new VectorAngle(MyLocalResult.TeachX, MyLocalResult.TeachY, MyLocalResult.TeachTheta);
MyVisionBase.AdjImgRow(GrabImg, ref TeachPixelPos.Row);
EyeToHandPos.TransEyeToHandPos(TeachPixelPos, CaliPara, TeachGrabPos, out TeachPosToRot);
view1.Repaint();
view1.SetString(100, 100, "red", "PosToRot: " + " X： " + PosToRot.Col.ToString("f3") + " Y： " +
PosToRot.Row.ToString("f3") + " Theta： " + PosToRot.Angle.ToString("f3"));
MyLocalResult.PosToRot = PosToRot;
MyLocalResult.TeachPosToRot = TeachPosToRot;
FileLib.Logger.Pop("示教的产品相对旋转中心的偏移量", false, "运行日志");
FileLib.Logger.Pop("PosToRot: " + " X： " + PosToRot.Col.ToString("f3") + " Y： " +
PosToRot.Row.ToString("f3") + " Theta： " + PosToRot.Angle.ToString("f3"), false, "运行日志");
FileLib.Logger.Pop("当前的产品相对旋转中心的偏移量", false, "运行日志");
FileLib.Logger.Pop("PosToRot: " + " X： " + TeachPosToRot.Col.ToString("f3") + " Y： " +
TeachPosToRot.Row.ToString("f3") + " Theta： " + TeachPosToRot.Angle.ToString("f3"), false, "运行日志");
//11.计算出X Y Theta的补偿量，
////图像坐标转到机械坐标系
//MyVisionBase.AffineTransPt(MyLocalResult.col, MyLocalResult.row, HHomMat, out MyLocalResult.x, out MyLocalResult.y);
////
//MyVisionBase.AffineTransPt(MyLocalResult.TeachCol, MyLocalResult.TeachRow, HHomMat, out MyLocalResult.TeachX, out MyLocalResult.TeachY);
}
public override void Do()
{
//1.打开光源
FileLib.Logger.Pop(" 打开光源：", false, "FOF运行日志");
foreach (LightPara item in MyVisionPara.camLightPara.lightPara)
{
LightCtrlManager.Instance.SetLightValue(item);
}
//2.采图
FileLib.Logger.Pop(" 开始采图：", false, "FOF运行日志");
if (GrabImg != null)
{
GrabImg.Dispose();
}
if (FilterImg != null)
{
FilterImg.Dispose();
}
CameraCtrl.Instance.SetExpos(MyVisionPara.camLightPara.CamName, MyVisionPara.camLightPara.Exposure);
if (CameraCtrl.Instance.GrabImg(MyVisionPara.camLightPara.CamName, out GrabImg))
{
if (MyVisionPara.camLightPara.CamName == CameraEnum.Cam2)
{
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.FOF_Grabed_ok, 1); //拍照OK
}
FileLib.Logger.Pop(" 采图OK：", false, "FOF运行日志");
}
else
{
if (MyVisionPara.camLightPara.CamName == CameraEnum.Cam2)
{
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.FOF_Grabed_ok, 2); //拍照NG
}
FileLib.Logger.Pop(" 采图NG：", false, "FOF运行日志");
}
ViewControl view1 = new ViewControl();
view1 = DisplaySystem.GetViewControl(CameraTest.UpCam2);
if (MyVisionPara.camLightPara.CamName == CameraEnum.Cam2)
{
view1 = DisplaySystem.GetViewControl(CameraTest.UpCam3);
}
view1.ResetView();
view1.Refresh();
//3.关闭光源
//LightCtrlManager.Instance.SetAllLightTo0();
//FileLib.Logger.Pop(" 关闭光源：", false, "FOF运行日志");
//4.0设置定位模式
MyLocal.SetLocalModel(MyVisionPara.localPara.localSetting.localModel);
//5.0设置定位参数
FileLib.Logger.Pop(" 开始偏移检测：", false, "FOF运行日志");
MyVisionBase.SaveImg(GrabImg, "FOF偏移检测图片");
if (MyVisionPara.camLightPara.IsFilter)
{
FileLib.Logger.Pop(" 开始频域滤波，FilterC：" + MyVisionPara.camLightPara.FilterC.ToString(), false, "FOF运行日志");
MyVisionBase.FilterImg(GrabImg, out FilterImg, MyVisionPara.camLightPara.FilterC);
view1.AddViewImage(FilterImg);
MyVisionBase.SaveImg(FilterImg, "FOF偏移检测图片");
MyLocal.SetParam(FilterImg, MyVisionPara.localPara);
}
else
{
view1.AddViewImage(GrabImg);
MyLocal.SetParam(GrabImg, MyVisionPara.localPara);
}
view1.Repaint();
//6.执行定位
try
{
FileLib.Logger.Pop(" 开始定位：", false, "FOF运行日志");
MyLocal.doLocal();
if (MyVisionPara.camLightPara.CamName == CameraEnum.Cam2)
{
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.FOF_Grabed_result, 1);
}
FileLib.Logger.Pop(" 告诉PLC拍照结果OK（告诉PLC定位结果OK）：", false, "FOF运行日志");
}
catch (Exception e0)
{
if (MyVisionPara.camLightPara.CamName == CameraEnum.Cam2)
{
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.FOF_Grabed_result, 2);
}
Logger.PopError1(e0, false, "视觉错误日志");
FileLib.Logger.Pop(" 告诉PLC拍照结果OK（告诉PLC定位结果NG）：", false, "FOF运行日志");
}
MyLocalResult = MyLocal.GetResult();
view1.AddViewObject(MyLocalResult.ShowContour);
view1.Repaint();
//8.结合标定坐标计算出产品的实际位置
MyHomMat2D HomMat = new MyHomMat2D();
HomMat = CaliParaManager.Instance.GetHomMAT(MyVisionPara.localPara.localSetting.CoordiCam); //获取示教的标定矩阵
FileLib.Logger.Pop(" 获取标定矩阵(CoordiEnumItem)：" + MyVisionPara.localPara.localSetting.CoordiCam.ToString()
, false, "FOF运行日志");
FileLib.Logger.Pop(HomMat.ToString(), false, "FOF运行日志");
HalconDotNet.HTuple HHomMat = new HalconDotNet.HTuple();
//9.标定矩阵的转换
MyVisionBase.MyHomMatToHalcon(HomMat, out HHomMat);
//10.图像坐标系的原点由左上角变到左下角
MyVisionBase.AdjImgRow(GrabImg, ref MyLocalResult.row);
MyVisionBase.AffineTransPt(MyLocalResult.col, MyLocalResult.row, HHomMat, out MyLocalResult.x, out MyLocalResult.y);
FileLib.Logger.Pop("col: " + MyLocalResult.col.ToString("f3") + " row: " + MyLocalResult.row.ToString("f3"), false, "FOF运行日志");
FileLib.Logger.Pop("X: " + MyLocalResult.x.ToString("f3") + " Y: " + MyLocalResult.y.ToString("f3"), false, "FOF运行日志");
MyLocalResult.Theta = MyLocalResult.angle;
if (FilterImg != null)
{
FilterImg.Dispose();
}
if (GrabImg != null)
{
GrabImg.Dispose();
}
}
/// <summary>
/// 计算胶带或者金手指中心在平台中的坐标
/// </summary>
/// <param name="ImgIn"></param>
/// <param name="CurHandEyeHomMat">手眼标定矩阵</param>
/// <param name="IsDn">是否是下相机</param>
/// <param name="CurMotionPos">当前平台的坐标</param>
/// <param name="TeachCirclePara0">圆的示教参数</param>
/// <param name="Row1">模板示教时的行坐标</param>
/// <param name="Col1">模板示教时的列坐标</param>
/// <param name="Theta1">模板示教时的角度</param>
/// <param name="Row2"></param>
/// <param name="Col2"></param>
/// <param name="Theta2"></param>
/// <param name="CenterRow">胶带或者金手指的中心在平台中的行坐标</param>
/// <param name="CenterCol">胶带或者金手指的中心在平台中的列坐标</param>
/// <param name="RotAngle"></param>
/// <returns></returns>
public static En_ImageProcessResult FindCenter(HObject ImgIn, MyHomMat2D CurHandEyeHomMat, bool IsUp, Point2Db CurMotionPos, St_CirclesParam TeachCirclePara0, HTuple Row1, HTuple Col1, HTuple Theta1,
HTuple Row2, HTuple Col2, HTuple Theta2, out double CenterRow, out double CenterCol, out double RotAngle, out HObject circleCont, out HObject CirContour, out HObject centerCont)
{
CenterRow = 0;
CenterCol = 0;
RotAngle = 0;
St_CirclesParam TeachCirclePara;
TeachCirclePara = TeachCirclePara0;
HObject ObjContour = new HObject();
CirContour = new HObject();
HOperatorSet.GenEmptyObj(out CirContour);
HObject CircleObj = new HObject();
centerCont = new HObject();
circleCont = new HObject();
// string strLog = "";
//1.0调整示教的坐标
try
{
MyVisionBase.VectorAngleToTransPt(Row1, Col1, Theta1, Row2, Col2, Theta2, TeachCirclePara0.CenterRows, TeachCirclePara0.CenterCols, out TeachCirclePara.CenterRows, out TeachCirclePara.CenterCols);
//if (TeachCirclePara.IsThreeFeeder)
//{
// TeachCirclePara.CenterRows[2] = TeachCirclePara0.CenterRows[2];
// TeachCirclePara.CenterRows[3] = TeachCirclePara0.CenterRows[3];
// TeachCirclePara0.CenterCols[2] = TeachCirclePara0.CenterCols[2];
// TeachCirclePara0.CenterCols[3] = TeachCirclePara0.CenterCols[3];
//}
HTuple CircleRows = new HTuple(), CircleCols = new HTuple();
HObject RoiContour = new HObject();
HTuple ResultRows = new HTuple(), ResultCols = new HTuple(), ArcType0 = new HTuple();
HTuple CircleCenterRow = new HTuple(), CircleCenterCol = new HTuple(), CircleRadius = new HTuple();
HTuple StartPhi = new HTuple(), EndPhi = new HTuple(), PointOrder = new HTuple(), IsTrueFlag = new HTuple();
int cirCount;
#region //用示教的参数找出圆
for (int i = 0; i < TeachCirclePara.CenterRows.Count; i++)
{
//2.0利用存下来的圆的信息，创建圆的边缘点坐标
MyVisionBase.GenCirclePts2(TeachCirclePara.CenterRows[i], TeachCirclePara.CenterCols[i], TeachCirclePara.CircleRs[i], TeachCirclePara.StartPhis[i], TeachCirclePara.EndPhis[i]
, TeachCirclePara.PointOrders[i], out CircleRows, out CircleCols);
//3.0找出边缘点用来拟合圆
MyVisionBase.spoke2(ImgIn, TeachCirclePara.Elements[i], TeachCirclePara.DetectHeights[i], 2, 2, TeachCirclePara.Thresholds[i], "all", "first", CircleRows, CircleCols,
TeachCirclePara.Directs[i], out ResultRows, out ResultCols, out ArcType0);
HOperatorSet.GenCrossContourXld(out CircleObj, ResultRows, ResultCols, TeachCirclePara.CircleRs[i] / 10, 0.6);
HOperatorSet.ConcatObj(CircleObj, CirContour, out CirContour);
cirCount = CirContour.CountObj();
//4.拟合圆找到圆的中心
HObject CircleContour = new HObject();
HTuple FitRow = new HTuple(), FitCol = new HTuple(), FitR = new HTuple(), SartP = new HTuple(), EndP = new HTuple(), IsFlag = new HTuple();
MyVisionBase.PtsToBestCircle1(out CircleObj, ResultRows, ResultCols, 4, "arc", out FitRow, out FitCol, out FitR, out SartP, out EndP, out IsFlag);
if (Math.Abs(TeachCirclePara.CircleRs[i] - FitR.D) > TeachCirclePara.CircleRs[i] * 0.5)
{
if (ObjContour != null)
{
ObjContour.Dispose();
}
if (CircleObj != null)
{
CircleObj.Dispose();
}
Logger.PopError("拟合的圆的半径和理论值偏差过大。");
return(En_ImageProcessResult.圆半径和理论值偏差过大);
}
cirCount = CircleObj.CountObj();
HOperatorSet.ConcatObj(CircleObj, CirContour, out CirContour);
CircleCenterRow[i] = FitRow.D;
CircleCenterCol[i] = FitCol.D;
CircleRadius[i] = FitR.D;
StartPhi[i] = SartP.D;
EndPhi[i] = EndP.D;
IsTrueFlag[i] = IsFlag[0];
}
#endregion
HTuple AngleLxPixel = new HTuple(), AngleLx = new HTuple();
if (CircleCenterRow.TupleLength() != TeachCirclePara0.CenterRows.Count)
{
if (ObjContour != null)
{
ObjContour.Dispose();
}
if (CircleObj != null)
{
CircleObj.Dispose();
}
Logger.PopError("CircleTypePos.FindCenter" + "没有找到圆");
return(En_ImageProcessResult.圆的数量少于示教的数量);
}
#region //生成圆心十字轮廓，非功能性代码
HTuple cRow = new HTuple(), cColumn = new HTuple();
int count = CircleCenterRow.TupleLength();
cRow = 0;
cColumn = 0;
if (count < 4)
{
for (int i = 0; i < count; i++)
{
cRow += CircleCenterRow[i];
cColumn += CircleCenterCol[i];
}
HOperatorSet.GenCrossContourXld(out centerCont, cRow / count, cColumn / count, 150, 0);
}
else if (count == 4)
{
for (int i = 0; i < 2; i++)
{
cRow += CircleCenterRow[i];
cColumn += CircleCenterCol[i];
}
HOperatorSet.GenCrossContourXld(out centerCont, cRow / 2, cColumn / 2, 150, 0);
}
#endregion
//5.0X轴到圆的中心连线的角度
HTuple hv_HandEyeHomMat = new HTuple();
MyVisionBase.MyHomMatToHalcon(CurHandEyeHomMat, out hv_HandEyeHomMat); //手眼标定矩阵格式的转换
HTuple FitHomMat = new HTuple();
HTuple CircleRowW = new HTuple(), CircleColW = new HTuple();
HTuple CircleRowAdj = new HTuple();
//5.1调整行坐标
//CircleRowAdj =CircleCenterRow;
CircleRowAdj = CircleCenterRow.Clone();
MyVisionBase.AdjImgRow(ImgIn, ref CircleRowAdj);
//5.2像素坐标调整到世界坐标，消除相机坐标系与世界坐标系之间的夹角
HOperatorSet.AffineTransPoint2d(hv_HandEyeHomMat, CircleCenterCol, CircleRowAdj, out CircleColW, out CircleRowW);
if (CircleColW.Length >= 2)
{
if (CircleColW[0].D < CircleColW[1].D)
{
HOperatorSet.AngleLl(0, 0, 1, 0, CircleColW[0].D, CircleRowW[0].D, CircleColW[1].D, CircleRowW[1].D, out AngleLx); //圆中心连线与世界坐标X轴之间的夹角
HOperatorSet.AngleLl(0, 0, 1, 0, CircleCenterCol[0].D, CircleRowAdj[0].D, CircleCenterCol[1].D, CircleRowAdj[1].D, out AngleLxPixel);
}
else
{
HOperatorSet.AngleLl(0, 0, 1, 0, CircleColW[1].D, CircleRowW[1].D, CircleColW[0].D, CircleRowW[0].D, out AngleLx);
HOperatorSet.AngleLl(0, 0, 1, 0, CircleCenterCol[1].D, CircleRowAdj[1].D, CircleCenterCol[0].D, CircleRowAdj[0].D, out AngleLxPixel);
}
if ((Math.PI / 4) < AngleLxPixel && AngleLxPixel < (Math.PI * 0.75))
{
AngleLxPixel = AngleLxPixel - Math.PI / 2;
}
else if (AngleLxPixel < (-45.0 / 180.0 * Math.PI))
{
AngleLxPixel = AngleLxPixel + Math.PI / 2;
}
}
else if (CircleColW.Length == 1)
{
AngleLxPixel = 0;
AngleLx = 0;
}
//6.0生成图像坐标原点到示教中心的平移放射矩阵
if (CircleColW.Length >= 2)
{
HOperatorSet.VectorAngleToRigid(0, 0, 0, (CircleCenterRow[1].D + CircleCenterRow[0].D) / 2, (CircleCenterCol[1].D + CircleCenterCol[0].D) / 2, AngleLxPixel, out FitHomMat);
}
else if (CircleColW.Length == 1)
{
HOperatorSet.VectorAngleToRigid(0, 0, 0, CircleCenterRow[0].D, CircleCenterCol[0].D, AngleLxPixel, out FitHomMat);
}
HTuple TapeCenterRow = new HTuple(), TapeCenterCol = new HTuple();
HOperatorSet.AffineTransPoint2d(FitHomMat, TeachCirclePara.OffsetPixelY, TeachCirclePara.OffsetPixelX, out TapeCenterRow, out TapeCenterCol); //计算出贴标中心在图像坐标系中的位置
HObject LineContour = new HObject(), OffsetArrowContour = new HObject();
HOperatorSet.GenContourPolygonXld(out LineContour, CircleCenterRow, CircleCenterCol);
double Dist = new double();
Dist = Math.Sqrt(TeachCirclePara.OffsetPixelX * TeachCirclePara.OffsetPixelX + TeachCirclePara.OffsetPixelY * TeachCirclePara.OffsetPixelY);
//7.0定位中心指向实际贴胶的箭头
if (CircleColW.Length >= 2)
{
MyVisionBase.GenArrowContourXld(out OffsetArrowContour, (CircleCenterRow[1].D + CircleCenterRow[0].D) / 2, (CircleCenterCol[1].D + CircleCenterCol[0].D) / 2, TapeCenterRow, TapeCenterCol, Dist / 20, Dist / 20);
}
else if (CircleColW.Length == 1)
{
MyVisionBase.GenArrowContourXld(out OffsetArrowContour, CircleCenterRow[0].D, CircleCenterCol[0].D, TapeCenterRow, TapeCenterCol, Dist / 20, Dist / 20);
}
HOperatorSet.ConcatObj(LineContour, OffsetArrowContour, out OffsetArrowContour);
HOperatorSet.ConcatObj(OffsetArrowContour, CirContour, out CirContour);
//8.0需要转换到平台坐标
HTuple TransRow = new HTuple(), TransCol = new HTuple();
//8.0图片原点调整到右下角
MyVisionBase.AdjImgRow(ImgIn, ref TapeCenterRow);
HOperatorSet.AffineTransPoint2d(hv_HandEyeHomMat, TapeCenterCol, TapeCenterRow, out TransCol, out TransRow);
//9.0减去平台的坐标，得到产品相对于平台旋转中心的坐标，下相机加上平台坐标，得到胶带在平台中的绝对位置
if (ObjContour != null)
{
ObjContour.Dispose();
}
if (CircleObj != null)
{
CircleObj.Dispose();
}
if (IsUp)
{
CenterRow = TransRow.D - CurMotionPos.Row;
CenterCol = TransCol.D - CurMotionPos.Col;
}
else
{
CenterRow = TransRow.D + CurMotionPos.Row;
CenterCol = TransCol.D + CurMotionPos.Col;
}
RotAngle = AngleLx.D;
St_Position NowPos = new St_Position();
// St_Position NowPos = MotionController.GetCurrentPos(true);
//if (TeachCirclePara.IsThreeFeeder)
//{
// HTuple DistMarkToFinger = new HTuple();
// HOperatorSet.DistancePl((CircleCenterCol[0].D + CircleCenterCol[1].D) / 2.0, (CircleCenterRow[0].D + CircleCenterRow[1].D) / 2.0,
// CircleCenterCol[2].D, CircleCenterRow[2].D, CircleCenterCol[3].D, CircleCenterRow[3].D, out DistMarkToFinger);
// HTuple Sx = new HTuple(), Sy = new HTuple(), Phi = new HTuple(), Theta = new HTuple(), Tx = new HTuple(), Ty = new HTuple();
// HOperatorSet.HomMat2dToAffinePar(hv_HandEyeHomMat, out Sx, out Sy, out Phi, out Theta, out Tx, out Ty);
// DistMarkToFinger = DistMarkToFinger.D * Sx;
// double OffSetX0 = 0,OffSetX1 =0;
// OffSetX0 = (DistMarkToFinger.D - 3.5) / 2;
// //CenterCol = CenterCol - OffSetX0 + 0.3 * OffSetX0;//laobanyaojiade
// CenterCol = CenterCol - OffSetX0;
// OffSetX1 = DistMarkToFinger.D - 3.5;
// //if (DistMarkToFinger.D < TeachCirclePara.DistMarkToFinger)
// // return En_ImageProcessResult.产品三号位置弯曲;
// // Logger.Pop1(string.Format("Y轴位置={0},变形量={1}", CurMotionPos.Row, OffSetX0), "产品变形量");
// if (Math.Abs(OffSetX1) > 0.3)
// return En_ImageProcessResult.产品三号位置弯曲;
//}
if (double.IsNaN(CenterCol) || double.IsNaN(CenterRow))
{
Logger.PopError("找圆定位出错坐标非数字");
return(En_ImageProcessResult.找圆定位出错坐标非数字);
}
return(En_ImageProcessResult.OK);
}
catch (Exception e)
{
Logger.PopError("CircleTypePos.FindCenter:" + e.Message + e.Source);
return(En_ImageProcessResult.找圆失败);
}
}
