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

1、康耐視相機(jī)操作使用說明書文件狀態(tài)：草稿修改定稿文檔密級不保密內(nèi)部機(jī)密項目名稱版本號1.0描述康耐視相機(jī)操作使用說明書編寫人余國鷹編寫日期2015/9/10審核人審核日期目錄一、康耐視相機(jī)具體設(shè)置錯誤!未指定書簽1.1軟件安裝 錯誤!未指定書簽。1.2流程編輯31.3 Codesys通信注意事項 6二、相機(jī)標(biāo)定72.1相機(jī)校準(zhǔn)72.2絕對坐標(biāo)實(shí)現(xiàn) 92.3相對坐標(biāo)實(shí)現(xiàn)錯誤！未指定書簽。三、示教器示例程序錯誤！未指定書簽。3.1絕對坐標(biāo)實(shí)現(xiàn)范例錯誤！未指定書簽。3.2相對坐標(biāo)實(shí)現(xiàn)范例錯誤！未指定書簽。四、Codesys邏輯開發(fā) 錯誤 味指定書簽。4.1 Socket通信開發(fā)錯誤！未指定書簽。4.2

2、外部點(diǎn)數(shù)據(jù)處理錯誤！未指定書簽。五、細(xì)節(jié)說明 錯誤！未指定書簽。一、康耐視相機(jī)具體設(shè)置1.1軟件安裝雙擊康耐視相機(jī)軟件Cog nex_l n-Sight_Software_4.8.1 ，按照步驟一步步安裝即可。1.2流程編輯1. 設(shè)置電腦本地連接IPV4地址為（設(shè)置為 39段即可）。2. 雙擊康耐視In-Sight瀏覽器軟件進(jìn)入相機(jī)設(shè)置界面，軟件會自動搜索連接的相機(jī)設(shè)備，如下圖1.1所示。圖1.13. 雙擊相機(jī)設(shè)備（紅色橢圓內(nèi)設(shè)備圖標(biāo)），進(jìn)入相機(jī)編輯界面，如下圖1.2所示。圖1.24. 在應(yīng)用程序步驟中，開始、設(shè)置工具、配置結(jié)果、完成這4個步驟形成一個完整的相機(jī)操作流程;?開始單擊“開始”中“

3、已連接”，如下圖1.3所示，可以進(jìn)行連接設(shè)備、斷開設(shè)備、刷新、添加等操作。圖1.3單擊“開始”中的“設(shè)置圖像”，如下圖 1.4所示。在“采集/加載圖像”對話框中，“觸發(fā)器”按鈕是進(jìn)行拍照，“實(shí)況視頻”按鈕是實(shí)時顯示相機(jī)中的圖像，“從 PC加載圖像”按鈕是加載一張存在的圖像。圖1.4在“編輯采集設(shè)置”對話框中，設(shè)置一系列相機(jī)參數(shù)，根據(jù)具體需要進(jìn)行調(diào)節(jié)?設(shè)置工具單擊“設(shè)置工具”中“定位部件”按鈕，對檢測模型進(jìn)行定位，作為識別的模型進(jìn)行對比；單擊“設(shè)置工具”中“檢測部件”按鈕，對模型進(jìn)行編輯操作。在“定位部件”的設(shè)置對話框中，如下圖1.5所示，合格閥值：每次拍照之后的得分如果大于閥值，則拍照成功，否

4、則失?。恍D(zhuǎn)公差：檢測部件能夠旋轉(zhuǎn)的角度范圍，如果在范圍之類 則會拍照成功，否則會失敗圖1.5?配置結(jié)果單擊“配置結(jié)果”中“通信”按鈕，進(jìn)行通訊設(shè)置操作，如下圖1.6所示。新時達(dá)機(jī)器人視覺通信采用的 TCP/IP通訊方式，單擊“ TCP/IP ，如下圖1.7所示，在“ TCP/IP設(shè)置” 對話框中，“服務(wù)器主機(jī)名”設(shè)為（機(jī)器人控制器IP地址），“端口”設(shè)置與 CodeSys中相同，本例中設(shè)為 9876，“超時時間”設(shè)為 15000，“結(jié)束符”設(shè)為字符串（ CR13 ）。單擊“格式化輸出字符串”，進(jìn)行輸出操作，如下圖1.8所示。勾選“使用分隔符”，表示在輸出之間用逗號分隔符進(jìn)行隔開，方便進(jìn)行數(shù)據(jù)

