优化角度计算与TCP服务器功能
在 `CDriverMainDlg::ProcessPipeMsg` 中修改了角度计算逻辑，增加了对角度绝对值的判断，并更新了 `CorrectAngle` 函数的调用。调整了 PID 控制器的参数以优化控制效果。 在 `CFastMainDialog` 中实现了 TCP 监听线程，新增了 `StartTCPServer` 和 `StopTCPServer` 函数，并在对话框初始化时启动 TCP 服务器。更新了项目文件以支持新的编译环境。
This commit is contained in:
美食博主
parent
b0b79c4f80
commit
22881ced28
@ -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<float>(currentAngled);
|
||||
float cad = static_cast<float>(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);
|
||||
|
||||
@ -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 "";
|
||||
|
||||
@ -23,18 +23,18 @@
|
||||
<ProjectGuid>{EDB92B51-C3C2-44DA-B21D-6BB824264D08}</ProjectGuid>
|
||||
<RootNamespace>GrabImage_Display</RootNamespace>
|
||||
<Keyword>Win32Proj</Keyword>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
<WindowsTargetPlatformVersion>10.0.26100.0</WindowsTargetPlatformVersion>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<PlatformToolset>v140</PlatformToolset>
|
||||
<PlatformToolset>v143</PlatformToolset>
|
||||
<CharacterSet>MultiByte</CharacterSet>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<PlatformToolset>v140</PlatformToolset>
|
||||
<PlatformToolset>v143</PlatformToolset>
|
||||
<CharacterSet>MultiByte</CharacterSet>
|
||||
<UseOfMfc>Dynamic</UseOfMfc>
|
||||
</PropertyGroup>
|
||||
|
||||
@ -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
|
||||
}
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user