For a project related to iiwa, C + + and matlab are needed for real-time data communication. The data transmission only needs the joint angle and spatial coordinate attitude of the robot (double array with length of 7)
Since most of the functions are implemented in matlab and called by C + +, the Server side is implemented in matlab and the client side is implemented in C + +.
The code is as follows:
Client (C + +)
#include<stdio.h> #include<stdlib.h> #include<string.h> #include<errno.h> #include<sys/types.h> #include<sys/socket.h> #include<netinet/in.h> #include<arpa/inet.h> #include<unistd.h> #define MAXLINE 4096 #define IP "127.0.0.1" double joint[6]={10.123456,11.123456,12.123456,13.123456,14.123456,15.123456}; double joint1[7]={0}; const int len_double = sizeof(double); void *dtoc(void *s,char *m,size_t n)//Convert double type to char type { char *ss = s; for (int i = 0; i < n; i++) m[i]=ss[i]; return m; } void btos(char *s)//Small end mode: low byte in low order and high byte in high address { char send_arr[len_double]; for(int j=0;j<7;j++){ for(int i=0;i<len_double;i++){ send_arr[i]=s[i+j*len_double]; } for(int i=0;i<len_double;i++){ s[7-i+j*len_double]=send_arr[i]; } } } void *ctod(void *s,double *m,size_t n)//Convert char type to double type { double *ss = s; for (int i = 0; i < 7; i++) m[i]=ss[i]; return m; } int main(){ int sockfd, n; char recvline[4096], sendline[4096]; struct sockaddr_in servaddr; //Create a socket if( (sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0){ printf("create socket error: %s(errno: %d)\n", strerror(errno),errno); return 0; } //Initialize to bind the serial port number and other information memset(&servaddr, 0, sizeof(servaddr)); servaddr.sin_family = AF_INET; servaddr.sin_addr.s_addr = inet_addr("127.0.0.1"); servaddr.sin_port = htons(6668); //Actively connect to the server if( connect(sockfd, (struct sockaddr*)&servaddr, sizeof(servaddr)) < 0){ printf("connect error: %s(errno: %d)\n",strerror(errno),errno); return 0; } printf("Connection succeeded\n"); int num=0; // while(1){ printf("send msg to server: \n"); dtoc(joint,sendline,sizeof(joint)); btos(sendline); //printf("%s\n",sendline); send(sockfd, sendline, strlen(sendline), 0); //strlen: calculates the character length by subtracting the first address from the last address of the number of character arrays // if( send(sockfd, sendline, strlen(sendline), 0) < 0){ // printf("send msg error: %s(errno: %d)\n", strerror(errno), errno); // return 0; // } recv(sockfd, recvline, MAXLINE, 0); btos(recvline); ctod(recvline,joint1,sizeof(recvline)); printf("%d\n",num++); for(int i=0;i<7;i++){ printf(" %f ", joint1[i]); }; printf("\n"); // } close(sockfd); return 0; }
Explain the above code: the code is mainly divided into two parts. 1. Communicate with the server from Matlab. 2. Convert the target array into a form convenient for transmission and reading.
1. Communicate with the server from Matlab. Adopt traditional TCP (socket communication)
The server first initializes the socket, then binds it to the port, listens to the port, calls accept to block, and waits for the client to connect.
socket() -> bind() -> listen() -> accept()
The client first initializes the socket and then connects with the server. If the server listens successfully, the connection is established
socket() -> connect()
2. Convert the target array into a form convenient for transmission and reading.
The format of data transmission is char (it is said that c + + transmission can only use char, which should be verified in theory after some time), and small end mode (low byte in low order and high byte in high order)
The conversion part has two points: 1. Convert the double precision array into char type and char type into double precision type; 2. Convert data format to small end format. dtoc and ctod functions, and btos functions, respectively.
Server (Matlab)
t_server=tcpip('127.0.0.1',6668,'NetworkRole','server');%Establish a connection with the first client requesting a connection, with port number 6668 and type server. t_server.InputBuffersize=10000;%Enlarge the buffer to 10000 fopen(t_server);%Open the server until one is established TCP Return only after connection; sprintf("The connection was established successfully"); pause(1); while(1) while(1)%Wait until there is data in the cache to jump out of the loop if t_server.BytesAvailable>0 %t_server.BytesAvailable%Displays the number of cache bytes break; end end data_recv=(fread(t_server,t_server.BytesAvailable));%Read digital data from buffer %%% data_send=[40.123456,21.123456,22.123456,23.123456,24.123456,25.123456,1]; send1=num2hex(data_send);%Convert to hexadecimal send2=[]; for j = 1:length(data_send) send2 = [send2,send1(j,:)]; %Small end mode end count1 = 0; send3 = []; %Convert into double while count1 < length(data_send)*8 send3 = [send3,hex2dec(send2((count1*2+1):(count1*2+2)))]; count1 = count1 + 1; end fwrite(t_server,send3,'char'); end fclose(t_server);
%%%%Incoming char Convert array to double type%%%% recv1 = dec2hex(data_recv);%ACSII Convert code to hexadecimal recv2 = []; %Put data together in small end form for i = 1:length(recv1) recv2 = [recv2,recv1(i,:)]; %Small end mode end %Convert into double count = 0; recv3 = []; while count < length(recv1)/8 recv3 = [recv3,hex2num(recv2((count*16+1):(count*16+16)))]; count = count + 1; end %%%take double Type conversion char array%%%%%% send1=num2hex(data_send);%Convert to hexadecimal send2=[]; for j = 1:length(data_send) send2 = [send2,send1(j,:)]; %Small end mode end %Convert into double count1 = 0; send3 = []; while count1 < length(data_send)*8 send3 = [send3,hex2dec(send2((count1*2+1):(count1*2+2)))]; count1 = count1 + 1; end
In addition to the TCP connection, the Matlab part is mainly about converting the char array passed in from C + + into double type, converting the double type into char type and then transmitting it to C + +. The conversion principle is that after converting the decimal number to hexadecimal, put it together in small end mode, and then divide it according to the required data type, and then return to the decimal number.
Reference: TCP/IP communication between Visual Studio C + + and Matlab to transfer double type data
Linux C/C++ TCP Socket communication example Etc;
New article, if there are mistakes and omissions, welcome the boss to point out.