5、處理操作；單擊“添加”按鈕，可以輸出很多數(shù)據(jù)，這里只需要輸岀4個數(shù)據(jù)，“失敗”、“定位器.X ”、“定位器.Y ”、“定位器.角度”；輸岀數(shù)據(jù)說明：均為 7個字節(jié)寬度，除圖案.失敗是整形外，其它數(shù)據(jù)均為浮點(diǎn)型，小數(shù)點(diǎn)位數(shù)為2位。1）第一位圖案.失?。号卸ǚ?，為 0表示采集到特征，拍照成功；為 1則拍照失敗。2） 第二位圖案.定位器.X :輸岀用戶坐標(biāo)系下的X方向絕對值。3） 第三位圖案.定位器.Y :輸岀用戶坐標(biāo)系下的丫方向絕對值。4）第四位圖案.定位器.角度：輸岀用戶坐標(biāo)系下的相對角度。圖1.6圖1.7圖1.8? 完成在“完成”中單擊“保存作業(yè)”，則保存當(dāng)前作業(yè)任務(wù)中的所有設(shè)置；在“完成”中

6、單擊“運(yùn)行作業(yè)”，則運(yùn)行當(dāng)前作業(yè)任務(wù)。注意，以上參數(shù)設(shè)定必須在脫機(jī)模式下才能夠設(shè)置，在聯(lián)機(jī)狀態(tài)下參數(shù)不能設(shè)置，與外界設(shè)備實(shí)時通訊必須在聯(lián)機(jī)模式下；物體旋轉(zhuǎn)一定角度拍照失敗，看檢測部件是否超過相機(jī)視野范圍和旋轉(zhuǎn)角度值設(shè)置是否合理。1.3 Codesys通信注意事項a）說明：設(shè)定 PC端的IP地址，IP地址必須，這里設(shè)定為 2,用網(wǎng)線與相機(jī)控制器相連；b）通信流程包括：建立TCPIP的服務(wù)器端并連接，給相機(jī)發(fā)送指令，從相機(jī)接收數(shù)據(jù)；c）圖1.7中如果把“字段分隔符” 改為“停止”，則兩個數(shù)據(jù)之間就是無分隔符的直接連接； 如果選擇其他，則有相應(yīng)的分隔符號；d）例：采用“消零” 為“有” ，“字段分隔

7、符” 為“停止” 的方式時， 相機(jī)發(fā)送數(shù)據(jù) “-001.00 和“ 1010.02”時，實(shí)際上發(fā)送了“ 454848494648484948494846485013 ” （13 是回車符的 ASIIC 碼 ）；e）在下載工程成功時，查看通訊是否連接正常。當(dāng) client_congnex 任務(wù)中的 step_cognex:=2 和 server_cognex 中的 step_server ： =2 時，此時通訊成功，否則通訊不正常，需要查看設(shè) 備是否連接好和 In-Sight 軟件是否處于聯(lián)機(jī)狀態(tài)。f）詳細(xì)指令及其他形式的通信可參考文檔通信設(shè)定 sdnb-cn5-714d_fh_fz5 。二、相

8、 機(jī)標(biāo)定 要實(shí)現(xiàn)將相機(jī)采集的數(shù)據(jù)轉(zhuǎn)換為機(jī)器人坐標(biāo)系下的位姿數(shù)據(jù)，必須建立相機(jī)坐標(biāo)系與機(jī)器人 坐標(biāo)系的對應(yīng)轉(zhuǎn)換關(guān)系，該過程通過相機(jī)標(biāo)定來實(shí)現(xiàn)。相機(jī)也分平面相機(jī)和三維相機(jī)，前者只支持平面數(shù)據(jù)采集，后者則可以獲取 xyz 空間值。以 三維相機(jī)為例， 要詳細(xì)建立相機(jī)三維空間的位置與機(jī)器人坐標(biāo)系的對應(yīng)關(guān)系， 必須通過嚴(yán)格的手眼 標(biāo)定來實(shí)現(xiàn)。不同相機(jī)有手眼標(biāo)定算法，通過示教多個點(diǎn)來建立手眼轉(zhuǎn)換關(guān)系。這里不詳述。如果只做平面工件抓取，那只需要工件變化的坐標(biāo)值x、y以及繞z軸的轉(zhuǎn)動角度 c,問題就簡單的多， 只需要進(jìn)行平面的簡單標(biāo)定即可實(shí)現(xiàn)。 以康耐視相機(jī)為例， 格力等客戶只需要實(shí)現(xiàn)流水 線來料的抓取操作，

