EPSON机器人固定下相机自动标定及下相机纠偏抓取程序

Function VisionCalib_FIXEDUP '固定下相机自动标定
    Integer i
    Double X_Distance, Y_Distance
    X_Distance = 10 '9宫格X偏移量(设太大可能超出CCD视野)
    Y_Distance = 10 '9宫格Y偏移量(设太大可能超出CCD视野)
    String buff$                '接收的视觉数据
    String Data$(30)            '存放解析的数据
    Double CCD_Pixel_X, CCD_Pixel_Y, CCD_Pixel_U
    
    'STEP 1 建立tool2坐标系
    '工具 -> 机器人管理 -> 工具数据 -> 根据引导完成2点示教
    '使用下相机拍照模板，机器人以2个不同的姿态，在视觉视野同一个模板坐标位置，示教两个示教点建立tool2坐标系
    
    'STEP 2 建立自动9点标定程序(无需示教9个点)
    Tool 2; Wait 0.2;
    PLabel 9, "Mark" 'P9点标签设为Mark点，使用Tool2坐标系示教在相机视野中心(模板中心与相机中心对齐)
    '创建机器人9宫格标定点
    P205 = Mark '中心点（中中）
    P201 = XY(CX(Mark) + X_Distance, CY(Mark) - Y_Distance, CZ(Mark), CU(Mark)) /L '左上
    P202 = XY(CX(Mark) + 0.00000000, CY(Mark) - Y_Distance, CZ(Mark), CU(Mark)) /L '中上
    P203 = XY(CX(Mark) - X_Distance, CY(Mark) - Y_Distance, CZ(Mark), CU(Mark)) /L '右上
    P204 = XY(CX(Mark) - X_Distance, CY(Mark) - 0.00000000, CZ(Mark), CU(Mark)) /L '右中
    P206 = XY(CX(Mark) + X_Distance, CY(Mark) - 0.00000000, CZ(Mark), CU(Mark)) /L '左中
    P207 = XY(CX(Mark) + X_Distance, CY(Mark) + Y_Distance, CZ(Mark), CU(Mark)) /L '左下
    P208 = XY(CX(Mark) + 0.00000000, CY(Mark) + Y_Distance, CZ(Mark), CU(Mark)) /L '中下
    P209 = XY(CX(Mark) - X_Distance, CY(Mark) + Y_Distance, CZ(Mark), CU(Mark)) /L '右下
    SavePoints "robot1.pts"; Wait 0.5; '保存到点位表中
    '设置网络参数
    SetNet #201, "192.168.0.110", 2000, CRLF, NONE, 0
    '检测网络是否连接
    If ChkNet(201) < 0 Then '小于0表示未连接
        OpenNet #201 As Client '视觉为服务端，机器人为客户端
        Print "等待连接网络中..."
        WaitNet #201 '等待连接网络
        Print "网络已连接."
    EndIf
    Wait 1
    For i = 201 To 209 '进行自动9点标定
        Go P(i) '拍照点
        Wait 1
        ReTakePhoto:
        buff$ = "" '清除缓存
        Print #201, "0,0,0,0;"  ' '触发视觉拍照 
        Print "接收视觉数据中..."
        Input #201, buff$        '接收视觉数据（视觉发送格式为："x;y;c;"）
        Print "接收的数据是：", buff$
        If buff$ = "" Then
            Print "Error1：视觉端发送了空数据!"
            Wait 1
            GoTo ReTakePhoto
        EndIf
        ParseStr buff$, Data$(), ";"  '拆分数据，分号(空格)
        CCD_Pixel_X = Val(Data$(0))
        CCD_Pixel_Y = Val(Data$(1))
        CCD_Pixel_U = Val(Data$(2))
        Print "CCD:", i - 200, " X=", CCD_Pixel_X, " Y=", CCD_Pixel_Y, " U=", CCD_Pixel_U
        If CCD_Pixel_X <> 0 And CCD_Pixel_Y <> 0 Then
            P(10 + i) = XY(CCD_Pixel_X, CCD_Pixel_Y, CZ(P205), CCD_Pixel_U) /L
        Else
            Print "Error2：物料未拍到或拍照失败!"
            GoTo ReTakePhoto
        EndIf
    Next i
    'CCD 9宫格像素点
    Print "机器人9点坐标与视觉9点坐标如下："
    For i = 0 To 8
        Print "P", 201 + i, P(201 + i) '机器人笛卡尔坐标
        Print "P", 211 + i, P(211 + i) '视觉像素坐标
        Wait 0.1
    Next i
    SavePoints "robot1.pts"; Wait 0.5;
    
    'STEP 3 校准
    'P211～P219为视觉像素坐标，P201～P209为机器人tooln坐标系下的机器人笛卡尔坐标
    VxCalib 0, VISION_CAMORIENT_FIXEDUP, P(211:219), P(201:209) '坐标校准
    If VxCalInfo(0, 1) = True Then
        Print "相机校准成功，结果如下："
        '----------------X--------------------------------
        Print " X方向的平均偏差[mm]：", VxCalInfo(0, 2)
        Print " X方向的最大偏差[mm]：", VxCalInfo(0, 3)
        Print " X方向一个像素/毫米(mm)：", VxCalInfo(0, 4)
        Print " X方向的倾斜角度(deg)：", VxCalInfo(0, 5)
        '----------------Y--------------------------------
        Print " Y方向的平均偏差[mm]：", VxCalInfo(0, 6)
        Print " Y方向的最大偏差[mm]：", VxCalInfo(0, 7)
        Print " Y方向一个像素/毫米(mm)：", VxCalInfo(0, 8)
        Print " Y方向的倾斜角度(deg)：", VxCalInfo(0, 9)
    Else
        Print "相机校准失败，请重新示教点校准相机!"
    EndIf
    VxCalSave "CCD_Calib.caa" '保存校准文件
    
    'STEP 4 视觉数据转换/拍照抓取
    Do
        Tool 0; Wait 0.1;
        Jump Pick '取料
        Jump Take '拍照
        Tool 2; Wait 0.1;
        '检测网络是否连接
        If ChkNet(201) < 0 Then '小于0表示未连接
                OpenNet #201 As Client '视觉为服务端，机器人为客户端
            Print "等待连接网络中..."
            WaitNet #201 '等待连接网络
            Print "网络已连接."
        EndIf
        Wait 1
        ReTakePhoto1:
        Print #201, "0,0,0,0;"  '触发视觉拍照
        buff$ = ""
        '视觉发送格式为：1个物料时 x;y;c; 2个物料时 x1;y1;c1;x2;y2;c2;
        Print "接收视觉数据中..."
        Input #201, buff$        '接收视觉数据
        Print "接收的数据是：", buff$
        If buff$ = "" Then
            Print "Error3：发送了空数据!"
            Wait 1
            GoTo ReTakePhoto1
        EndIf
        ParseStr buff$, Data$(), ";"  '拆分数据，分号(空格)
        CCD_Pixel_X = Val(Data$(0)) '拍摄工件的视觉坐标x
        CCD_Pixel_Y = Val(Data$(1)) '拍摄工件的视觉坐标y
        CCD_Pixel_U = Val(Data$(2)) '拍摄工件的视觉坐标u
        Print "CCD:", " X=", CCD_Pixel_X, " Y=", CCD_Pixel_Y, " U=", CCD_Pixel_U
        If CCD_Pixel_X <> 0 And CCD_Pixel_Y <> 0 Then
            P270 = XY(CCD_Pixel_X, CCD_Pixel_Y, CZ(P205), CCD_Pixel_U) /L
        Else
            Print "Error4：物料未拍到或拍照失败!"
            GoTo ReTakePhoto1
        EndIf
        Print "转换前的视觉坐标为：", P270
        P260 = VxTrans(0, P270) '像素坐标转笛卡尔坐标
        Print "转换后的机器人坐标为：", P260
        Tool 0; Wait 0.1;
        '重新生成新工具坐标进行纠偏
        TLSet 2, XY(CX(P260) - CX(Here), CY(P260) - CY(Here), CZ(P260) - CZ(Here), CU(P260) - CU(Here))
        Wait 0.5
        Tool 2
        Print "put：", Put
        Jump Put
    Loop
Fend
 
 ———————————————— 
版权声明：本文为CSDN博主「Robotics-Studio」的原创文章，遵循CC 4.0 by-sa版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/isS_MK_GmbH/article/details/88991545