こんにちは、私は現在、KUKA ロボットと通信するための UDP サーバーを作成するプロジェクトに取り組んでいます。ロボットとの接続を確立し、データをやり取りすることはできますが、モーターの過トルクや何かが原因でロボットが故障するなどのイベントが発生した場合、recvfrom 関数があるため、サーバー ループから飛び出すことができません。まだそこに座って返事を待っています。どうすればそれを修正できるかについて何か提案はありますか?
#include "stdafx.h"
#include "HapticRobot.h"
#include "CMaths.h"
using namespace chai3d;
#include <winsock.h>
using namespace std;
#pragma comment(lib, "Ws2.lib")
#include <fstream>
#include <string>
#include <sstream>
#define REFRESH_INTERVAL 0 // sec
const int kBufferSize = 1024;
extern HapticDevice hd;
extern HWND g_hWndHapticBox;
extern bool bRobotInMotion, bRobotConnectInit, bRobotConnected;
extern Handles hc;
bool err;
std::string stSend, stSendXML, stLine;
std::string stRobotStatus , stAppend;
TCHAR *chRobotStatus;
//// Prototypes ////////////////////////////////////////////////////////
SOCKET SetUpListener(const char* pcAddress, int nPort);
void AcceptConnections(SOCKET ListeningSocket);
bool EchoIncomingPackets(SOCKET sd);
DWORD WINAPI EchoHandler(void* sd_);
//// DoWinsock /////////////////////////////////////////////////////////
// The module's driver function -- we just call other functions and
// interpret their results.
int DoWinsock(const char* pcAddress, int nPort)
{
int nRetval = 0;
ifstream inFile("HardDisk/ExternalData.xml");
if (inFile.is_open())
{
while ( inFile.good() )
{
getline ( inFile, stLine);
stSendXML.append(stLine);
}
inFile.close();
}
else
{
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)L"Unable to open XML file");
}
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)L"Establishing the listener...");
SOCKET ListeningSocket = SetUpListener(pcAddress, htons(nPort));
if (ListeningSocket == INVALID_SOCKET)
{
stRobotStatus = WSAGetLastErrorMessage("establish listener");
chRobotStatus = GetStatus(stRobotStatus);
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)chRobotStatus);
return 3;
}
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)L"Waiting for connections...");
bRobotConnectInit = true;
SetDlgItemText(g_hWndHapticBox, IDC_STATIC_RobotStatus, _T("Waiting for robot"));
while (1)
{
if (!EchoIncomingPackets(ListeningSocket))
{
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)WSAGetLastErrorMessage("Echo incoming packets failed"));
nRetval = 3;
}
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)L"Shutting connection down...");
if (ShutdownConnection(ListeningSocket))
{
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)L"Connection is down.");
}
else
{
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)WSAGetLastErrorMessage("Connection shutdown failed"));
nRetval = 3;
}
bRobotConnected = false;
bRobotConnectInit = true;
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)L"Acceptor restarting...");
}
#if defined(_MSC_VER)
return 0; // warning eater
#endif
}
//// SetUpListener /////////////////////////////////////////////////////
// Sets up a listener on the given interface and port, returning the
// listening socket if successful; if not, returns INVALID_SOCKET.
SOCKET SetUpListener(const char* pcAddress, int nPort)
{
u_long nInterfaceAddr = inet_addr(pcAddress);
if (nInterfaceAddr != INADDR_NONE)
{
SOCKET sd = socket(AF_INET, SOCK_DGRAM, 0);
if (sd != INVALID_SOCKET)
{
sockaddr_in sinInterface;
sinInterface.sin_family = AF_INET;
sinInterface.sin_addr.s_addr = nInterfaceAddr;
sinInterface.sin_port = nPort;
if (bind(sd, (sockaddr*)&sinInterface,
sizeof(sockaddr_in)) != SOCKET_ERROR)
{
//listen(sd, SOMAXCONN);
//DWORD nThreadID;
//CreateThread(0, 0, EchoHandler, (void*)sd, 0, &nThreadID);
return sd;
}
else
{
stRobotStatus = WSAGetLastErrorMessage("bind() failed");
chRobotStatus = GetStatus(stRobotStatus);
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)chRobotStatus);
//cerr << WSAGetLastErrorMessage("bind() failed") <<
// endl;
}
}
}
return INVALID_SOCKET;
}
//// EchoHandler ///////////////////////////////////////////////////////
// Handles the incoming data by reflecting it back to the sender.
DWORD WINAPI EchoHandler(void* sd_)
{
DWORD Priority = CeGetThreadPriority(GetCurrentThread());
CeSetThreadPriority(GetCurrentThread(),Priority - 2);
//Below is scan time in ms
CeSetThreadQuantum(GetCurrentThread(),10);
int nRetval = 0;
SOCKET sd = (SOCKET)sd_;
if (!EchoIncomingPackets(sd))
{
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)WSAGetLastErrorMessage("Echo incoming packets failed"));
nRetval = 3;
}
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)L"Shutting connection down...");
if (ShutdownConnection(sd))
{
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)L"Connection is down.");
}
else
{
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)WSAGetLastErrorMessage("Connection shutdown failed"));
nRetval = 3;
}
bRobotConnected = false;
bRobotConnectInit = true;
return nRetval;
}
//// AcceptConnections /////////////////////////////////////////////////
// Spins forever waiting for connections. For each one that comes in,
// we create a thread to handle it and go back to waiting for
// connections. If an error occurs, we return.
void AcceptConnections(SOCKET ListeningSocket)
{
sockaddr_in sinRemote;
int nAddrSize = sizeof(sinRemote);
while (1)
{
SOCKET sd = accept(ListeningSocket, (sockaddr*)&sinRemote,
&nAddrSize);
if (sd != INVALID_SOCKET)
{
stRobotStatus = inet_ntoa(sinRemote.sin_addr);
stAppend = "Accepted connection from ";
stAppend.append(stRobotStatus);
chRobotStatus = GetStatus(stAppend);
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)chRobotStatus);
//chRobotStatus = GetStatus(stRobotStatus);
//SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)chRobotStatus);
//stRobotStatus = ntohs(sinRemote.sin_port);
//chRobotStatus = GetStatus(stRobotStatus);
//SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)chRobotStatus);
bRobotConnectInit = false;
bRobotConnected = true;
SetDlgItemText(g_hWndHapticBox, IDC_STATIC_RobotStatus, _T("Connected to Robot"));
//SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_ADDSTRING, 0, (LPARAM)inet_ntoa(sinRemote.sin_addr));
//SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_ADDSTRING, 0, (LPARAM)ntohs(sinRemote.sin_port));
DWORD nThreadID;
CreateThread(0, 0, EchoHandler, (void*)sd, 0, &nThreadID);
}
else
{
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)WSAGetLastErrorMessage("accept() failed"));
return;
}
}
}
//// EchoIncomingPackets ///////////////////////////////////////////////
// Bounces any incoming packets back to the client. We return false
// on errors, or true if the client closed the socket normally.
bool EchoIncomingPackets(SOCKET sd)
{
// Read data from client
std::string stReceive;
std::string stIPOC;
std::wstring stTime;
int nStartPos, nEndPos;
char acReadBuffer[kBufferSize], acWriteBuffer[512];
int nReadBytes;
struct sockaddr_in clientAddr;
int sockAddrSize = sizeof(struct sockaddr_in);
//declarations for the low pass filter
int CURRENT_VALUE = 2;
double T = .004, w_co, OMEGA_co, f_co;
do
{
nReadBytes = recvfrom(sd, acReadBuffer, sizeof(acReadBuffer), 0, (struct sockaddr*)&clientAddr, &sockAddrSize);
if (nReadBytes > 0)
{
if (bRobotConnectInit)
{
bRobotConnectInit = false;
bRobotConnected = true;
SetDlgItemText(g_hWndHapticBox, IDC_STATIC_RobotStatus, _T("Connected to Robot"));
}
bRobotConnectInit = false;
bRobotConnected = true;
}
stSend = stSendXML;
stReceive = acReadBuffer;
nStartPos = stReceive.find ("<IPOC>") + 6;
nEndPos = stReceive.find ("</IPOC>");
stIPOC = stReceive.substr (nStartPos, nEndPos - nStartPos);
nStartPos = stSend.find ("<IPOC>") + 6;
nEndPos = stSend.find ("</IPOC>");
stSend.replace(nStartPos, nEndPos - nStartPos, stIPOC);
//Raw sensor data
nStartPos = stReceive.find ("RFx=") + 5;
nEndPos = stReceive.find ("RFy=") - 2;
hd.stRFx = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szRFx = hd.stRFx.c_str();
hd.RFx = strtod(hd.szRFx, NULL);
hd.RFx = hd.RFx * 0.22;
nStartPos = stReceive.find ("RFy=") + 5;
nEndPos = stReceive.find ("RFz=") - 2;
hd.stRFy = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szRFy = hd.stRFy.c_str();
hd.RFy = strtod(hd.szRFy, NULL);
hd.RFy = hd.RFy * 0.22;
nStartPos = stReceive.find ("RFz=") + 5;
nEndPos = stReceive.find ("Fx") - 2;
hd.stRFz = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szRFz = hd.stRFz.c_str();
hd.RFz = strtod(hd.szRFz, NULL);
hd.RFz = hd.RFz * 0.22;
//Sensor data to be filtered
nStartPos = stReceive.find ("Fx=") + 4;
nEndPos = stReceive.find ("Fy=") - 2;
hd.stFx = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szFx = hd.stFx.c_str();
hd.Fx = strtod(hd.szFx, NULL);
hd.Fx = hd.Fx * 0.22;
nStartPos = stReceive.find ("Fy=") + 4;
nEndPos = stReceive.find ("Fz=") - 2;
hd.stFy = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szFy = hd.stFy.c_str();
hd.Fy = strtod(hd.szFy, NULL);
hd.Fy = hd.Fy * 0.22;
nStartPos = stReceive.find ("Fz=") + 4;
nEndPos = stReceive.find ("<IPOC>") - 3;
hd.stFz = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szFz = hd.stFz.c_str();
hd.Fz = strtod(hd.szFz, NULL);
hd.Fz = hd.Fz * 0.22;
//*Added by JMM for reading start cartesian position
nStartPos = stReceive.find ("X=") + 3;
nEndPos = stReceive.find ("Y=") - 2;
hd.stRobotXPosition = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szRobotXPosition = hd.stRobotXPosition.c_str();
hd.RobotXPosition = strtod(hd.szRobotXPosition, NULL);
nStartPos = stReceive.find ("Y=") + 3;
nEndPos = stReceive.find ("Z=") - 2;
hd.stRobotYPosition = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szRobotYPosition = hd.stRobotYPosition.c_str();
hd.RobotYPosition = strtod(hd.szRobotYPosition, NULL);
nStartPos = stReceive.find ("Z=") + 3;
nEndPos = stReceive.find ("A=") - 2;
hd.stRobotZPosition = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szRobotZPosition = hd.stRobotZPosition.c_str();
hd.RobotZPosition = strtod(hd.szRobotZPosition, NULL);
//Data to be passed from the HMI
nStartPos = stReceive.find ("ForThresh=") + 11;
nEndPos = stReceive.find ("StifFree=") - 2;
hd.stForThresh = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szForThresh = hd.stForThresh.c_str();
hd.ForThresh = strtod(hd.szForThresh, NULL);
hd.ForThresh = hd.ForThresh/100;
nStartPos = stReceive.find ("StifFree=") + 10;
nEndPos = stReceive.find ("StifStick=") - 2;
hd.stStifFree = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szStifFree = hd.stStifFree.c_str();
hd.StifFree = strtod(hd.szStifFree, NULL);
hd.StifFree = hd.StifFree/100;
nStartPos = stReceive.find ("StifStick=") + 11;
nEndPos = stReceive.find ("StifCont=") - 2;
hd.stStifStick = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szStifStick = hd.stStifStick.c_str();
hd.StifStick = strtod(hd.szStifStick, NULL);
hd.StifStick = hd.StifStick/100;
nStartPos = stReceive.find ("StifCont=") + 10;
nEndPos = stReceive.find ("KForce=") - 2;
hd.stStifCont = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szStifCont = hd.stStifCont.c_str();
hd.StifCont = strtod(hd.szStifCont, NULL);
hd.StifCont = hd.StifCont/100;
nStartPos = stReceive.find ("KForce=") + 8;
nEndPos = stReceive.find ("LScale=") - 2;
hd.stKForce = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szKForce = hd.stKForce.c_str();
hd.KForce = strtod(hd.szKForce, NULL);
hd.KForce = hd.KForce/100;
nStartPos = stReceive.find ("LScale=") + 8;
nEndPos = stReceive.find ("<HMI") - 3;
hd.stLScale = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szLScale = hd.stLScale.c_str();
hd.LScale = strtod(hd.szLScale, NULL);
nStartPos = stReceive.find ("HapFeed=") + 9;
nEndPos = stReceive.find ("<IPOC>") - 3;
hd.stHapFeed = stReceive.substr (nStartPos, nEndPos - nStartPos);
hd.szHapFeed = hd.stHapFeed.c_str();
hd.HapFeed = atoi(hd.szHapFeed);
//data the is to be sent to the robot
if (hd.FirstTimePosition)
{
hd.RobotXStartPosition = hd.RobotXPosition;
hd.RobotYStartPosition = hd.RobotYPosition;
hd.RobotZStartPosition = hd.RobotZPosition;
hd.FirstTimePosition = false;
}
//tells the filter to turn on or off
if (hd.LinearScale == 0 || hd.LinearScale == 0.5 || hd.LinearScale == 1)
{
hd.NewScaletoRobot = 1;
hd.stScaletoRobot = dtostr(hd.NewScaletoRobot);
nStartPos = stSend.find ("SCX=") + 5;
stSend.replace(nStartPos, 1, hd.stScaletoRobot);
hd.stScaletoRobot = dtostr(hd.NewScaletoRobot);
nStartPos = stSend.find ("SCY=") + 5;
stSend.replace(nStartPos, 1, hd.stScaletoRobot);
hd.stScaletoRobot = dtostr(hd.NewScaletoRobot);
nStartPos = stSend.find ("SCZ=") + 5;
stSend.replace(nStartPos, 1, hd.stScaletoRobot);
}
else
{
hd.NewScaletoRobot = 0;
hd.stScaletoRobot = dtostr(hd.NewScaletoRobot);
nStartPos = stSend.find ("SCX=") + 5;
stSend.replace(nStartPos, 1, hd.stScaletoRobot);
hd.stScaletoRobot = dtostr(hd.NewScaletoRobot);
nStartPos = stSend.find ("SCY=") + 5;
stSend.replace(nStartPos, 1, hd.stScaletoRobot);
hd.stScaletoRobot = dtostr(hd.NewScaletoRobot);
nStartPos = stSend.find ("SCZ=") + 5;
stSend.replace(nStartPos, 1, hd.stScaletoRobot);
}
if(hd.LinearScale == 4)
{
f_co = 0.5;
}
else if (hd.LinearScale == 3)
{
f_co = 0.5;
}
else if (hd.LinearScale == 2)
{
f_co = 1;
}
else if (hd.LinearScale == 1)
{
f_co = 2;
}
else if (hd.LinearScale == 0.5)
{
f_co = 2;
}
else
{
f_co = 0.5;
}
w_co = f_co * C_TWO_PI;
OMEGA_co = (2/T) * cTanRad((w_co * T) / 2);
hd.raw_x[CURRENT_VALUE] = hd.XtoRobot;
hd.raw_y[CURRENT_VALUE] = hd.YtoRobot;
hd.raw_z[CURRENT_VALUE] = hd.ZtoRobot;
hd.filtered_x[CURRENT_VALUE] = (pow(((OMEGA_co) / ((2 / T) + OMEGA_co)), 2)) *
((hd.raw_x[CURRENT_VALUE]) + (2 * hd.raw_x[CURRENT_VALUE - 1] + hd.raw_x[CURRENT_VALUE - 2]))
- (((2 * (OMEGA_co - (2 / T))) / ((2 / T) + OMEGA_co)) * hd.filtered_x[CURRENT_VALUE - 1])
- ((pow(((OMEGA_co - (2 / T)) / ((2 / T) + OMEGA_co)),2)) * hd.filtered_x[CURRENT_VALUE - 2]);
hd.filtered_y[CURRENT_VALUE] = (pow(((OMEGA_co) / ((2 / T) + OMEGA_co)), 2)) *
((hd.raw_y[CURRENT_VALUE]) + (2 * hd.raw_y[CURRENT_VALUE - 1] + hd.raw_y[CURRENT_VALUE - 2]))
- (((2 * (OMEGA_co - (2 / T))) / ((2 / T) + OMEGA_co)) * hd.filtered_y[CURRENT_VALUE - 1])
- ((pow(((OMEGA_co - (2 / T)) / ((2 / T) + OMEGA_co)),2)) * hd.filtered_y[CURRENT_VALUE - 2]);
hd.filtered_z[CURRENT_VALUE] = (pow(((OMEGA_co) / ((2 / T) + OMEGA_co)), 2)) *
((hd.raw_z[CURRENT_VALUE]) + (2 * hd.raw_z[CURRENT_VALUE - 1] + hd.raw_z[CURRENT_VALUE - 2]))
- (((2 * (OMEGA_co - (2 / T))) / ((2 / T) + OMEGA_co)) * hd.filtered_z[CURRENT_VALUE - 1])
- ((pow(((OMEGA_co - (2 / T)) / ((2 / T) + OMEGA_co)),2)) * hd.filtered_z[CURRENT_VALUE - 2]);
hd.raw_x[CURRENT_VALUE - 2] = hd.raw_x[CURRENT_VALUE - 1];
hd.raw_y[CURRENT_VALUE - 2] = hd.raw_y[CURRENT_VALUE - 1];
hd.raw_z[CURRENT_VALUE - 2] = hd.raw_z[CURRENT_VALUE - 1];
hd.raw_x[CURRENT_VALUE - 1] = hd.raw_x[CURRENT_VALUE];
hd.raw_y[CURRENT_VALUE - 1] = hd.raw_y[CURRENT_VALUE];
hd.raw_z[CURRENT_VALUE - 1] = hd.raw_z[CURRENT_VALUE];
hd.filtered_x[CURRENT_VALUE - 2] = hd.filtered_x[CURRENT_VALUE - 1];
hd.filtered_y[CURRENT_VALUE - 2] = hd.filtered_y[CURRENT_VALUE - 1];
hd.filtered_z[CURRENT_VALUE - 2] = hd.filtered_z[CURRENT_VALUE - 1];
hd.filtered_x[CURRENT_VALUE - 1] = hd.filtered_x[CURRENT_VALUE];
hd.filtered_y[CURRENT_VALUE - 1] = hd.filtered_y[CURRENT_VALUE];
hd.filtered_z[CURRENT_VALUE - 1] = hd.filtered_z[CURRENT_VALUE];
//hd.CommGain = 0.001;
//hd.XtoRobot = hd.XtoRobot / (1+(hd.CommGain * abs(hd.Fx - hd.PreviousFx)));
//hd.YtoRobot = hd.YtoRobot / (1+(hd.CommGain * abs(hd.Fy - hd.PreviousFy)));
//hd.ZtoRobot = hd.ZtoRobot / (1+(hd.CommGain * abs(hd.Fz - hd.PreviousFz)));
//hd.CurrentXtoRobot = hd.XtoRobot - hd.PreviousXtoRobot;
hd.stXtoRobot = dtostr(hd.filtered_x[CURRENT_VALUE]);
nStartPos = stSend.find ("X=") + 3;
stSend.replace(nStartPos, 6, hd.stXtoRobot);
//hd.CurrentYtoRobot = hd.YtoRobot - hd.PreviousYtoRobot;
hd.stYtoRobot = dtostr(hd.filtered_y[CURRENT_VALUE]);
nStartPos = stSend.find ("Y=") + 3;
stSend.replace(nStartPos, 6, hd.stYtoRobot);
//hd.CurrentZtoRobot = hd.ZtoRobot - hd.PreviousZtoRobot;
hd.stZtoRobot = dtostr(hd.filtered_z[CURRENT_VALUE]);
nStartPos = stSend.find ("Z=") + 3;
stSend.replace(nStartPos, 6, hd.stZtoRobot);
//hd.CurrentGtoRobot = hd.GtoRobot - hd.PreviousGtoRobot;
//hd.stGtoRobot = dtostr(hd.GtoRobot);
//nStartPos = stSend.find ("A=") + 3;
//stSend.replace(nStartPos, 6, hd.stGtoRobot);
//hd.CurrentBtoRobot = hd.BtoRobot - hd.PreviousBtoRobot;
//hd.stBtoRobot = dtostr(hd.BtoRobot);
//nStartPos = stSend.find ("B=") + 3;
//stSend.replace(nStartPos, 6, hd.stBtoRobot);
//hd.CurrentAtoRobot = hd.AtoRobot - hd.PreviousAtoRobot;
//hd.stAtoRobot = dtostr(hd.AtoRobot);
//nStartPos = stSend.find ("C=") + 3;
//stSend.replace(nStartPos, 6, hd.stAtoRobot);
//hd.PreviousXtoRobot = hd.XtoRobot;
//hd.PreviousYtoRobot = hd.YtoRobot;
//hd.PreviousZtoRobot = hd.ZtoRobot;
//hd.PreviousAtoRobot = hd.AtoRobot;
//hd.PreviousBtoRobot = hd.BtoRobot;
//hd.PreviousGtoRobot = hd.GtoRobot;
strcpy( static_cast<char*>( &acWriteBuffer[0] ), stSend.c_str() );
if (nReadBytes > 0)
{
int nSentBytes = 0;
int SendLength = strlen(acWriteBuffer);
while (nSentBytes < SendLength)
{
int nTemp = sendto(sd, acWriteBuffer, SendLength, 0, (struct sockaddr*)&clientAddr, sockAddrSize);
if (nTemp > 0)
{
nSentBytes += nTemp;
}
else if (nTemp == SOCKET_ERROR)
{
return false;
}
else
{
// Client closed connection before we could reply to
// all the data it sent, so bomb out early.
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)L"Peer unexpectedly dropped connection!");
return true;
}
}
}
else if (nReadBytes == SOCKET_ERROR)
{
return false;
}
} while (nReadBytes != 0);
SendDlgItemMessage(g_hWndHapticBox, IDC_LIST_Server, LB_INSERTSTRING, 0, (LPARAM)L"Connection closed by peer.");
bRobotConnected = false;
bRobotConnectInit = true;
SetDlgItemText(g_hWndHapticBox, IDC_STATIC_RobotStatus, _T("Waiting for robot"));
SetDlgItemText(g_hWndHapticBox, IDC_STATIC_RobotInMotion, _T("Robot not in motion"));
return true;
}