9、標(biāo)定平面坐標(biāo)系即可， 康耐視相機(jī)可以提供移動后的工件相對于移動前的偏移 位置量，或者提供工件的絕對移動位置。這里，我們提供這兩種工作方式的實(shí)現(xiàn)過程。1.4 相機(jī)校準(zhǔn)相機(jī)默認(rèn)輸出的坐標(biāo)值是相機(jī)采集到的像素值，并非實(shí)際工件位置尺寸，因此需要將物理坐 標(biāo)與像素值進(jìn)行映射標(biāo)定。 設(shè)定校準(zhǔn)后， 可使測量結(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)格”按鈕，然后將

10、打印的網(wǎng)格紙張放在相機(jī)視野正中間。3. 在“設(shè)置”界面中，“網(wǎng)格類型”為方格圖案（帶基準(zhǔn)），其它值均為默認(rèn)。4. 單擊“姿勢”，進(jìn)入姿勢設(shè)置界面，如圖2.3 所示，原點(diǎn)位置為默認(rèn)，單擊“觸發(fā)器”按鈕，相機(jī)會自動拍照，最后單擊“校準(zhǔn)”，校準(zhǔn)工作就結(jié)束。5. 單擊“結(jié)果”，可以查看校準(zhǔn)情況，如圖2.4 所示。圖 2.2圖 2.3圖 2.46. 建立檢測模型。單擊“定位部件”，出現(xiàn)如圖2.5 所示界面，單擊“位置工具”中“圖案”，然后單擊“添加”，單擊“ OK ”，接著單擊界面右下角“模型”，拖動綠色模型矩形框，使被檢 測模型處于綠色矩形框中，最后單擊界面右下角“訓(xùn)練”，此時檢測模型已經(jīng)建立。圖 2

11、.57. 查看模型建立是否成功。在界面右側(cè)選擇板中，可以查看建立圖案的情況，綠色圓點(diǎn)表示模型建立成功，同時會輸出檢測模型的位置、角度、得分，如圖2.6 所示。圖 2.6通過以上 7步即可完成相機(jī)的校準(zhǔn)過程。1.5 絕對坐標(biāo)實(shí)現(xiàn)絕對坐標(biāo)的實(shí)現(xiàn)必須借助機(jī)器人的用戶坐標(biāo)系，即機(jī)器人在用戶坐標(biāo)系下走絕對位置運(yùn)動。 具體實(shí)現(xiàn)由以下幾個步驟組成。首先，用 step 機(jī)器人三點(diǎn)法示教出一個固定用戶坐標(biāo)系。用戶坐標(biāo)系的原點(diǎn)根據(jù)實(shí)際情況而 定， 一般選擇流水線上一個固定位置參考點(diǎn)， 該參考點(diǎn)要方便相機(jī)進(jìn)行坐標(biāo)轉(zhuǎn)化標(biāo)定。 關(guān)于用戶坐 標(biāo)系的標(biāo)定，可參見新時達(dá)機(jī)器人操作使用說明書；其次，進(jìn)行相機(jī)坐標(biāo)與實(shí)際位置坐標(biāo)的

12、標(biāo)定轉(zhuǎn)換。在完成第一步中的固定用戶坐標(biāo)系標(biāo)定后， 在該坐標(biāo)系下選取工件上的三點(diǎn)，計算岀這三點(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)絕對位置運(yùn)動。在完成第二步后，工件每偏移一點(diǎn)，相機(jī)均可計算岀其在用戶坐標(biāo)系下新的位置值 X、丫和繞Z軸的theta角度值。這樣，只需要在示教器程序中設(shè)置抓取運(yùn)動 點(diǎn)參考的坐標(biāo)系為用戶坐標(biāo)系即可，即RefSys語句下走絕對cpe點(diǎn)。該cpe點(diǎn)是codesys里直接讀取的相機(jī)返回值。1.6 相對坐標(biāo)實(shí)現(xiàn)相對坐標(biāo)的實(shí)現(xiàn)就比

13、較簡單，只需要將相機(jī)坐標(biāo)轉(zhuǎn)換為實(shí)際位置坐標(biāo)即可。在工件上選取三個特征點(diǎn)，用帶尖機(jī)器人示教岀這三點(diǎn)在機(jī)器人基坐標(biāo)系下的坐標(biāo)值(主要是X和丫)。按照2.1中的校準(zhǔn)流程制作岀校準(zhǔn)參數(shù)。這樣，工件偏移后，相機(jī)可以直接計算岀其新的坐標(biāo)X、丫和theta。需要注意的是，使用相對坐標(biāo)運(yùn)動時，相機(jī)輸岀數(shù)據(jù)必須是相對量，即測量坐標(biāo)與基準(zhǔn)坐標(biāo) 之差，而不是絕對測量坐標(biāo)。具體信息見后續(xù)章節(jié)。三、示教器示例程序1.7 絕對坐標(biāo)實(shí)現(xiàn)范例Tool(tool0); / 若帶工具，則先加載好PTP(ap0); / 走到一個安全位置點(diǎn)Lin(cp3); /走到標(biāo)準(zhǔn)抓取位置(工件處于標(biāo)準(zhǔn)位置時機(jī)器人的抓取位姿，提前示教好)Ref

