diff --git a/Plugin/Driver/DriverMainDlg.cpp b/Plugin/Driver/DriverMainDlg.cpp index be7581e..0da2f82 100644 --- a/Plugin/Driver/DriverMainDlg.cpp +++ b/Plugin/Driver/DriverMainDlg.cpp @@ -344,11 +344,19 @@ void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen) { Json::Value data = root["params"]; double currentAngled = data["angle"].asDouble(); // 实际角度（度） - float currentAngle = static_cast(currentAngled); + float cad = static_cast(currentAngled); + float currentAngle = -cad; + //abs + if (std::abs(currentAngle) < 0.2f) { + // 角度绝对值小于5，直接返回，不执行后续代码 + theApp.m_fAngCalForFast = 0; + LogOutToFile("currentAngled : = %f, CorrectAng = %f", currentAngled, theApp.m_fAngCalForFast); + return; + } float targetAngle = 0.0f; // 目标角度（度） //计算纠正角度 - theApp.m_fAngleForFast = CorrectAngle(currentAngle, targetAngle); - LogOutToFile("currentAngled : = %f, CorrectAng = %f", currentAngled, theApp.m_fAngleForFast); + theApp.m_fAngCalForFast = CorrectAngle(currentAngle, targetAngle); + LogOutToFile("currentAngled : = %f, CorrectAng = %f", currentAngled, theApp.m_fAngCalForFast); } //获取设备配置信息返回 @@ -395,9 +403,10 @@ float CDriverMainDlg::CorrectAngle(float currentAngle, float targetAngle) { // 这些参数 是要根据实际情况进行调整的 //pid 对象在第一次调用 CorrectAngle 函数时创建，并且在程序的整个运行期间不会被销毁。即使函数执行结束，pid 对象仍然存在，其内部状态（如积分项 integral、上一次误差 prevError 等）会被持久化保存 static PIDController pid( - 0.6f, //比例系数 P - 0.0f, //积分系数 I - 0.0f, //微分系数 D + //pid参数修改 + 0.4f, //比例系数 P + 0.1f, //积分系数 I + 0.1f, //微分系数 D -10.0f, //积分限幅 10.0f, //积分上限 @@ -1252,10 +1261,10 @@ UINT CDriverMainDlg::SendCanThreadForFast(LPVOID v) { theApp.m_fVelCalForFast = 0.3f; //因为theApp.m_fAngCalForFast 不是指针变量 所以不是判是否为nullptr的 - if (std::isnan(theApp.m_fAngCalForFast)) { - // 尚未初始化 - theApp.m_fAngCalForFast = 0.0f; - } + //if (std::isnan(theApp.m_fAngCalForFast)) { + // // 尚未初始化 + // theApp.m_fAngCalForFast = 0.0f; + //} //theApp.m_fAngCalForFast = UserAng; //下发运动控制参数 LogOutToFile("(inner)SendCanThreadForFast: Vel = %f, Ang = %f", theApp.m_fVelCalForFast, theApp.m_fAngCalForFast); diff --git a/Plugin/Fast/Fast.cpp b/Plugin/Fast/Fast.cpp index 3749db5..8ad57c8 100644 --- a/Plugin/Fast/Fast.cpp +++ b/Plugin/Fast/Fast.cpp @@ -133,7 +133,6 @@ CString CFastApp::SendMsg2Platform(CString strReceiver, int nMsgType, Json::Valu string strJson = writer.write(root); g_pstPipeClient->SendeMsg(WCS_2_WMS_DATA, (char*)strJson.c_str(), strJson.length()); - LogOutToFile("FAST::SendMsg2Platform End"); return ""; diff --git a/Plugin/Fast/Fast.vcxproj b/Plugin/Fast/Fast.vcxproj index 7fba8e1..0efa13f 100644 --- a/Plugin/Fast/Fast.vcxproj +++ b/Plugin/Fast/Fast.vcxproj @@ -23,18 +23,18 @@ {EDB92B51-C3C2-44DA-B21D-6BB824264D08} GrabImage_Display Win32Proj - 10.0 + 10.0.26100.0 Application - v140 + v143 MultiByte true Application - v140 + v143 MultiByte Dynamic diff --git a/Plugin/Fast/FastMainDialog.cpp b/Plugin/Fast/FastMainDialog.cpp index 718e3a5..645a1d1 100644 --- a/Plugin/Fast/FastMainDialog.cpp +++ b/Plugin/Fast/FastMainDialog.cpp @@ -121,7 +121,135 @@ void CFastMainDialog::ProcessPipeMsg(int lMsgId, char* pData, int lLen) LogOutToFile("HttpServiceListener::OnRecvRequest End"); } +// TCP߳ +/*UINT CFastMainDialog::TCPListenerThread(LPVOID pParam) { + CFastMainDialog* pThis = (CFastMainDialog*)pParam; + SOCKET hServer = pThis->m_hServerSocket; + while (true) { + SOCKET hClient = accept(hServer, NULL, NULL); + if (hClient == INVALID_SOCKET) continue; + + char buffer[4096]; + int bytesRecv = recv(hClient, buffer, sizeof(buffer), 0); + if (bytesRecv > 0) { + CString strMsg(buffer, bytesRecv); + pThis->ProcessTCPMessage(strMsg); + } + closesocket(hClient); + } + return 0; +}*/ +UINT CFastMainDialog::TCPListenerThread(LPVOID pParam) { + CFastMainDialog* pThis = (CFastMainDialog*)pParam; + SOCKET hServer = pThis->m_hServerSocket; + + while (true) { + SOCKET hClient = accept(hServer, NULL, NULL); + if (hClient == INVALID_SOCKET) continue; + + // Nagle㷨ѡ + int optval = 1; + setsockopt(hClient, IPPROTO_TCP, TCP_NODELAY, (char*)&optval, sizeof(optval)); + + // ֱӹر + char buffer[4096]; + while (true) { + int bytesRecv = recv(hClient, buffer, sizeof(buffer), 0); + if (bytesRecv <= 0) { // ӶϿ + LogOutToFile("[TCP] Client disconnected or error: %d", WSAGetLastError()); + break; + } + + CString strMsg(buffer, bytesRecv); + pThis->ProcessTCPMessage(strMsg); + } + closesocket(hClient); // չر + } + return 0; +} + +// յTCPϢ +void CFastMainDialog::ProcessTCPMessage(const CString& strJson) { + LogOutToFile("FAST::ProcessTCPMessage Received: %s", strJson); + + Json::Reader reader; + Json::Value root; + if (!reader.parse((LPCTSTR)strJson, root)) { + LogOutToFile("JSON parse error!"); + return; + } + + // ֤Ҫֶ + if (!root.isMember("receiver") || !root.isMember("type")) { + LogOutToFile("Invalid message format"); + return; + } + + // ͨƽ̨ӿת + CString strReceiver = root["receiver"].asString().c_str(); + int nMsgType = root["type"].asInt(); + Json::Value params = root.get("params", Json::nullValue); + + theApp.SendMsg2Platform(strReceiver, nMsgType, params); +} + +// TCP +void CFastMainDialog::StartTCPServer() +{ + // 1. ʼWinsock + WSADATA wsaData; + int wsResult = WSAStartup(MAKEWORD(2, 2), &wsaData); + if (wsResult != 0) { + LogOutToFile("[TCP] WSAStartup failed: %d", wsResult); + return; + } + + // 2. Socket + m_hServerSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + if (m_hServerSocket == INVALID_SOCKET) { + LogOutToFile("[TCP] Socket creation failed: %d", WSAGetLastError()); + WSACleanup(); + return; + } + + // 3. ö˿ڸãѡ + int optval = 1; + setsockopt(m_hServerSocket, SOL_SOCKET, SO_REUSEADDR, (char*)&optval, sizeof(optval)); + + // 4. 󶨶˿ + sockaddr_in service; + service.sin_family = AF_INET; + service.sin_addr.s_addr = INADDR_ANY; + service.sin_port = htons(6000); + + if (::bind(m_hServerSocket, (SOCKADDR*)&service, sizeof(service)) == SOCKET_ERROR) + { + LogOutToFile("[TCP] Bind failed on port 6000: %d", WSAGetLastError()); + closesocket(m_hServerSocket); + WSACleanup(); + return; + } + + // 5. ʼ + if (listen(m_hServerSocket, SOMAXCONN) == SOCKET_ERROR) + { + LogOutToFile("[TCP] Listen failed: %d", WSAGetLastError()); + closesocket(m_hServerSocket); + WSACleanup(); + return; + } + + // 6. ߳ + m_pListenerThread = AfxBeginThread(TCPListenerThread, this); + if (m_pListenerThread == NULL) { + LogOutToFile("[TCP] Thread creation failed!"); + closesocket(m_hServerSocket); + WSACleanup(); + return; + } + LogOutToFile("[TCP] Server successfully started on port 6000"); +} BOOL CFastMainDialog::OnInitDialog() { CDialogEx::OnInitDialog(); @@ -155,7 +283,7 @@ BOOL CFastMainDialog::OnInitDialog() //ʼ InitLocalCamera(); - + StartTCPServer(); // TCP return TRUE; // return TRUE unless you set the focus to a control // 쳣: OCX ҳӦ FALSE } diff --git a/Plugin/Fast/FastMainDialog.h b/Plugin/Fast/FastMainDialog.h index 9da1c0d..d485dda 100644 --- a/Plugin/Fast/FastMainDialog.h +++ b/Plugin/Fast/FastMainDialog.h @@ -13,6 +13,9 @@ public: CFastMainDialog(CWnd* pParent = NULL); // ׼캯 virtual ~CFastMainDialog(); + void StartTCPServer(); + void StopTCPServer(); + // Ի #ifdef AFX_DESIGN_TIME enum { IDD = IDD_MAIN_DIALOG }; @@ -35,4 +38,10 @@ public: afx_msg void OnTimer(UINT_PTR nIDEvent); void ProcessPipeMsg(int lMsgId, char* pData, int lLen); + +private: + static UINT __cdecl TCPListenerThread(LPVOID pParam); + void ProcessTCPMessage(const CString& strJson); + CWinThread* m_pListenerThread; + SOCKET m_hServerSocket; };