Winsock UDP server

Winsock UDP server

本文关键字:server UDP Winsock      更新时间:2023-10-16

嗨,我目前正在做一个项目,我们正在创建一个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;
}

我认为您需要查看异步套接字,这里有一个URL,介绍如何使用它们:http://www.win32developer.com/tutorial/winsock/winsock_tutorial_7.shtm