14、Sys(ref1); /切到用戶坐標(biāo)系下WaitTime(uint3);BOOLEXTSet(boolbasepos,TRUE);/端口號為2,發(fā)送信號到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ī)拍照命令，端口號 0WaitTime(ui ntO); /等待PLC處理時間，建議在 300ms以上boolO:=BOOLEXTRe

15、ad(boolenableget); /讀取抓取標(biāo)志位，為TRUE 則能抓取BOOLEXTSet(boolphoto,FALSE); /關(guān)閉該端口，保證下次為高電平觸發(fā)IF(bool0=0)THEN 為true則可抓取，否則重新發(fā)送拍照命令WaitTime(ui nt1);GOTO(int0);END_IFRefSys(ref1); /切到用戶坐標(biāo)系下Lin(rcpeO); /走到codesys里輸岀的絕對位置(外部點(diǎn)形式，端口號0)BOOLEXTSet(boolfinishget,TRUE); /發(fā)送抓取完成標(biāo)志W(wǎng)aitTime(uint2);BOOLEXTSet(boolfinishget

16、,FALSE); /關(guān)閉抓取完成標(biāo)志PTP(ap1);GOTO(int1); /回到循環(huán)開始，等待下一個工件1.8 相對坐標(biāo)實(shí)現(xiàn)范例Tool(tool0); /若帶工具，則先加載好PTP(ap0); /走到一個安全位置點(diǎn)Lin(cp3); /走到標(biāo)準(zhǔn)抓取位置(工件處于標(biāo)準(zhǔn)位置時機(jī)器人的抓取位姿，提前示教好) WaitTime(uint3);BOOLEXTSet(boolbasepos,TRUE); /端口號為2,讀取當(dāng)前位置作為標(biāo)準(zhǔn)抓取位置 WaitTime(uint4);BOOLEXTSet(boolbasepos,FALSE); /關(guān)閉該端口/以上為獲得標(biāo)準(zhǔn)抓取位置需要的步驟PTP(ap0

17、); /回到安全點(diǎn)LP:int0;BOOLEXTSet(boolphoto,TRUE); /發(fā)送相機(jī)拍照命令，端口號 0WaitTime(ui nt0);/等待PLC處理時間，建議在 300ms以上bool0:=BOOLEXTRead(boolenableget); /讀取抓取標(biāo)志位，為TRUE 則能抓取 BOOLEXTSet(boolphoto,FALSE); /關(guān)閉該端口，保證下次為高電平觸發(fā)IF(bool0=0)THEN 為true則可抓取，否則重新發(fā)送拍照命令WaitTime(ui nt1);GOTO(int0);END_IFLin(rcpe1); /走到codesys里輸岀的絕對位置

18、(外部點(diǎn)形式，該位置是機(jī)器人當(dāng)前位置加上相對偏 移， 端口號 1 )BOOLEXTSet(boolfinishget,TRUE); /發(fā)送抓取完成標(biāo)志 WaitTime(uint2);BOOLEXTSet(boolfinishget,FALSE); /關(guān)閉抓取完成標(biāo)志PTP(ap1);GOTO(int1); /回到循環(huán)開始，等待下一個工件四、Codesys邏輯開發(fā)1.9 Socket 通信開發(fā)首先，要在codesys上完成socket通訊開發(fā)。通訊開發(fā)的要點(diǎn)包括協(xié)議類型、協(xié)議數(shù)據(jù)、數(shù)據(jù) 收發(fā)、邏輯判斷等。一般的相機(jī)都是支持 TCP/IP 協(xié)議的， 我們也多是采用該協(xié)議完成step 控制器與各類

