Cognex康耐視相機(jī)操作使用說明書.doc

上海新時(shí)達(dá)機(jī)器人有限公司康耐視相機(jī)操作使用說明書文件狀態(tài)：草稿修改定稿文檔密級(jí)不保密內(nèi)部機(jī)密項(xiàng)目名稱版本號(hào)1.0描述康耐視相機(jī)操作使用說明書編寫人余國(guó)鷹編寫日期2015/9/10審核人審核日期 康耐視相機(jī)操作使用說明書目錄一、康耐視相機(jī)具體設(shè)置31.1軟件安裝錯(cuò)誤!未定義書簽。1.2流程編輯31.3Codesys通信注意事項(xiàng)6二、相機(jī)標(biāo)定72.1相機(jī)校準(zhǔn)72.2絕對(duì)坐標(biāo)實(shí)現(xiàn)92.3相對(duì)坐標(biāo)實(shí)現(xiàn)10三、示教器示例程序113.1絕對(duì)坐標(biāo)實(shí)現(xiàn)范例113.2相對(duì)坐標(biāo)實(shí)現(xiàn)范例11四、CODESYS邏輯開發(fā)134.1Socket通信開發(fā)134.2外部點(diǎn)數(shù)據(jù)處理14五、細(xì)節(jié)說明17一、 康耐視相機(jī)具體設(shè)置1.1 軟件安裝雙擊康耐視相機(jī)軟件Cognex_In-Sight_Software_4.8.1，按照步驟一步步安裝即可。1.2 流程編輯1. 設(shè)置電腦本地連接IPV4地址為192.168.39.12（設(shè)置為39段即可）。2. 雙擊康耐視In-Sight瀏覽器軟件進(jìn)入相機(jī)設(shè)置界面，軟件會(huì)自動(dòng)搜索連接的相機(jī)設(shè)備，如下圖1.1所示。圖1.13. 雙擊相機(jī)設(shè)備（紅色橢圓內(nèi)設(shè)備圖標(biāo)），進(jìn)入相機(jī)編輯界面，如下圖1.2所示。圖1.2 4. 在應(yīng)用程序步驟中，開始、設(shè)置工具、配置結(jié)果、完成這4個(gè)步驟形成一個(gè)完整的相機(jī)操作流程； 開始 單擊“開始”中“已連接”，如下圖1.3所示，可以進(jìn)行連接設(shè)備、斷開設(shè)備、刷新、添加等操作。圖1.3單擊“開始”中的“設(shè)置圖像”，如下圖1.4所示。在“采集/加載圖像”對(duì)話框中，“觸發(fā)器”按鈕是進(jìn)行拍照，“實(shí)況視頻”按鈕是實(shí)時(shí)顯示相機(jī)中的圖像，“從PC加載圖像”按鈕是加載一張存在的圖像。圖1.4在“編輯采集設(shè)置”對(duì)話框中，設(shè)置一系列相機(jī)參數(shù)，根據(jù)具體需要進(jìn)行調(diào)節(jié) 設(shè)置工具 單擊“設(shè)置工具”中“定位部件”按鈕，對(duì)檢測(cè)模型進(jìn)行定位，作為識(shí)別的模型進(jìn)行對(duì)比；單擊“設(shè)置工具”中“檢測(cè)部件”按鈕，對(duì)模型進(jìn)行編輯操作。在“定位部件”的設(shè)置對(duì)話框中，如下圖1.5所示，合格閥值：每次拍照之后的得分如果大于閥值，則拍照成功，否則失??；旋轉(zhuǎn)公差：檢測(cè)部件能夠旋轉(zhuǎn)的角度范圍，如果在范圍之類則會(huì)拍照成功，否則會(huì)失敗圖1.5 配置結(jié)果 單擊“配置結(jié)果”中“通信”按鈕，進(jìn)行通訊設(shè)置操作，如下圖1.6所示。新時(shí)達(dá)機(jī)器人視覺通信采用的TCP/IP通訊方式，單擊“TCP/IP”，如下圖1.7所示，在“TCP/IP設(shè)置”對(duì)話框中，“服務(wù)器主機(jī)名”設(shè)為192.168.39.220（機(jī)器人控制器IP地址），“端口”設(shè)置與CodeSys中相同，本例中設(shè)為9876，“超時(shí)時(shí)間”設(shè)為15000，“結(jié)束符”設(shè)為字符串（CR13）。單擊“格式化輸出字符串”，進(jìn)行輸出操作，如下圖1.8所示。勾選“使用分隔符”，表示在輸出之間用逗號(hào)分隔符進(jìn)行隔開，方便進(jìn)行數(shù)據(jù)處理操作；單擊“添加”按鈕，可以輸出很多數(shù)據(jù)，這里只需要輸出4個(gè)數(shù)據(jù)，“失敗”、“定位器.X”、“定位器.Y”、“定位器.角度”；輸出數(shù)據(jù)說明：均為7個(gè)字節(jié)寬度，除圖案.失敗是整形外，其它數(shù)據(jù)均為浮點(diǎn)型，小數(shù)點(diǎn)位數(shù)為2位。1） 第一位圖案.失?。号卸ǚ?，為0表示采集到特征，拍照成功；為1則拍照失敗。2） 第二位圖案.定位器.X：輸出用戶坐標(biāo)系下的X方向絕對(duì)值。3） 第三位圖案.定位器.Y：輸出用戶坐標(biāo)系下的Y方向絕對(duì)值。4) 第四位圖案.定位器.角度：輸出用戶坐標(biāo)系下的相對(duì)角度。圖1.6圖1.7圖1.8 完成 在“完成”中單擊“保存作業(yè)”，則保存當(dāng)前作業(yè)任務(wù)中的所有設(shè)置；在“完成”中單擊“運(yùn)行作業(yè)”，則運(yùn)行當(dāng)前作業(yè)任務(wù)。注意，以上參數(shù)設(shè)定必須在脫機(jī)模式下才能夠設(shè)置，在聯(lián)機(jī)狀態(tài)下參數(shù)不能設(shè)置，與外界設(shè)備實(shí)時(shí)通訊必須在聯(lián)機(jī)模式下；物體旋轉(zhuǎn)一定角度拍照失敗，看檢測(cè)部件是否超過相機(jī)視野范圍和旋轉(zhuǎn)角度值設(shè)置是否合理。1.3 Codesys通信注意事項(xiàng)a) 說明：設(shè)定PC端的IP地址，IP地址必須192.168.39.XXX，這里設(shè)定為192.168.39.12，用網(wǎng)線與相機(jī)控制器相連；b) 通信流程包括：建立TCPIP的服務(wù)器端并連接，給相機(jī)發(fā)送指令，從相機(jī)接收數(shù)據(jù)；c) 圖1.7中如果把“字段分隔符”改為“停止”，則兩個(gè)數(shù)據(jù)之間就是無分隔符的直接連接；如果選擇其他，則有相應(yīng)的分隔符號(hào)；d) 例：采用“消零”為“有”，“字段分隔符”為“停止”的方式時(shí)，相機(jī)發(fā)送數(shù)據(jù)“-001.00”和“1010.02”時(shí)，實(shí)際上發(fā)送了“45 48 48 49 46 48 48 49 48 49 48 46 48 50 13”(13是回車符的ASIIC碼)；e) 在下載工程成功時(shí)，查看通訊是否連接正常。當(dāng)client_congnex任務(wù)中的step_cognex:=2和server_cognex中的step_server：=2時(shí)，此時(shí)通訊成功，否則通訊不正常，需要查看設(shè)備是否連接好和In-Sight軟件是否處于聯(lián)機(jī)狀態(tài)。f) 詳細(xì)指令及其他形式的通信可參考文檔通信設(shè)定sdnb-cn5-714d_fh_fz5。二、 相機(jī)標(biāo)定要實(shí)現(xiàn)將相機(jī)采集的數(shù)據(jù)轉(zhuǎn)換為機(jī)器人坐標(biāo)系下的位姿數(shù)據(jù)，必須建立相機(jī)坐標(biāo)系與機(jī)器人坐標(biāo)系的對(duì)應(yīng)轉(zhuǎn)換關(guān)系，該過程通過相機(jī)標(biāo)定來實(shí)現(xiàn)。相機(jī)也分平面相機(jī)和三維相機(jī)，前者只支持平面數(shù)據(jù)采集，后者則可以獲取xyz空間值。以三維相機(jī)為例，要詳細(xì)建立相機(jī)三維空間的位置與機(jī)器人坐標(biāo)系的對(duì)應(yīng)關(guān)系，必須通過嚴(yán)格的手眼標(biāo)定來實(shí)現(xiàn)。不同相機(jī)有手眼標(biāo)定算法，通過示教多個(gè)點(diǎn)來建立手眼轉(zhuǎn)換關(guān)系。這里不詳述。如果只做平面工件抓取，那只需要工件變化的坐標(biāo)值x、y以及繞z軸的轉(zhuǎn)動(dòng)角度c，問題就簡(jiǎn)單的多，只需要進(jìn)行平面的簡(jiǎn)單標(biāo)定即可實(shí)現(xiàn)。以康耐視相機(jī)為例，格力等客戶只需要實(shí)現(xiàn)流水線來料的抓取操作，標(biāo)定平面坐標(biāo)系即可，康耐視相機(jī)可以提供移動(dòng)后的工件相對(duì)于移動(dòng)前的偏移位置量，或者提供工件的絕對(duì)移動(dòng)位置。這里，我們提供這兩種工作方式的實(shí)現(xiàn)過程。22.1 相機(jī)校準(zhǔn)相機(jī)默認(rèn)輸出的坐標(biāo)值是相機(jī)采集到的像素值，并非實(shí)際工件位置尺寸，因此需要將物理坐標(biāo)與像素值進(jìn)行映射標(biāo)定。設(shè)定校準(zhǔn)后，可使測(cè)量結(jié)果像素值轉(zhuǎn)換為實(shí)際尺寸并輸出，康耐視提供了校準(zhǔn)參數(shù)的制作過程。1.單擊“設(shè)置圖像“，界面右下角出現(xiàn)下圖2.1所示界面。校準(zhǔn)類型中有很多種，根據(jù)實(shí)際需要選擇，這里選擇“網(wǎng)格”，然后單擊“校準(zhǔn)”按鈕，出現(xiàn)下圖2.2所示界面。圖2.12.打印校準(zhǔn)網(wǎng)格紙張。在圖2.2中，單擊“打印網(wǎng)格”按鈕，然后將打印的網(wǎng)格紙張放在相機(jī)視野正中間。3.在“設(shè)置”界面中，“網(wǎng)格類型”為方格圖案（帶基準(zhǔn)），其它值均為默認(rèn)。4.單擊“姿勢(shì)”，進(jìn)入姿勢(shì)設(shè)置界面，如圖2.3所示，原點(diǎn)位置為默認(rèn)，單擊“觸發(fā)器”按鈕， 相機(jī)會(huì)自動(dòng)拍照，最后單擊“校準(zhǔn)”，校準(zhǔn)工作就結(jié)束。5.單擊“結(jié)果”，可以查看校準(zhǔn)情況，如圖2.4所示。圖2.2圖2.3圖2.46.建立檢測(cè)模型。單擊“定位部件”，出現(xiàn)如圖2.5所示界面，單擊“位置工具”中“圖案”，然后單擊“添加”，單擊“OK”，接著單擊界面右下角“模型”，拖動(dòng)綠色模型矩形框，使被檢測(cè)模型處于綠色矩形框中，最后單擊界面右下角“訓(xùn)練”，此時(shí)檢測(cè)模型已經(jīng)建立。圖2.57.查看模型建立是否成功。在界面右側(cè)選擇板中，可以查看建立圖案的情況，綠色圓點(diǎn)表示模型建立成功，同時(shí)會(huì)輸出檢測(cè)模型的位置、角度、得分，如圖2.6所示。圖2.6通過以上7步即可完成相機(jī)的校準(zhǔn)過程。2.2 絕對(duì)坐標(biāo)實(shí)現(xiàn) 絕對(duì)坐標(biāo)的實(shí)現(xiàn)必須借助機(jī)器人的用戶坐標(biāo)系，即機(jī)器人在用戶坐標(biāo)系下走絕對(duì)位置運(yùn)動(dòng)。具體實(shí)現(xiàn)由以下幾個(gè)步驟組成。首先，用step機(jī)器人三點(diǎn)法示教出一個(gè)固定用戶坐標(biāo)系。用戶坐標(biāo)系的原點(diǎn)根據(jù)實(shí)際情況而定，一般選擇流水線上一個(gè)固定位置參考點(diǎn)，該參考點(diǎn)要方便相機(jī)進(jìn)行坐標(biāo)轉(zhuǎn)化標(biāo)定。關(guān)于用戶坐標(biāo)系的標(biāo)定，可參見新時(shí)達(dá)機(jī)器人操作使用說明書；其次，進(jìn)行相機(jī)坐標(biāo)與實(shí)際位置坐標(biāo)的標(biāo)定轉(zhuǎn)換。在完成第一步中的固定用戶坐標(biāo)系標(biāo)定后，在該坐標(biāo)系下選取工件上的三點(diǎn)，計(jì)算出這三點(diǎn)在用戶坐標(biāo)系的X、Y值（該步驟可通過機(jī)器人協(xié)助示教獲得在用戶坐標(biāo)系下的位姿值）。在圖像輸入的“校準(zhǔn)”模塊中，按照2.1中的步驟完成相機(jī)坐標(biāo)與實(shí)際位置坐標(biāo)的校準(zhǔn)參數(shù)制作轉(zhuǎn)換；最后，機(jī)器人實(shí)現(xiàn)絕對(duì)位置運(yùn)動(dòng)。在完成第二步后，工件每偏移一點(diǎn)，相機(jī)均可計(jì)算出其在用戶坐標(biāo)系下新的位置值X、Y和繞Z軸的theta角度值。這樣，只需要在示教器程序中設(shè)置抓取運(yùn)動(dòng)點(diǎn)參考的坐標(biāo)系為用戶坐標(biāo)系即可，即RefSys語句下走絕對(duì)cpe點(diǎn)。該cpe點(diǎn)是codesys里直接讀取的相機(jī)返回值。2.3 相對(duì)坐標(biāo)實(shí)現(xiàn)相對(duì)坐標(biāo)的實(shí)現(xiàn)就比較簡(jiǎn)單，只需要將相機(jī)坐標(biāo)轉(zhuǎn)換為實(shí)際位置坐標(biāo)即可。在工件上選取三個(gè)特征點(diǎn)，用帶尖機(jī)器人示教出這三點(diǎn)在機(jī)器人基坐標(biāo)系下的坐標(biāo)值（主要是X和Y）。按照2.1中的校準(zhǔn)流程制作出校準(zhǔn)參數(shù)。這樣，工件偏移后，相機(jī)可以直接計(jì)算出其新的坐標(biāo)X、Y和theta。需要注意的是，使用相對(duì)坐標(biāo)運(yùn)動(dòng)時(shí)，相機(jī)輸出數(shù)據(jù)必須是相對(duì)量，即測(cè)量坐標(biāo)與基準(zhǔn)坐標(biāo)之差，而不是絕對(duì)測(cè)量坐標(biāo)。具體信息見后續(xù)章節(jié)。三、 示教器示例程序33.1 絕對(duì)坐標(biāo)實(shí)現(xiàn)范例Tool(tool0);/若帶工具，則先加載好PTP(ap0); /走到一個(gè)安全位置點(diǎn)Lin(cp3);/走到標(biāo)準(zhǔn)抓取位置（工件處于標(biāo)準(zhǔn)位置時(shí)機(jī)器人的抓取位姿，提前示教好）RefSys(ref1);/切到用戶坐標(biāo)系下WaitTime(uint3);BOOLEXTSet(boolbasepos,TRUE);/端口號(hào)為2，發(fā)送信號(hào)到codesys，讀取當(dāng)前位置作為標(biāo)準(zhǔn)抓取位置WaitTime(uint4);BOOLEXTSet(boolbasepos,FALSE);/關(guān)閉該端口/以上為獲得標(biāo)準(zhǔn)抓取位置需要的步驟RefSys(WORLD);PTP(ap0);LP:int0;BOOLEXTSet(boolphoto,TRUE); /發(fā)送相機(jī)拍照命令，端口號(hào)0WaitTime(uint0); /等待PLC處理時(shí)間，建議在300ms以上bool0:=BOOLEXTRead(boolenableget); /讀取抓取標(biāo)志位,為TRUE則能抓取BOOLEXTSet(boolphoto,FALSE); /關(guān)閉該端口，保證下次為高電平觸發(fā)IF(bool0=0)THEN /為true則可抓取，否則重新發(fā)送拍照命令WaitTime(uint1);GOTO(int0);END_IFRefSys(ref1); /切到用戶坐標(biāo)系下Lin(rcpe0); /走到codesys里輸出的絕對(duì)位置（外部點(diǎn)形式，端口號(hào)0）BOOLEXTSet(boolfinishget,TRUE); /發(fā)送抓取完成標(biāo)志W(wǎng)aitTime(uint2);BOOLEXTSet(boolfinishget,FALSE); /關(guān)閉抓取完成標(biāo)志PTP(ap1);GOTO(int1); /回到循環(huán)開始，等待下一個(gè)工件3.2 相對(duì)坐標(biāo)實(shí)現(xiàn)范例Tool(tool0);/若帶工具，則先加載好PTP(ap0); /走到一個(gè)安全位置點(diǎn)Lin(cp3);/走到標(biāo)準(zhǔn)抓取位置（工件處于標(biāo)準(zhǔn)位置時(shí)機(jī)器人的抓取位姿，提前示教好）WaitTime(uint3);BOOLEXTSet(boolbasepos,TRUE);/端口號(hào)為2，讀取當(dāng)前位置作為標(biāo)準(zhǔn)抓取位置WaitTime(uint4);BOOLEXTSet(boolbasepos,FALSE);/關(guān)閉該端口/以上為獲得標(biāo)準(zhǔn)抓取位置需要的步驟PTP(ap0);/回到安全點(diǎn)LP:int0;BOOLEXTSet(boolphoto,TRUE); /發(fā)送相機(jī)拍照命令，端口號(hào)0WaitTime(uint0); /等待PLC處理時(shí)間，建議在300ms以上bool0:=BOOLEXTRead(boolenableget); /讀取抓取標(biāo)志位,為TRUE則能抓取BOOLEXTSet(boolphoto,FALSE); /關(guān)閉該端口，保證下次為高電平觸發(fā)IF(bool0=0)THEN /為true則可抓取，否則重新發(fā)送拍照命令WaitTime(uint1);GOTO(int0);END_IFLin(rcpe1); /走到codesys里輸出的絕對(duì)位置（外部點(diǎn)形式，該位置是機(jī)器人當(dāng)前位置加上相對(duì)偏移，端口號(hào)1）BOOLEXTSet(boolfinishget,TRUE); /發(fā)送抓取完成標(biāo)志W(wǎng)aitTime(uint2);BOOLEXTSet(boolfinishget,FALSE); /關(guān)閉抓取完成標(biāo)志PTP(ap1);GOTO(int1); /回到循環(huán)開始，等待下一個(gè)工件四、 Codesys邏輯開發(fā)44.1 Socket通信開發(fā)首先，要在codesys上完成socket通訊開發(fā)。通訊開發(fā)的要點(diǎn)包括協(xié)議類型、協(xié)議數(shù)據(jù)、數(shù)據(jù)收發(fā)、邏輯判斷等。一般的相機(jī)都是支持TCP/IP協(xié)議的，我們也多是采用該協(xié)議完成step控制器與各類相機(jī)的通訊連接及數(shù)據(jù)交互的。一般來說，機(jī)器人控制器作為主機(jī)server（服務(wù)器），相機(jī)處理器作為從機(jī)client（客戶端）?？刂破髯鳛橹鳈C(jī)的好處在于，控制器“知道”自己什么時(shí)候需要拍照、需要數(shù)據(jù)，此時(shí)給相機(jī)發(fā)送命令即可。相機(jī)作為客戶端始終處于監(jiān)聽狀態(tài)。主機(jī)控制器的IP地址為192.168.39.220，端口號(hào)任意，如取為9876，用網(wǎng)線與相機(jī)控制器相連，在完成相機(jī)與控制器的通信數(shù)據(jù)格式設(shè)定后，接下來就是codesys端編程實(shí)現(xiàn)。通信示例說明：CASE step OF0:SAddr.sAddr := 192.168.39.120;/服務(wù)器地址server(xEnable:=TRUE,ipAddr:=SAddr,uiPort:=9876);/建立服務(wù)器IF(server.xBusy = TRUE) THENconnect(xEnable:=server.xBusy,hServer:=server.hServer);/連接IF(connect.xActive = TRUE) THEN/連接成功step := 1;/執(zhí)行接收數(shù)據(jù)END_IFIF(connect.xError = TRUE)THEN/連接報(bào)錯(cuò)step := 3;/復(fù)位END_IFEND_IFIF(server.xError = TRUE)THENstep := 3;END_IF/從相機(jī)傳過來的數(shù)據(jù)以ASCII碼的形式存儲(chǔ)到recvokcr數(shù)組,數(shù)組的第一位存儲(chǔ)的是串行數(shù)據(jù)輸出中設(shè)定的首位/例如設(shè)定的整數(shù)位為1，小數(shù)位為1，當(dāng)相機(jī)發(fā)送一個(gè)1.0時(shí)，用BYTE型recvokcr數(shù)組“1.0回車（CR）”對(duì)應(yīng)的ASCII碼“49“，”62“，”48“，”13“1:size1 := SIZEOF(recvokcr);/數(shù)組長(zhǎng)度precv := ADR(recvokcr);/數(shù)組地址recv(xEnable:=TRUE,hConnection:=connect.hConnection,szSize:=size1,pData:=precv);/接收數(shù)據(jù)IF(recv.szCount7) THEN/判斷是否收到數(shù)據(jù)輸出，這里的數(shù)字應(yīng)小于數(shù)據(jù)總長(zhǎng)度step:=5;/數(shù)據(jù)處理END_IFIF recv.xError THEN/接收數(shù)據(jù)報(bào)錯(cuò)step:=3;END_IF/以ASCII碼的形式從senddata數(shù)組傳給相機(jī)，給相機(jī)發(fā)送“M回車（CR）”命令可以采集一張圖片2:size2 := SIZEOF(senddata);/數(shù)組長(zhǎng)度senddata0:=16#4D;/M的ASCII碼，16進(jìn)制表示，是單次測(cè)量的指令。senddata1:=16#0D;/回車（CR）的ASCII碼，16進(jìn)制表示（歐姆龍相機(jī)用MCR來發(fā)送數(shù)據(jù)）psend := ADR(senddata);/指令地址send(xExecute:=TRUE,hConnection:=connect.hConnection,szSize:=size2,pData:=psend,udiTimeOut:=1000000);IF(send.xDone = TRUE) THEN/發(fā)送成功send(xExecute:=FALSE);/函數(shù)復(fù)位step := 1;END_IFIF(send.xError = TRUE) THEN/發(fā)送報(bào)錯(cuò)send(xExecute:=FALSE);step := 3;END_IF5：/數(shù)據(jù)處理程序.3: /復(fù)位后返回connect(xEnable:=FALSE);server(xEnable:=FALSE);recv(xEnable:=FALSE,hConnection:=connect.hConnection,szSize:=size,pData:=precv);step := 99;99:step:=0;END_CASE注意，多數(shù)socket函數(shù)都是上升沿執(zhí)行，所以在調(diào)用后，應(yīng)在合適的地方將其關(guān)閉（給下降沿），這樣下一周期執(zhí)行時(shí)才會(huì)繼續(xù)生效。否則可能造成函數(shù)執(zhí)行無效，程序死循環(huán)卡死在某一步、或者報(bào)錯(cuò)處理。4.2 外部點(diǎn)數(shù)據(jù)處理在步驟1.1通過socket實(shí)現(xiàn)控制器與相機(jī)的通信連接及數(shù)據(jù)收發(fā)處理后，本步驟主要完成與HMI的命令收發(fā)處理以及外部cpe點(diǎn)位置計(jì)算與返回。完整流程邏輯如下：/獲取示教器輸入GetHMIBOOL(Enable := TRUE,PortNumber := 0,Data = ReqGet); /獲取拍照命令GetHMIBOOL(Enable := TRUE,PortNumber := 1,Data = FinishGet); /獲取完成抓取命令GetHMIBOOL(Enable := TRUE,PortNumber := 2,Data = BasePosGet); /獲取標(biāo)準(zhǔn)抓取位置命令/(*獲取標(biāo)準(zhǔn)抓取位置（當(dāng)前通過IO語句發(fā)送命令實(shí)現(xiàn)，端口號(hào)為2）*)IF (BasePosGet = TRUE AND (BasePosCount = 0) THENBasePosCount := 1;ReadRefSys(Enable := TRUE,RefSys = RefSys_qz);/注意，ReadRefSys讀出來的當(dāng)前坐標(biāo)系的ABC為弧度，需要轉(zhuǎn)換為角度RefSys_qz.a := RefSys_qz.a * R2D;RefSys_qz.b := RefSys_qz.b * R2D;RefSys_qz.c := RefSys_qz.c * R2D;ReadTCPData(Enable := TRUE,RefSys := RefSys_qz,CartPos =BasePos); /注意，當(dāng)前讀取用戶坐標(biāo)系下的位置值END_IFIF BasePosGet = FALSE THENBasePosCount := 0;END_IF/(*請(qǐng)求相機(jī)數(shù)據(jù)并計(jì)算抓取外部點(diǎn)與通訊部分分開使用，通過標(biāo)志符確定是否獲取數(shù)據(jù)成功*)rtrig(CLK :=TakePhoPLC);IF rtrig.Q = TRUE THENTakePhoPLC := FALSE;/ baseTCP為標(biāo)準(zhǔn)抓取位置baseTCP.x := BasePos.x;baseTCP.y := BasePos.y;baseTCP.z := BasePos.z;baseTCP.a := BasePos.a*D2R;baseTCP.b := BasePos.b*D2R;baseTCP.c := BasePos.c*D2R;/ Tpos為相機(jī)輸出數(shù)據(jù)Tpos.x := server.Pos_x;Tpos.y := server.Pos_y;Tpos.z := 0;Tpos.a := 0;Tpos.b := 0;Tpos.c := server.Pos_c*D2R;/ 以下過程為坐標(biāo)變換，將相機(jī)坐標(biāo)系下的角度theta轉(zhuǎn)換到用戶坐標(biāo)系下 FB_Cartpos2Homomatrix_0(SR_RefSys_0 := baseTCP);baseTCP_homomatrix := FB_Cartpos2Homomatrix_0.Ref_Homomatrix;FB_Cartpos2Homomatrix_1(SR_RefSys_0 := Tpos);basecamera_homomatrix := FB_Cartpos2Homomatrix_1.Ref_Homomatrix;FB_MulHomomatrix_0(homomatrix_1 := basecamera_homomatrix, homomatrix_2 := baseTCP_homomatrix);T0TCP := FB_MulHomomatrix_0.homomatrix_out;(* change to xyzabc*)FB_Homomatrix2Cartpos_0(Ref_Homomatrix := T0TCP, SR_RefSys_0 = SR_test);FB_Homomatrix2Cartpos_0(Ref_Homomatrix := T0TCP, SR_RefSys_0 = SR_RefSys_0);SR_RefSys_0.a := SR_RefSys_0.a * R2D;SR_RefSys_0.b := SR_RefSys_0.b * R2D;SR_RefSys_0.c := SR_RefSys_0.c * R2D;/絕對(duì)坐標(biāo)輸出，端口號(hào)0EXTPos.x:=server.Pos_x; / x和y值用相機(jī)輸出數(shù)據(jù)EXTPos.y:=server.Pos_y;EXTPos.z:=BasePos.z; / z、a、b用標(biāo)準(zhǔn)抓取位置值EXTPos.a:=BasePos.a;EXTPos.b:=BasePos.b;EXTPos.c:= SR_RefSys_0.c; /theta用轉(zhuǎn)換過的角度SetCartPos(Enable:=TRUE,PortNumber:=0,RobotCartPos:=EXTPos);/相對(duì)坐標(biāo)輸出，端口號(hào)1EXTPos_1.x:=BasePos.x + server.Pos_x;EXTPos_1.y:=BasePos.y + server.Pos_y;EXTPos_1.z:=BasePos.z;EX