3.2. Examples of UDP/IP Program for the Use of Online Tracking
3.2. Examples of UDP/IP Program for the Use of Online Tracking
The following examples are the examples of the UDP/IP communication program which is driven on the PC for the use of the online tracking function. This program is built in the C language and may operate a robot by means of keyboard input. Please contact our A/S center with respect to relevant questions.
#include <tchar.h> #include <winsock2.h> #include <stdio.h> #include <stdlib.h> #include <conio.h> #include <math.h>
#pragma comment(lib, "ws2_32.lib")
#define _PI 3.141592 #define Hi5_Ts 0.005
typedef struct{
char Command; char char_dummy[3]; int State; int Count; int int_dummy; double dData[6];
} RECEIVE_INTERFACE;
typedef struct{
char Command; char char_dummy[3]; int State; int Count; int int_dummy; double dData[6];
} SEND_INTERFACE;
unsigned long __stdcall Thread1( LPVOID lpParam ); void Init_Command_Data(); void Init_UDP(const char *PC_IP, unsigned short PC_Port, const char *ROBOT_IP, unsigned short ROBOT_Port);
WSADATA wsaData; SOCKET PC_Socket; SOCKADDR_IN PC_Address, Hi5_Address; int PC_AddressSize = sizeof(PC_Address); char *IP_Hi5 = new char [16]; char *IP_PC = new char [16]; unsigned short Port_Hi5; unsigned short Port_PC;
RECEIVE_INTERFACE *pRECEIVE = new RECEIVE_INTERFACE; SEND_INTERFACE *pSEND = new SEND_INTERFACE;
int _tmain(int argc, _TCHAR* argv[]) { int i, ReturnVal, ch; double DeltaPosCmd[6];
IP_Hi5="127.0.0.1"; // IP address of robot controller (Hi5 controller) Port_Hi5 = 6001; // port number of robot controller (Hi5 controller) IP_PC="127.0.0.1"; // IP address of PC Port_PC = 7127; // port number of PC
Init_UDP(IP_PC, Port_PC, IP_Hi5, Port_Hi5); // UDP/IP is initialized
HANDLE hThread1; DWORD dwThreadID1; hThread1 = CreateThread( NULL, 0, Thread1, 0, 0, &dwThreadID1 );
do { for(i=0; i<6; i++) { DeltaPosCmd[i] = 0.0; } ch = _getch();
switch(ch) { // calculate incremental position command (DeltaPosCmd) case 'u': case 'U': DeltaPosCmd[0] = 150.0*Hi5_Ts; // 150mm/sec * 0.005sec break; case 'j': case 'J': DeltaPosCmd[0] = -150.0*Hi5_Ts; break; case 'i': case 'I': DeltaPosCmd[1] = 150.0*Hi5_Ts; break; case 'k': case 'K': DeltaPosCmd[1] = -150.0*Hi5_Ts; break; case 'o': case 'O': DeltaPosCmd[2] = 150.0*Hi5_Ts; break; case 'l': case 'L': DeltaPosCmd[2] = -150.0*Hi5_Ts; break; case 'q': case 'Q': DeltaPosCmd[3] = 100.0*Hi5_Ts; // 100deg/sec * 0.005sec break; case 'a': case 'A': DeltaPosCmd[3] = -100.0*Hi5_Ts; break; case 'W': case 'w': DeltaPosCmd[4] = 100.0*Hi5_Ts; break; case 's': case 'S': DeltaPosCmd[4] = -100.0*Hi5_Ts; break; case 'e': case 'E': DeltaPosCmd[5] = 100.0*Hi5_Ts; break; case 'd': case 'D': DeltaPosCmd[5] = -100.0*Hi5_Ts; break; default: break; }
pSEND->Count++; pSEND->State = 2; pSEND->Command = 'P'; // incremental position command must be expressed in terms of meter & radian for(i=0; i<3; i++) { pSEND->dData[i] = DeltaPosCmd[i] * 0.001; // mm -> m pSEND->dData[i+3] = DeltaPosCmd[i+3] * _PI/180.0; // deg -> rad }
// send the data to Hi5 controller ReturnVal = sendto( PC_Socket, (char *)pSEND, sizeof(SEND_INTERFACE), 0, (struct sockaddr *)&Hi5_Address, sizeof(Hi5_Address) );
} while(ch!='x' & ch!='X');
Sleep( 500 ); closesocket( PC_Socket ); // Close the socket.. WSACleanup();
printf("Program is terminated.*n");
return 0; }
unsigned long __stdcall Thread1( LPVOID lpParam ) { int ReturnVal; bool Start_flag=false; WSANETWORKEVENTS event;
WSAEVENT SockEvent = WSACreateEvent(); printf( "Waiting for the beginning of On-line tracking by Hi5 controller...*n" ); WSAEventSelect( PC_Socket, SockEvent, FD_READ );
while(1) { WSAEnumNetworkEvents( PC_Socket, SockEvent, &event ); if(event.lNetworkEvents & FD_READ)==FD_READ) { // receive the data from Hi5 controller ReturnVal = recvfrom( PC_Socket, (char *)pRECEIVE, sizeof(RECEIVE_INTERFACE), 0, (struct sockaddr *)&PC_Address, &PC_AddressSize );
if(Start_flag==false) { if(pRECEIVE->Command == 'S') // Start { system( "cls" ); printf( "recv> Command: %c, Count: %d, [X: %.3f, Y: %.3f, Z: %.3f, Rx: %.3f, Ry: %.3f, Rz: %.3f] *n", pRECEIVE->Command, pRECEIVE->Count, pRECEIVE->dData[0]*1000, // m -> mm pRECEIVE->dData[1]*1000, pRECEIVE->dData[2]*1000, pRECEIVE->dData[3]*180.0/_PI, // rad -> deg pRECEIVE->dData[4]*180.0/_PI, pRECEIVE->dData[5]*180.0/_PI);
Start_flag = true; Init_Command_Data(); pSEND->Command = 'S'; pSEND->Count = 0; pSEND->State = 1; ReturnVal = sendto( PC_Socket, (char *)pSEND, sizeof(SEND_INTERFACE), 0, (struct sockaddr *)&Hi5_Address, sizeof(Hi5_Address) ); printf("On-line tracking is started by Hi5 controller.*n");
} } else { switch(pRECEIVE->Command) { case 'P': // Play system( "cls" ); printf( "recv> Command: %c, Count: %d, [X: %.3f, Y: %.3f, Z: %.3f, Rx: %.3f, Ry: %.3f, Rz: %.3f] *n", pRECEIVE->Command, pRECEIVE->Count, pRECEIVE->dData[0]*1000, // m -> mm pRECEIVE->dData[1]*1000, pRECEIVE->dData[2]*1000, pRECEIVE->dData[3]*180.0/_PI, // rad -> deg pRECEIVE->dData[4]*180.0/_PI, pRECEIVE->dData[5]*180.0/_PI); break; case 'F': // Finish printf( "On-line tracking is finished by Hi5 controller.*n" ); Start_flag = false; break; default: break; } } } }
WSACloseEvent( SockEvent ); closesocket( PC_Socket ); WSACleanup(); exit( 0 );
return 0; }
void Init_Command_Data() { int i, ReturnVal=0;;
pSEND->Command = NULL; pSEND->State = 0; pSEND->Count = 0; pRECEIVE->Command = NULL; pRECEIVE->State = 0; pRECEIVE->Count = 0; for(i=0; i<6; i++) { pSEND->dData[i] = 0.0; pRECEIVE->dData[i] = 0.0; }
return; }
void Init_UDP(const char *PC_IP, unsigned short PC_Port, const char *ROBOT_IP, unsigned short ROBOT_Port) { PC_Address.sin_family = AF_INET; PC_Address.sin_addr.s_addr = inet_addr( PC_IP ); PC_Address.sin_port = htons( PC_Port ); Hi5_Address.sin_family = AF_INET; Hi5_Address.sin_addr.s_addr = inet_addr( ROBOT_IP ); Hi5_Address.sin_port = htons( ROBOT_Port );
// initiate use of WS2_32.DLL by a process if (WSAStartup(0x202,&wsaData) == SOCKET_ERROR) { printf( "Trouble occurs in WSAStartup setting.*n" ); WSACleanup(); exit( 0 ); } // create PC socket for UDP PC_Socket = socket(AF_INET, SOCK_DGRAM,0); //
if( PC_Socket == INVALID_SOCKET ) { printf( "PC_Socket can't be created.*n" ); WSACleanup(); exit( 0 ); }
// associate PC address with PC socket if( bind(PC_Socket,(struct sockaddr*)&PC_Address,sizeof(PC_Address) ) == SOCKET_ERROR ) { printf( "Socket can't be binded." ); closesocket( PC_Socket ); WSACleanup(); exit( 0 ); }
return; }
|