19、相機(jī)的通訊連接及數(shù)據(jù)交互的。一般來說，機(jī)器人控制器作為主機(jī)server (服務(wù)器)，相機(jī)處理器作為從機(jī)client (客戶端)?？刂破髯鳛橹鳈C(jī)的好處在于，控制器“知道”自己什么時候需要拍照、需要數(shù) 據(jù)，此時給相機(jī)發(fā)送命令即可。相機(jī)作為客戶端始終處于監(jiān)聽狀態(tài)。主機(jī)控制器的IP 地址為 220 ，端口號任意，如取為 9876，用網(wǎng)線與相機(jī)控制器相連，在完成相機(jī)與控制器的通信數(shù)據(jù)格式設(shè)定 后，接下來就是 codesys 端編程實(shí)現(xiàn)。通信示例說明：CASEstepOF0:SAddr.sAddr:=20; / 服務(wù)器地址 server(xEnable:=TRUE,ipAddr:=SAddr,uiPort

20、:=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 / 連接報錯step:=3; 復(fù)位END_IFEND_IFIF(server.xError=TRUE)THENstep:=3;END_IF/ 從相機(jī)傳過來的數(shù)據(jù)以 ASCII 碼的形式存儲到 recvokcr 數(shù)組 ,數(shù)組的第一位

21、存儲的是串行數(shù)據(jù) 輸出中設(shè)定的首位/ 例如設(shè)定的整數(shù)位為 1 ，小數(shù)位為 1 ，當(dāng)相機(jī)發(fā)送一個 1.0 時，用 BYTE 型 recvokcr 數(shù)組“ 1.0 回車(CR)對應(yīng)的 ASCII 碼“ 49 “，” 62 “，” 48 “，” 13 “1:size1:=SIZEOF(recvokcr); /數(shù)組長度precv:=ADR(recvokcr); /數(shù)組地址recv(xEnable:=TRUE,hConnection:=connect.hConnection,szSize:=size1,pData:=precv); /接收數(shù)據(jù)IF(recv.szCount7)THEN /判斷是否收到數(shù)據(jù)

22、輸出，這里的數(shù)字應(yīng)小于數(shù)據(jù)總長度step:=5;數(shù)據(jù)處理END_IFIFrecv.xErrorTHEN /接收數(shù)據(jù)報錯step:=3;END_IF/以ASCII碼的形式從senddata數(shù)組傳給相機(jī)，給相機(jī)發(fā)送“ M回車(CR)命令可以采集一張圖片2:size2:=SIZEOF(senddata); / 數(shù)組長度senddata0:=16#4D; /M 的 ASCII 碼， 16 進(jìn)制表示，是單次測量的指令。senddata1:=16#0D;/回車(CR)的ASCII碼，16進(jìn)制表示(歐姆龍相機(jī)用 MCR來發(fā)送數(shù)據(jù)) psend:=ADR(senddata); /指令地址 send(xExec

23、ute:=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ā)送報錯send(xExecute:=FALSE);step:=3;END_IF5：/數(shù)據(jù)處理程序3:/復(fù)位后返回connect(xEnable:=FALSE);server(xEnable:=FALSE);recv(xEn

24、able:=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í)行時才會繼續(xù)生效。 否則可能造成函數(shù)執(zhí)行無效， 程序死循環(huán)卡死在某一步、 或者報錯處理。1.10 外部點(diǎn)數(shù)據(jù)處理在步驟 1.1 通過 socket 實(shí)現(xiàn)控制器與相機(jī)的通信連接及數(shù)據(jù)收發(fā)處理后，本步驟主要完成與HMI 的命令收發(fā)處理以及外部 cpe 點(diǎn)位置計算與返回。 完整流程邏輯如下：

25、/獲取示教器輸入 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)，端口號為 2) *)IF(BasePosGet=TRUEAND(BasePosCount=0)THENBasePosCount:=1;Rea

26、dRefSys(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_IFIFBasePosGet=FALSETHENBasePosCount:=0;END_IF/(*

27、請求相機(jī)數(shù)據(jù)并計算抓取外部點(diǎn) 與通訊部分分開使用，通過標(biāo)志符確定是否獲取數(shù)據(jù)成功 *) rtrig(CLK:=TakePhoPLC);IFrtrig.Q=TRUETHENTakePhoPLC:=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;Tpo

28、s.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_Cartpos2Homomat

29、rix_1.Ref_Homomatrix;FB_MulHomomatrix_0(homomatrix_1:=basecamera_homomatrix,homomatrix_2:=baseTCP_homomatri x);T0TCP:=FB_MulHomomatrix_0.homomatrix_out;(*changetoxyzabc*) FB_Homomatrix2Cartpos_0(Ref_Homomatrix:=T0TCP,SR_RefSys_0=SR_test); FB_Homomatrix2Cartpos_0(Ref_Homomatrix:=T0TCP,SR_RefSys_0=SR_

30、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;/ 絕對坐標(biā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);/ 相對坐標(biāo)輸出，端口號 1 EXTPos_1.x:=BasePos.x+server.Pos_x;EXTPos_1.y:=BasePos.y+server.Pos_y;EXTPos_1.z:=BasePos.z;EXTPos_1.a:=BasePos.a;EXTPos_1.b:=Bas