abb端socket通訊代碼：
MODULE connect_pc
TASK PERS tooldata tool100:=[TRUE,[[0,0,0],[0,0.70711,0,0.70711]],[1.3,[0,0,150],[1,0,0,0],0,0,0]];
VAR robtarget P100:=[[1000,0,500],[1,0,0,0],[-1,0,-1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
VAR num posedatab{7};
VAR bool okb;
VAR pos pos1b;
VAR num Eangle{3};
VAR string strtrans;
VAR string strrot;
VAR string str;
VAR string PC_adressb;
!192.168.125.1//127.0.0.1;
!虛擬機地址127.0.0.1，真實機械臂地址爲192.168.125.1
VAR string IRC5_adressb:="192.168.125.1";
CONST num pro_portb :=8885;
VAR socketdev server_socketb;
VAR socketdev client_socketb;
VAR socketstatus statusb;
VAR num peek_valueb:=65;
VAR num p:=5;
VAR pose objectb;
!error
VAR string str_datab;
VAR string str_databb:="[1,1000.1,0.2,300.3,0,90,0]";
VAR num retry_nob:=0;
PROC main3()
intialb;
!MoveAbsJ [[0,0,0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]]\NoEOffs,v300,z50,tool0;
WHILE TRUE DO
str_datab:="[1,1000.1,0.2,300.03,0,90,0]";
TRAN;
SocketSend client_socketb\str:=str;!發送位姿，第一次發送的是P100初始值
Receivedatab;!接受數據，第一位爲校驗位，後六位爲位姿
Moveitb;
ENDWHILE
socketclose server_socketb;!關閉
socketclose client_socketb;
ENDPROC
PROC Moveitb()!??
IF posedatab{1}=1 OR posedatab{1}=2 THEN
MoveJ P100,v50,fine,tool0;
ELSE
MoveL Offs(P100,200,0,0),v20,fine,tool100;
ENDIF
ENDPROC
PROC TRAN()
Eangle{1}:=EulerZYX(\X,P100.rot);!姿態四元數轉歐拉角
Eangle{2}:=EulerZYX(\Y,P100.rot);
Eangle{3}:=EulerZYX(\Z,P100.rot);
strrot:=ValToStr(Eangle);
strtrans:=ValToStr(P100.trans);!位值類型轉string
str:=strtrans+strrot;!字節合併
ENDPROC
PROC Receivedatab()
SocketReceive client_socketb\Str:=str_datab\Time:=200;!接收字符串
TPWrite"inputvlaue="+str_datab;!示教器上顯示
okb:=strtoval(str_datab,posedatab);!字符串轉數字變量
IF okb=TRUE THEN
P100.trans.x:=posedatab{2}*1000;!位置賦值
P100.trans.y:=posedatab{3}*1000;
P100.trans.z:=posedatab{4}*1000;
IF posedatab{1}=1 THEN
objectb.rot:=OrientZYX(posedatab{7},posedatab{6},posedatab{5});!歐拉角轉四元數
P100.rot:=objectb.rot;
ELSEIF posedatab{1}=2 THEN
objectb.rot:=OrientZYX(0,0,posedatab{5});
P100.rot:=objectb.rot;
ENDIF
ELSE
TPWrite "DATA ERROR.";
ENDIF
ENDPROC
PROC intialb()
socketclose server_socketb;
SocketCreate server_socketb;
IF RobOS() THEN
!TheRobOS() is just for testing if the code is running on a real controller or a Virtualcontroller
SocketBind server_socketb,IRC5_adressb,pro_portb;
ELSE
SocketBind server_socketb,"192.168.125.1",pro_portb;
ENDIF
SocketListen server_socketb;
!listen input
SocketAccept server_socketb,client_socketb\ClientAddress:=PC_adressb\Time:=30;
!SocketSend client_socket\Str:="serverwithip-address"+IRC5_adress;
statusb:=SocketGetStatus(server_socketb);
IF statusb=SOCKET_CREATED THEN
TPWrite "Instruction SocketCreate has been executed";
ELSEIF statusb=SOCKET_CLOSED THEN
TPWrite"Instruction SocketClose has been executed";
ELSEIF statusb=SOCKET_BOUND THEN
TPWrite"Instruction SocketBind has been executed";
ELSEIF statusb=SOCKET_LISTENING THEN
TPWrite"Instruction SocketListen or SocketAccept has been executed";
ELSEIF statusb=SOCKET_CONNECTED THEN
TPWrite"Instruction SocketConnect,SocketReceive or SocketSend has been executed";
ELSE
TPWrite"Unknown socket status";
ENDIF
ERROR
IF ERRNO=ERR_SOCK_TIMEOUT THEN
RETRY;
ELSEIF ERRNO=ERR_SOCK_CLOSED THEN
socketclose server_socketb;
RETRY;
ELSE
!No error recovery handling
ENDIF
ENDPROC
ENDMODULE
pc端socket通訊代碼：（這一份是手眼標定的時候使用，只用修改position數組內容，相機採集圖片的函數換成自己的就行）
#include<WINSOCK2.H>
#include<STDIO.H>
#include<iostream>
#include<cstring>
using namespace std;
#pragma comment(lib,"ws2_32.lib")
void CaptureImages1(const string& rgb_folder, const string& tof_folder)
{
int frame = 0;
cv::namedWindow("OpenCV Display Window", cv::WINDOW_NORMAL); // other options: CV_AUTOSIZE, CV_FREERATIO
cv::namedWindow("ToF Intensity", cv::WINDOW_NORMAL); // other options: CV_AUTOSIZE, CV_FREERATIO
int i = 0;
sensors::BaslerTOF tofCamera;
sensors::BaslerRGB rgbCamera;
rgbCamera.start();
tofCamera.start();
std::cout << "ABB通訊客戶端程序." << endl;
std::cout << "創建tcp客戶端中..." << endl;
WORD sockVersion = MAKEWORD(2, 2);
WSADATA wsd;//加載套接字
if (WSAStartup(sockVersion, &wsd) != 0)
{
std::cout << "創建失敗，請重試";
return;
}
else
{
SOCKET sclient = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);//創建套接字
if (sclient == INVALID_SOCKET)
{
std::cout << "invalid socket!";
return;
}
sockaddr_in serAddr;
serAddr.sin_family = AF_INET;
serAddr.sin_port = htons(8885);
PCWSTR src = TEXT("192.168.125.1");
InetPton(AF_INET, src, &serAddr.sin_addr.s_addr);
//serAddr.sin_addr.S_un.S_addr = inet_addr("127.0.0.1");//服務器地址，虛擬機地址爲127.0.0.1
if (connect(sclient, (sockaddr*)&serAddr, sizeof(serAddr)) == SOCKET_ERROR)
{ //連接失敗
std::cout << "connect error !";
closesocket(sclient);
return;
}
std::cout << "success" << endl;
while (rgbCamera.isGrabbing()&&i<9)
{
bool brgb = rgbCamera.grabImages();
bool btof = tofCamera.grabImages();
if (brgb && btof) {
cv::Mat rgb = rgbCamera.getRGBMap();
double factor = 0.3;
cv::Mat outImg;
cv::resize(rgb, outImg, cv::Size(), factor, factor);
cv::imshow("OpenCV Display Window", outImg);
cv::Mat intensity = tofCamera.getIntensityMap();
cv::imshow("ToF Intensity", intensity);
while (int c = cv::waitKey(10))
{
if (c == 's')
{
//float r1 = 0, r2 = 0, r3 = 0;
float position[48] = { 0.06483,-0.16373,1.11918,-94.66,-30.01,177.08,0.05403,-0.16,1.11941,-104.88,-30.29,175.24,
0.03095,-0.15406,1.14088,-89.16,-37.47,152.1,0.06645,-0.16408,1.15401,-92.58,-30.54,177.04,0.07179,-0.15373,1.14292,
-120.81,-24.08,-167.41,0.11183,-0.14032,1.17467,-122.13,-4.46,-147.65,0.00929,-0.21911,1.16927,-123.81,-0.72,178.38,
-0.01208,-0.22469,1.18641,-111.88,-6.51,166.18 };
float x = position[6 * i], y = position[6 * i + 1], z = position[6 * i + 2], rx = position[6 * i + 3], ry = position[6 * i + 4], rz = position[6 * i + 5];
char recData[127];
int ret = recv(sclient, recData, 127, 0);
if (ret > 0 && i > 0)
{
recData[ret] = 0x00;
std::cout << "當前位姿：" << recData << endl;
// save ToF Intensity image
ostringstream stringStream;
stringStream << rgb_folder << "/frame_" << frame << ".jpg";
std::string file_path = stringStream.str();
bool bwrite_jpg = cv::imwrite(file_path, rgb);
if (!bwrite_jpg)
std::cout << "cannot save rgb images." << endl;
stringStream.str("");
stringStream.clear();
stringStream << tof_folder << "/frame_" << frame << ".png";
file_path = stringStream.str();
bool bwrite_png = cv::imwrite(file_path, intensity);
if (!bwrite_png)
std::cout << "cannot save intensity images." << endl;
frame++;
}
if (i < 8) {
char data[100];
sprintf_s(data, "[%d,%f,%f,%f,%f,%f,%f]", 1, x, y, z, rx, ry, rz);
//第一位爲校驗位，二至四四位置，五至七是歐拉角
send(sclient, data, strlen(data), 0);
std::cout << "發送位姿" << data << endl;
}
i++;
Sleep(3000);
}
if (c == 'q')
{
rgbCamera.stop();
tofCamera.stop();
}
break;
}
}
}
closesocket(sclient);
WSACleanup();
}
tofCamera.stop();
rgbCamera.stop();
}
小功能的rapid代碼：
!*****************************************************
! set speed
!*****************************************************
FUNC speeddata shifting(num base)
IF base = 0 THEN RETURN v100;
ELSEIF base = 1 THEN RETURN v300;
ELSEIF base = 2 THEN RETURN v600;
ELSE RETURN v1000;
ENDIF
ENDFUNC
!*****************************************************
! detect reachable
!*****************************************************
FUNC bool IsReachable(robtarget pReach, PERS tooldata ToolReach, PERS wobjdata WobjReach)
! Check if specified robtarget can be reach with given tool and wobj.
!
! Output:
! Return TRUE if given robtarget is reachable with given tool and wobj
! otherwise return FALSE
!
! Parameters:
! pReach - robtarget to be checked, if robot can reach this robtarget
! ToolReach - tooldata to be used for possible movement
! WobjReach - wobjdata to be used for possible movement
VAR bool bReachable;
VAR jointtarget jntReach;
bReachable := TRUE;
jntReach := CalcJointT(pReach, ToolReach\Wobj:=WobjReach);
RETURN bReachable;
ERROR
IF ERRNO = ERR_ROBLIMIT THEN
bReachable := FALSE;
TRYNEXT;
ENDIF
ENDFUNC
! return robot postion
FUNC robtarget Robot_Position()
VAR robtarget p1;
p1 := CRobT(\Tool:=tool0 \WObj:=wobj0);
RETURN p1;
ENDFUNC