agv/agv-control
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

引入PID控制器并更新相关功能

Browse Source 
在多个文件中进行了重要更改：
- 更新 `Driver.rc` 和 `resource.h` 的二进制文件。
- 在 `Driver.vcxproj` 中添加了多线程调试DLL的运行库设置。
- 引入 `pid_controller.h` 和 `pid_controller.cpp`，实现PID控制器功能。
- 在 `DriverMainDlg.cpp` 中添加了对PID控制的支持，包括新成员变量和方法。
- 增加了自动发送和车辆位置更新的功能。
- 在 `Protocol.h` 中添加了新的消息类型 `GUIDE_FAST`。
This commit is contained in:
CaiXiang 2025-06-16 11:16:20 +08:00
parent 88acb23465
commit 4be62027c6
12 changed files with 414 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Plugin/Driver/Driver.rc
View File

Binary file not shown.

3
Plugin/Driver/Driver.vcxproj
View File

@ -118,6 +118,7 @@
<PreprocessorDefinitions>_WINDOWS;_DEBUG;%(PreprocessorDefinitions)</PreprocessorDefinitions> <PreprocessorDefinitions>_WINDOWS;_DEBUG;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<SDLCheck>false</SDLCheck> <SDLCheck>false</SDLCheck>
<AdditionalIncludeDirectories>$(SolutionDir)3rdparty\json\inc;$(SolutionDir)CCEXPipe;$(SolutionDir)3rdparty\curl\inc;..\modbus;$(SolutionDir)3rdparty\can\inc;$(SolutionDir)3rdparty\sqlite;$(SolutionDir)3rdparty\whttp-server-core;$(SolutionDir)3rdparty\openssl\inc;$(SolutionDir)3rdparty\pthread\inc;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> <AdditionalIncludeDirectories>$(SolutionDir)3rdparty\json\inc;$(SolutionDir)CCEXPipe;$(SolutionDir)3rdparty\curl\inc;..\modbus;$(SolutionDir)3rdparty\can\inc;$(SolutionDir)3rdparty\sqlite;$(SolutionDir)3rdparty\whttp-server-core;$(SolutionDir)3rdparty\openssl\inc;$(SolutionDir)3rdparty\pthread\inc;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
<RuntimeLibrary>MultiThreadedDebugDLL</RuntimeLibrary>
</ClCompile> </ClCompile>
<Link> <Link>
<SubSystem>Windows</SubSystem> <SubSystem>Windows</SubSystem>
@ -202,6 +203,7 @@
<ClInclude Include="ColoredListCtrl.h" /> <ClInclude Include="ColoredListCtrl.h" />
<ClInclude Include="DriverMainDlg.h" /> <ClInclude Include="DriverMainDlg.h" />
<ClInclude Include="Map.h" /> <ClInclude Include="Map.h" />
<ClInclude Include="pid_controller.h" />
<ClInclude Include="PluginDriver.h" /> <ClInclude Include="PluginDriver.h" />
<ClInclude Include="PositionView.h" /> <ClInclude Include="PositionView.h" />
<ClInclude Include="Resource.h" /> <ClInclude Include="Resource.h" />
@ -216,6 +218,7 @@
<ClCompile Include="CEXVirtualListCtrl.cpp" /> <ClCompile Include="CEXVirtualListCtrl.cpp" />
<ClCompile Include="ColoredListCtrl.cpp" /> <ClCompile Include="ColoredListCtrl.cpp" />
<ClCompile Include="DriverMainDlg.cpp" /> <ClCompile Include="DriverMainDlg.cpp" />
<ClCompile Include="pid_controller.cpp" />
<ClCompile Include="PluginDriver.cpp" /> <ClCompile Include="PluginDriver.cpp" />
<ClCompile Include="PositionView.cpp" /> <ClCompile Include="PositionView.cpp" />
<ClCompile Include="stdafx.cpp"> <ClCompile Include="stdafx.cpp">

2
Plugin/Driver/Driver.vcxproj.filters
View File

@ -17,6 +17,7 @@
<ClCompile Include="PluginDriver.cpp" /> <ClCompile Include="PluginDriver.cpp" />
<ClCompile Include="DriverMainDlg.cpp" /> <ClCompile Include="DriverMainDlg.cpp" />
<ClCompile Include="PositionView.cpp" /> <ClCompile Include="PositionView.cpp" />
<ClCompile Include="pid_controller.cpp" />
</ItemGroup> </ItemGroup>
<ItemGroup> <ItemGroup>
<ClInclude Include="Resource.h" /> <ClInclude Include="Resource.h" />
@ -31,6 +32,7 @@
<ClInclude Include="PluginDriver.h" /> <ClInclude Include="PluginDriver.h" />
<ClInclude Include="DriverMainDlg.h" /> <ClInclude Include="DriverMainDlg.h" />
<ClInclude Include="PositionView.h" /> <ClInclude Include="PositionView.h" />
<ClInclude Include="pid_controller.h" />
</ItemGroup> </ItemGroup>
<ItemGroup> <ItemGroup>
<ResourceCompile Include="Driver.rc" /> <ResourceCompile Include="Driver.rc" />

361
Plugin/Driver/DriverMainDlg.cpp
View File

@ -1,4 +1,4 @@
// CanDeviceDlg.cpp : 实现文件 // CanDeviceDlg.cpp : 实现文件
// //
#include "stdafx.h" #include "stdafx.h"
@ -6,14 +6,18 @@
#include "resource.h" #include "resource.h"
#include "afxdialogex.h" #include "afxdialogex.h"
#include "PluginDriver.h" #include "PluginDriver.h"
#include "pid_controller.h"
#include <iostream>
#include <cmath>
int StopFlag = 0; int StopFlag = 0;
unsigned long nextrow; unsigned long nextrow;
#define TIMER_UPDATE_POS (19999) #define TIMER_UPDATE_POS (19999)
// CCanDeviceDlg 对话框 // CCanDeviceDlg 对话框
//控件ID只需要在当前窗口中唯一，这里随便写一个就好 //控件ID只需要在当前窗口中唯一，这里随便写一个就好
#define ID_VIRTUAL_LIST_CTRL (20000) #define ID_VIRTUAL_LIST_CTRL (20000)
#define TIMER_ID_ADD_TEST_DATA (2024) #define TIMER_ID_ADD_TEST_DATA (2024)
IMPLEMENT_DYNAMIC(CDriverMainDlg, CDialogEx) IMPLEMENT_DYNAMIC(CDriverMainDlg, CDialogEx)
@ -26,11 +30,13 @@ CDriverMainDlg::CDriverMainDlg(CWnd* pParent /*=NULL*/)
m_bCanRxEn = TRUE; m_bCanRxEn = TRUE;
m_bAutoRefresh = TRUE; m_bAutoRefresh = TRUE;
m_bAutoSend2 = TRUE; m_bAutoSend2 = TRUE;
m_bAutoSendForFast= TRUE;
m_strSendID = "00 00 00 88"; m_strSendID = "00 00 00 88";
m_strSendData = "01 02 03 04 05 06 07 08"; m_strSendData = "01 02 03 04 05 06 07 08";
//m_pMainWnd = (CPluginMainDialog*) pParent; //m_pMainWnd = (CPluginMainDialog*) pParent;
m_StopSendFlag = 0; m_StopSendFlag = 0;
m_StopSendFlag2 = 0; m_StopSendFlag2 = 0;
m_StopSendFlagForFast = 0;
m_DevType = VCI_USBCAN2; m_DevType = VCI_USBCAN2;
m_nAgvAction = A_STOP; m_nAgvAction = A_STOP;
m_nAgvReturnState= RE_A_STOP; m_nAgvReturnState= RE_A_STOP;
@ -57,6 +63,7 @@ void CDriverMainDlg::DoDataExchange(CDataExchange* pDX)
DDX_Check(pDX, IDC_CHECK_CANRX_EN, m_bCanRxEn); DDX_Check(pDX, IDC_CHECK_CANRX_EN, m_bCanRxEn);
DDX_Check(pDX, IDC_AUTO_SEND, m_bAutoSend); DDX_Check(pDX, IDC_AUTO_SEND, m_bAutoSend);
DDX_Check(pDX, IDC_AUTO_SEND2, m_bAutoSend2); DDX_Check(pDX, IDC_AUTO_SEND2, m_bAutoSend2);
DDX_Check(pDX, IDC_AUTO_SEND3, m_bAutoSendForFast);
DDX_Check(pDX, IDC_CHECK_AUTO_REFRESH_SHOW, m_bAutoRefresh); DDX_Check(pDX, IDC_CHECK_AUTO_REFRESH_SHOW, m_bAutoRefresh);
//DDX_Control(pDX, IDC_LIST1, m_MessageList); //DDX_Control(pDX, IDC_LIST1, m_MessageList);
@ -78,10 +85,12 @@ BEGIN_MESSAGE_MAP(CDriverMainDlg, CDialogEx)
ON_BN_CLICKED(IDC_BUTTON_AGVFORWARD, &CDriverMainDlg::OnBnClickedButtonAgvforward) ON_BN_CLICKED(IDC_BUTTON_AGVFORWARD, &CDriverMainDlg::OnBnClickedButtonAgvforward)
ON_BN_CLICKED(IDC_BUTTON_AGVBACKWARD, &CDriverMainDlg::OnBnClickedButtonAgvbackward) ON_BN_CLICKED(IDC_BUTTON_AGVBACKWARD, &CDriverMainDlg::OnBnClickedButtonAgvbackward)
ON_WM_TIMER() ON_WM_TIMER()
ON_EN_CHANGE(IDC_EDIT1, &CDriverMainDlg::OnEnChangeEdit1)
ON_BN_CLICKED(IDC_AUTO_SEND3, &CDriverMainDlg::OnBnClickedAutoSend3)
END_MESSAGE_MAP() END_MESSAGE_MAP()
//一位十六进制转换为十进制 //一位十六进制转换为十进制
int CDriverMainDlg::HexChar(char c) int CDriverMainDlg::HexChar(char c)
{ {
if ((c >= '0') && (c <= '9')) if ((c >= '0') && (c <= '9'))
@ -94,7 +103,7 @@ int CDriverMainDlg::HexChar(char c)
return 0x10; return 0x10;
} }
//两位十六进制数转换为十进制 //两位十六进制数转换为十进制
int CDriverMainDlg::Str2Hex(CString str) int CDriverMainDlg::Str2Hex(CString str)
{ {
int len = str.GetLength(); int len = str.GetLength();
@ -123,7 +132,7 @@ int CDriverMainDlg::Str2Hex(CString str)
void CDriverMainDlg::InitVirtualList() void CDriverMainDlg::InitVirtualList()
{ {
//信息显示列表初始化 //信息显示列表初始化
m_pstVirtualList->InsertColumn(0, "Seq"); m_pstVirtualList->InsertColumn(0, "Seq");
m_pstVirtualList->SetColumnWidth(0, 110); m_pstVirtualList->SetColumnWidth(0, 110);
m_pstVirtualList->InsertColumn(1, "Time"); m_pstVirtualList->InsertColumn(1, "Time");
@ -153,7 +162,7 @@ void CDriverMainDlg::UpdatePositionView()
InvalidateRect(rect); InvalidateRect(rect);
} }
//更新车辆位置，nPosition记录从巷道口的里程，从0开始计算 //更新车辆位置，nPosition记录从巷道口的里程，从0开始计算
void CDriverMainDlg::UpdateVehiclePosition(int nPosition) void CDriverMainDlg::UpdateVehiclePosition(int nPosition)
{ {
m_PositionView.m_nPosition = nPosition; m_PositionView.m_nPosition = nPosition;
@ -162,18 +171,18 @@ void CDriverMainDlg::UpdateVehiclePosition(int nPosition)
void CDriverMainDlg::OnTimer(UINT_PTR nIDEvent) void CDriverMainDlg::OnTimer(UINT_PTR nIDEvent)
{ {
// TODO: 在此添加消息处理程序代码和/或调用默认值 // TODO: 在此添加消息处理程序代码和/或调用默认值
static int n = 0; static int n = 0;
if (TIMER_UPDATE_POS == nIDEvent) if (TIMER_UPDATE_POS == nIDEvent)
{ {
//定时刷新车辆位置 //定时刷新车辆位置
m_PositionView.m_nPosition = n++ * 600; m_PositionView.m_nPosition = n++ * 600;
UpdatePositionView(); UpdatePositionView();
} }
else else
{ {
//自定义定时器，无需调用基类OnTimer，否则定时器会被强制结束 //自定义定时器，无需调用基类OnTimer，否则定时器会被强制结束
CDialog::OnTimer(nIDEvent); CDialog::OnTimer(nIDEvent);
} }
} }
@ -181,21 +190,21 @@ void CDriverMainDlg::OnTimer(UINT_PTR nIDEvent)
void CDriverMainDlg::WriteVirtualList(CString strFrameId, CString strFrameData, int nTxRx, int nCanIdx, int nFrameType, int nFrameFormat) void CDriverMainDlg::WriteVirtualList(CString strFrameId, CString strFrameData, int nTxRx, int nCanIdx, int nFrameType, int nFrameFormat)
{ {
//数据按行格式化 //数据按行格式化
char acTemp[DATA_LINE_SIZE + 1] = { 0 }; char acTemp[DATA_LINE_SIZE + 1] = { 0 };
//发送信息列表显示 //发送信息列表显示
CString strTime; CString strTime;
SYSTEMTIME systime; SYSTEMTIME systime;
GetLocalTime(&systime); GetLocalTime(&systime);
char acId[11] = { 0 }; char acId[11] = { 0 };
memset(acId, 32, 10); //初始化为空格字符串 memset(acId, 32, 10); //初始化为空格字符串
memcpy(acId, strFrameId.GetBuffer(), strFrameId.GetLength()); memcpy(acId, strFrameId.GetBuffer(), strFrameId.GetLength());
char acData[33] = { 0 }; char acData[33] = { 0 };
memset(acData, 32, 32); //初始化为空格字符串 memset(acData, 32, 32); //初始化为空格字符串
memcpy(acData, strFrameData.GetBuffer(), strFrameData.GetLength()); memcpy(acData, strFrameData.GetBuffer(), strFrameData.GetLength());
sprintf_s(acTemp, "%-10d,%02d:%02d:%02d:%03d,%-3d,%-3d,%s,%-3d,%-3d,%-3d,%s\n", m_pQueue->GetAutoSn(), sprintf_s(acTemp, "%-10d,%02d:%02d:%02d:%03d,%-3d,%-3d,%s,%-3d,%-3d,%-3d,%s\n", m_pQueue->GetAutoSn(),
@ -208,31 +217,31 @@ void CDriverMainDlg::WriteVirtualList(CString strFrameId, CString strFrameData,
8, 8,
acData); acData);
//调用接口写入数据 //调用接口写入数据
m_pQueue->WriteItem(acTemp); m_pQueue->WriteItem(acTemp);
} }
//和模块进程通讯的管道回调函数： //和模块进程通讯的管道回调函数：
void g_PipeCallBack(void* pObj, int lMsgId, WPARAM wparam, LPARAM lparam) void g_PipeCallBack(void* pObj, int lMsgId, WPARAM wparam, LPARAM lparam)
{ {
CDriverMainDlg* pModule = (CDriverMainDlg*)pObj; CDriverMainDlg* pModule = (CDriverMainDlg*)pObj;
if (CEXPIPE_CONNECT_OK == lMsgId) if (CEXPIPE_CONNECT_OK == lMsgId)
{ {
//连接成功 //连接成功
//if (FALSE == pModule->PostMessage(WM_PLATFORM_CONNECT_OK, NULL, NULL)) { LogOutToFile("g_PipeCallBack PostMessage error[%d]", lMsgId); } //if (FALSE == pModule->PostMessage(WM_PLATFORM_CONNECT_OK, NULL, NULL)) { LogOutToFile("g_PipeCallBack PostMessage error[%d]", lMsgId); }
LogOutToFile("已连接父进程管道"); LogOutToFile("已连接父进程管道");
//向WMS服务器请求设备配置信息 //向WMS服务器请求设备配置信息
//theApp.SendMsg2Platform("WMS", DEVICE_CONFIG_REQ); //theApp.SendMsg2Platform("WMS", DEVICE_CONFIG_REQ);
} }
else if (CEXPIPE_DIS_CLIENT == lMsgId) else if (CEXPIPE_DIS_CLIENT == lMsgId)
{ {
//管道断开，结束 //管道断开，结束
#ifndef _DEBUG #ifndef _DEBUG
LogOutToFile("父进程连接管道断开，本模块自动退出"); LogOutToFile("父进程连接管道断开，本模块自动退出");
//if (FALSE == pModule->PostMessage(WM_CLOSE, NULL, NULL)) { LogOutToFile("g_PipeCallBack PostMessage error[%d]", lMsgId); } //if (FALSE == pModule->PostMessage(WM_CLOSE, NULL, NULL)) { LogOutToFile("g_PipeCallBack PostMessage error[%d]", lMsgId); }
pModule->PostMessage(WM_COMMAND, MAKEWPARAM(ID_TRAY_EXIT, 0), 0); pModule->PostMessage(WM_COMMAND, MAKEWPARAM(ID_TRAY_EXIT, 0), 0);
#endif #endif
@ -258,6 +267,8 @@ void CDriverMainDlg::GetRoadwayInfo(int nReqPosId)
theApp.SendMsg2Platform("WMS", ROADWAY_CONFIG_REQ, param); theApp.SendMsg2Platform("WMS", ROADWAY_CONFIG_REQ, param);
} }
//处理 平台各个模块 传过来的消息
void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen) void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen)
{ {
if (lLen == 0) if (lLen == 0)
@ -267,7 +278,7 @@ void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen)
PIPE_DATA_STRUCT* pstData = (PIPE_DATA_STRUCT*)pData; PIPE_DATA_STRUCT* pstData = (PIPE_DATA_STRUCT*)pData;
//平台转发给插件的消息 //平台转发给插件的消息
if (pstData->lMsgId == MAIN_2_MODULE_WMS && pstData->lDataLen > 0) if (pstData->lMsgId == MAIN_2_MODULE_WMS && pstData->lDataLen > 0)
{ {
Json::Reader reader; Json::Reader reader;
@ -279,13 +290,13 @@ void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen)
int nMsgType = root["type"].asInt(); int nMsgType = root["type"].asInt();
//从科聪控制器收到切换导航指令，向wms请求获取通道信息，包括标签等 //从科聪控制器收到切换导航指令，向wms请求获取通道信息，包括标签等
if (nMsgType == CHANGE_GUIDE_TYPE) if (nMsgType == CHANGE_GUIDE_TYPE)
{ {
AfxMessageBox("AGV到达请求点，切换导航方式"); AfxMessageBox("AGV到达请求点，切换导航方式");
GetRoadwayInfo(0); GetRoadwayInfo(0);
} }
//从wms获取巷道信息返回 //从wms获取巷道信息返回
else if (nMsgType == ROADWAY_CONFIG_RET) else if (nMsgType == ROADWAY_CONFIG_RET)
{ {
Json::Value data = root["params"]; Json::Value data = root["params"];
@ -306,7 +317,7 @@ void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen)
UpdatePositionView(); UpdatePositionView();
} }
//从QR插件收到二维码信息 //从QR插件收到二维码信息
else if (nMsgType == GUIDE_QRCODE) else if (nMsgType == GUIDE_QRCODE)
{ {
Json::Value data = root["params"]; Json::Value data = root["params"];
@ -317,16 +328,37 @@ void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen)
} }
//获取设备配置信息返回 //author: caixiang
//todo 从FAST插件 收到视觉信息
//else if (nMsgType == GUIDE_FAST)
//{
// Json::Value data = root["params"];
// theApp.m_fAngleForFast = data["angle"].asDouble();
// //theApp.m_nX = data["x"].asInt();
// //theApp.m_nY = data["y"].asInt();
// //theApp.m_nTag = data["tag"].asInt();
//}
else if (nMsgType == GUIDE_FAST)
{
Json::Value data = root["params"];
double currentAngled = data["angle"].asDouble(); // 实际角度（度）
float currentAngle = static_cast<float>(currentAngled);
float targetAngle = 0.0f; // 目标角度（度）
//计算纠正角度
theApp.m_fAngleForFast = CorrectAngle(currentAngle, targetAngle);
}
//获取设备配置信息返回
/*if (nMsgType == DEVICE_CONFIG_RET) /*if (nMsgType == DEVICE_CONFIG_RET)
{ {
} }
else if (nMsgType == SET_TRANS_MODE_REQ && m_bDeviceInit) //设置输送线的工作模式 else if (nMsgType == SET_TRANS_MODE_REQ && m_bDeviceInit) //设置输送线的工作模式
{ {
} }
else if (nMsgType == GET_TRANS_STATE_REQ)//请求获取输送线状态 else if (nMsgType == GET_TRANS_STATE_REQ)//请求获取输送线状态
{ {
}*/ }*/
@ -343,8 +375,48 @@ void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen)
LogOutToFile("HttpServiceListener::OnRecvRequest End"); LogOutToFile("HttpServiceListener::OnRecvRequest End");
} }
// 角度规范化函数，将角度限制在-180°到180°之间
float CDriverMainDlg::NormalizeAngle(float angle) {
angle = std::fmod(angle, 360.0f);
if (angle > 180.0f)
angle -= 360.0f;
else if (angle < -180.0f)
angle += 360.0f;
return angle;
}
// CCanDeviceDlg 消息处理程序 //auth : caixiang
// 基于PID的角度纠正函数
float CDriverMainDlg::CorrectAngle(float currentAngle, float targetAngle) {
// 创建PID控制器实例
// 参数：比例系数、积分系数、微分系数、积分限幅、积分上限、输出下限、输出上限、时间步长(秒)
// 这些参数 是要根据实际情况进行调整的
static PIDController pid(
0.5f, //比例系数
0.2f, //积分系数
0.1f, //微分系数
-10.0f, //积分限幅
10.0f, //积分上限
-45.0f, //输出下限
45.0f, //输出上限
0.01f //时间步长(秒)
);
// 规范化角度，确保角度在-180°到180°之间
currentAngle = NormalizeAngle(currentAngle);
targetAngle = NormalizeAngle(targetAngle);
// 计算纠正角度，然后通过can协议，下发给电机。
float correction = pid.compute(targetAngle, currentAngle);
return correction;
}
// CCanDeviceDlg 消息处理程序
BOOL CDriverMainDlg::OnInitDialog() BOOL CDriverMainDlg::OnInitDialog()
{ {
@ -364,7 +436,7 @@ BOOL CDriverMainDlg::OnInitDialog()
CString iniPath = theApp.m_strModulePath.Left(theApp.m_strModulePath.ReverseFind('\\') + 1); CString iniPath = theApp.m_strModulePath.Left(theApp.m_strModulePath.ReverseFind('\\') + 1);
//m_strModulePath = theApp.m_strModulePath.ReverseFind("\\"); //m_strModulePath = theApp.m_strModulePath.ReverseFind("\\");
iniPath = iniPath + "pipe.ini"; iniPath = iniPath + "pipe.ini";
//中文名称，仅显示用 //中文名称，仅显示用
GetPrivateProfileString("AGV-MODULE", "PIPE_DRIVER", "", acPipe, 256, iniPath); GetPrivateProfileString("AGV-MODULE", "PIPE_DRIVER", "", acPipe, 256, iniPath);
strPipe = acPipe; strPipe = acPipe;
} }
@ -381,7 +453,7 @@ BOOL CDriverMainDlg::OnInitDialog()
TRACE0("Failed to create MyVirtualListCtrl\n"); TRACE0("Failed to create MyVirtualListCtrl\n");
delete m_pstVirtualList; delete m_pstVirtualList;
m_pstVirtualList = nullptr; m_pstVirtualList = nullptr;
return FALSE; // 返回 FALSE 以使框架知道未成功初始化 return FALSE; // 返回 FALSE 以使框架知道未成功初始化
} }
InitVirtualList(); InitVirtualList();
@ -394,7 +466,7 @@ BOOL CDriverMainDlg::OnInitDialog()
m_pQueue = new CEXMemFileQueue(m_strDataDir.GetString()); m_pQueue = new CEXMemFileQueue(m_strDataDir.GetString());
m_pstVirtualList->SetData(m_pQueue); m_pstVirtualList->SetData(m_pQueue);
//m_pstVirtualList->StartAutoRefresh(10);//列表10毫秒刷新一次 //m_pstVirtualList->StartAutoRefresh(10);//列表10毫秒刷新一次
CDialogEx::OnInitDialog(); CDialogEx::OnInitDialog();
@ -410,7 +482,7 @@ BOOL CDriverMainDlg::OnInitDialog()
if (m_bCanRxEn) if (m_bCanRxEn)
{ {
StopFlag = 0; StopFlag = 0;
//开启接收线程 //开启接收线程
AfxBeginThread(ReceiveCanThread, this); AfxBeginThread(ReceiveCanThread, this);
} }
else else
@ -419,9 +491,16 @@ BOOL CDriverMainDlg::OnInitDialog()
if (m_bAutoSend2) if (m_bAutoSend2)
{ {
m_StopSendFlag2 = 0; m_StopSendFlag2 = 0;
//开启发送线程 //开启发送线程
AfxBeginThread(SendCanThread2, this); AfxBeginThread(SendCanThread2, this);
} }
if (m_bAutoSendForFast)
{
m_StopSendFlagForFast = 0;
//开启发送线程
AfxBeginThread(SendCanThreadForFast, this);
}
else else
m_StopSendFlag2 = 1; m_StopSendFlag2 = 1;
@ -436,12 +515,12 @@ BOOL CDriverMainDlg::OnInitDialog()
} }
//打开设备 //打开设备
BOOL CDriverMainDlg::OpenCanDevice() BOOL CDriverMainDlg::OpenCanDevice()
{ {
DWORD Reserved = 0; DWORD Reserved = 0;
//打开设备 //打开设备
if (VCI_OpenDevice(VCI_USBCAN2, 0, Reserved) != 1) if (VCI_OpenDevice(VCI_USBCAN2, 0, Reserved) != 1)
{ {
//MessageBox("open failed"); //MessageBox("open failed");
@ -455,7 +534,7 @@ BOOL CDriverMainDlg::OpenCanDevice()
InitInfo->AccCode = 0x80000000; InitInfo->AccCode = 0x80000000;
InitInfo->AccMask = 0xFFFFFFFF; InitInfo->AccMask = 0xFFFFFFFF;
InitInfo->Mode = 0; //Data ;Remote; InitInfo->Mode = 0; //Data ;Remote;
//初始化通道0 //初始化通道0
if (VCI_InitCAN(VCI_USBCAN2, 0, 0, InitInfo) != 1) //can-0 if (VCI_InitCAN(VCI_USBCAN2, 0, 0, InitInfo) != 1) //can-0
{ {
//MessageBox("Init-CAN failed!"); //MessageBox("Init-CAN failed!");
@ -463,14 +542,14 @@ BOOL CDriverMainDlg::OpenCanDevice()
return FALSE; return FALSE;
} }
Sleep(100); Sleep(100);
//初始化通道0 //初始化通道0
if (VCI_StartCAN(VCI_USBCAN2, 0, 0) != 1) //can-0 if (VCI_StartCAN(VCI_USBCAN2, 0, 0) != 1) //can-0
{ {
//MessageBox("Start-CAN failed!"); //MessageBox("Start-CAN failed!");
//m_pMainWnd->InsertLog(LOG_ERROR, "Start-CAN-Index0 Failed!"); //m_pMainWnd->InsertLog(LOG_ERROR, "Start-CAN-Index0 Failed!");
return FALSE; return FALSE;
} }
//初始化通道1 //初始化通道1
//if (m_nDevType == 1) //if (m_nDevType == 1)
{ {
if (VCI_InitCAN(VCI_USBCAN2, 0, 1, InitInfo) != 1) //can-1 if (VCI_InitCAN(VCI_USBCAN2, 0, 1, InitInfo) != 1) //can-1
@ -483,7 +562,7 @@ BOOL CDriverMainDlg::OpenCanDevice()
Sleep(100); Sleep(100);
InitInfo->Mode = 0; //Data ;Remote; InitInfo->Mode = 0; //Data ;Remote;
//初始化通道1 //初始化通道1
if (VCI_StartCAN(VCI_USBCAN2, 0, 1) != 1) //can-0 if (VCI_StartCAN(VCI_USBCAN2, 0, 1) != 1) //can-0
{ {
//MessageBox("Start-CAN failed!"); //MessageBox("Start-CAN failed!");
@ -503,18 +582,18 @@ void CDriverMainDlg::UpdateCanStatue(BOOL bStatue)
{ {
if (TRUE == bStatue) if (TRUE == bStatue)
{ {
GetDlgItem(IDC_STATUS_CAN)->SetWindowText("已连接"); GetDlgItem(IDC_STATUS_CAN)->SetWindowText("已连接");
} }
else else
{ {
GetDlgItem(IDC_STATUS_CAN)->SetWindowText("已关闭"); GetDlgItem(IDC_STATUS_CAN)->SetWindowText("已关闭");
} }
} }
void CDriverMainDlg::ReadConfigFromIni() void CDriverMainDlg::ReadConfigFromIni()
{ {
CString strIniPath = theApp.m_strModulePath + "\\config.ini"; CString strIniPath = theApp.m_strModulePath + "\\config.ini";
// 初始化设备配置 // 初始化设备配置
m_nSendFrameFormatIdx0 = GetPrivateProfileInt("CAN", "FRAME_FORMAT_IDX0", 0, strIniPath); m_nSendFrameFormatIdx0 = GetPrivateProfileInt("CAN", "FRAME_FORMAT_IDX0", 0, strIniPath);
m_nSendFrameTypeIdx0 = GetPrivateProfileInt("CAN", "FRAME_TYPE_IDX0", 0, strIniPath); m_nSendFrameTypeIdx0 = GetPrivateProfileInt("CAN", "FRAME_TYPE_IDX0", 0, strIniPath);
m_nSendFrameFormatIdx1 = GetPrivateProfileInt("CAN", "FRAME_FORMAT_IDX1", 0, strIniPath); m_nSendFrameFormatIdx1 = GetPrivateProfileInt("CAN", "FRAME_FORMAT_IDX1", 0, strIniPath);
@ -522,14 +601,14 @@ void CDriverMainDlg::ReadConfigFromIni()
return; // return TRUE unless you set the focus to a control return; // return TRUE unless you set the focus to a control
// 异常: OCX 属性页应返回 FALSE // 异常: OCX 属性页应返回 FALSE
} }
//void CDriverMainDlg::Drive_enable() //使能 //void CDriverMainDlg::Drive_enable() //使能
//{ //{
// char can2_buff[8] = { 0 }; // char can2_buff[8] = { 0 };
// can2_buff[0] = 0x23; // can2_buff[0] = 0x23;
@ -543,7 +622,7 @@ void CDriverMainDlg::ReadConfigFromIni()
// SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, 0x600001, can2_buff); // SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, 0x600001, can2_buff);
// SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, 0x600002, can2_buff); // SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, 0x600002, can2_buff);
//} //}
//void CDriverMainDlg::Drive_disable() //失能 //void CDriverMainDlg::Drive_disable() //失能
//{ //{
// char can2_buff[8] = { 0 }; // char can2_buff[8] = { 0 };
// can2_buff[0] = 0x23; // can2_buff[0] = 0x23;
@ -560,15 +639,16 @@ void CDriverMainDlg::ReadConfigFromIni()
/* /*
param0: can设备索引 param0: can设备索引
param1: can口索引 param1: can口索引
param2: standard/extend param2: standard/extend
param3: data/remote param3: data/remote
received can msg
*/ */
void CDriverMainDlg::SendCanData(int nDevIdx, int nCanIdx, int nFrameType, int nFrameFormat, UINT32 acFrameId, char acFrameData[8]) void CDriverMainDlg::SendCanData(int nDevIdx, int nCanIdx, int nFrameType, int nFrameFormat, UINT32 acFrameId, char acFrameData[8])
{ {
//从界面获取发送信息 //从界面获取发送信息
VCI_CAN_OBJ sendbuf[1]; VCI_CAN_OBJ sendbuf[1];
int datanum = 8; int datanum = 8;
@ -583,18 +663,18 @@ void CDriverMainDlg::SendCanData(int nDevIdx, int nCanIdx, int nFrameType, int n
sendbuf->Data[i] = acFrameData[i]; sendbuf->Data[i] = acFrameData[i];
}*/ }*/
/****************************************************************************/ /****************************************************************************/
/******************************从界面获取发送信息完毕***********************/ /******************************从界面获取发送信息完毕***********************/
/****************************************************************************/ /****************************************************************************/
static bool snederr_flag = false; static bool snederr_flag = false;
//调用动态链接库发送函数 //调用动态链接库发送函数
int flag = VCI_Transmit(m_DevType, m_DevIndex, nCanIdx, sendbuf, 1);//CAN message send int flag = VCI_Transmit(m_DevType, m_DevIndex, nCanIdx, sendbuf, 1);//CAN message send
if (flag<1) if (flag<1)
{ {
//VCI_CloseDevice(m_DevType, m_DevIndex); //VCI_CloseDevice(m_DevType, m_DevIndex);
if (flag == -1) if (flag == -1)
{ {
//Can设备已断开 //Can设备已断开
//MessageBox("failed- device not open\n"); //MessageBox("failed- device not open\n");
//m_pMainWnd->InsertLog(LOG_ERROR, "Failed- Device Not Open!"); //m_pMainWnd->InsertLog(LOG_ERROR, "Failed- Device Not Open!");
OpenCanDevice(); OpenCanDevice();
@ -638,7 +718,7 @@ void CDriverMainDlg::OnCheckCanrxEn()
if (m_bCanRxEn) if (m_bCanRxEn)
{ {
StopFlag = 0; StopFlag = 0;
//开启接收线程 //开启接收线程
AfxBeginThread(ReceiveCanThread, this); AfxBeginThread(ReceiveCanThread, this);
} }
else else
@ -657,11 +737,11 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
for (int kCanIndex = 0; kCanIndex<2; kCanIndex++) for (int kCanIndex = 0; kCanIndex<2; kCanIndex++)
{ {
//调用动态链接看接收函数 //调用动态链接看接收函数
int NumValue = VCI_Receive(pThis->m_DevType, pThis->m_DevIndex, kCanIndex, pCanObj, 200, 0); int NumValue = VCI_Receive(pThis->m_DevType, pThis->m_DevIndex, kCanIndex, pCanObj, 200, 0);
if (NumValue >0) if (NumValue >0)
{ {
//接收信息列表显示 //接收信息列表显示
CString strTime; CString strTime;
SYSTEMTIME systime; SYSTEMTIME systime;
GetLocalTime(&systime); GetLocalTime(&systime);
@ -673,7 +753,7 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
CString strFrameId, strFrameData, strTmp; CString strFrameId, strFrameData, strTmp;
strFrameId.Format("%08X", pCanObj[num].ID); strFrameId.Format("%08X", pCanObj[num].ID);
for (int i = 0; i < (pCanObj[num].DataLen); i++) //数据信息 for (int i = 0; i < (pCanObj[num].DataLen); i++) //数据信息
{ {
strTmp.Format("%02X ", pCanObj[num].Data[i]); strTmp.Format("%02X ", pCanObj[num].Data[i]);
strFrameData += strTmp; strFrameData += strTmp;
@ -681,9 +761,9 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
pThis->WriteVirtualList(strFrameId, strFrameData, 1, kCanIndex); pThis->WriteVirtualList(strFrameId, strFrameData, 1, kCanIndex);
//在这里处理收到的CAN消息 //在这里处理收到的CAN消息
//解析定位引导数据 //解析定位引导数据
pThis->AnalysiseCanData(kCanIndex, pCanObj[num]); pThis->AnalysiseCanData(kCanIndex, pCanObj[num]);
/*if (MANUAL_MODE ==pThis->m_pMainWnd->m_nAgvMode) /*if (MANUAL_MODE ==pThis->m_pMainWnd->m_nAgvMode)
@ -692,7 +772,7 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
} }
else else
{ {
//计算运动控制参数(自动模式),需要考虑m_nAgvAction值 //计算运动控制参数(自动模式),需要考虑m_nAgvAction值
if (Control_Mode_Switch) if (Control_Mode_Switch)
{ {
Control_Mode_Switch = 0; Control_Mode_Switch = 0;
@ -706,7 +786,7 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
} }
else if (NumValue == -1) else if (NumValue == -1)
{ {
// USB-CAN设备不存在或USB掉线，可以调用VCI_CloseDevice并重新VCI_OpenDevice。如此可以达到USB-CAN设备热插拔的效果。 // USB-CAN设备不存在或USB掉线，可以调用VCI_CloseDevice并重新VCI_OpenDevice。如此可以达到USB-CAN设备热插拔的效果。
pThis->UpdateCanStatue(FALSE); pThis->UpdateCanStatue(FALSE);
pThis->OpenCanDevice(); pThis->OpenCanDevice();
} }
@ -723,7 +803,7 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
} }
//解析接收到的CAN数据 //解析接收到的CAN数据
void CDriverMainDlg::AnalysiseCanData(int nCanIndex, VCI_CAN_OBJ objCan) void CDriverMainDlg::AnalysiseCanData(int nCanIndex, VCI_CAN_OBJ objCan)
{ {
CString nEdit; CString nEdit;
@ -731,13 +811,13 @@ void CDriverMainDlg::AnalysiseCanData(int nCanIndex, VCI_CAN_OBJ objCan)
{ {
if (objCan.ID == 0x5B1) if (objCan.ID == 0x5B1)
{ {
//里程数据 //里程数据
m_stSensorData.Mile_info = (float *)objCan.Data; m_stSensorData.Mile_info = (float *)objCan.Data;
CString nEdit; CString nEdit;
nEdit.Format(_T("%f"), *m_stSensorData.Mile_info); // %x-16进制显示; %d-10进制显示 nEdit.Format(_T("%f"), *m_stSensorData.Mile_info); // %x-16进制显示; %d-10进制显示
Mil_info.SetWindowText(nEdit); Mil_info.SetWindowText(nEdit);
nEdit.Format(_T("%d"), theApp.m_nTag); // %x-16进制显示; %d-10进制显示 nEdit.Format(_T("%d"), theApp.m_nTag); // %x-16进制显示; %d-10进制显示
TAG.SetWindowText(nEdit); TAG.SetWindowText(nEdit);
} }
} }
@ -772,19 +852,19 @@ void CDriverMainDlg::CalculateAutoCanParam()
} }
} }
int CDriverMainDlg::Auto_Forward(float speedpar) //去载货区 进入雷达减速区通过速度系数调整速度 int CDriverMainDlg::Auto_Forward(float speedpar) //去载货区 进入雷达减速区通过速度系数调整速度
{ {
int Position_P = 4000; //转向比例 int Position_P = 4000; //转向比例
if (m_stSensorData.FMagnetism_Valid == 1) //磁条数据有效 if (m_stSensorData.FMagnetism_Valid == 1) //磁条数据有效
{ {
m_stDrvierControl.Diversion_Position = Position_P * m_stSensorData.FMagnetism_Offset; // 计算出比例部分 m_stDrvierControl.Diversion_Position = Position_P * m_stSensorData.FMagnetism_Offset; // 计算出比例部分
m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle - m_stDrvierControl.Diversion_Position; m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle - m_stDrvierControl.Diversion_Position;
m_stDrvierControl.Drive_Speed = 3000 * speedpar; m_stDrvierControl.Drive_Speed = 3000 * speedpar;
if (m_stDrvierControl.Diversion_Position < m_stDrvierControl.AutoMIN_Angle) if (m_stDrvierControl.Diversion_Position < m_stDrvierControl.AutoMIN_Angle)
m_stDrvierControl.Diversion_Position = m_stDrvierControl.AutoMIN_Angle; m_stDrvierControl.Diversion_Position = m_stDrvierControl.AutoMIN_Angle;
else if(m_stDrvierControl.Diversion_Position > m_stDrvierControl.AutoMAX_Angle) else if(m_stDrvierControl.Diversion_Position > m_stDrvierControl.AutoMAX_Angle)
m_stDrvierControl.Diversion_Position = m_stDrvierControl.AutoMAX_Angle; m_stDrvierControl.Diversion_Position = m_stDrvierControl.AutoMAX_Angle;
if (m_stSensorData.FMagnetism_ALLTrue) //横向磁条停车 正常停车 if (m_stSensorData.FMagnetism_ALLTrue) //横向磁条停车 正常停车
{ {
m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle; m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle;
m_stDrvierControl.Drive_Speed = 0; m_stDrvierControl.Drive_Speed = 0;
@ -792,26 +872,26 @@ int CDriverMainDlg::Auto_Forward(float speedpar) //ȥ
} }
return RE_NORMAL_RUN; return RE_NORMAL_RUN;
} }
else //磁条无效异常停车 else //磁条无效异常停车
{ {
m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle; m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle;
m_stDrvierControl.Drive_Speed = 0; m_stDrvierControl.Drive_Speed = 0;
return RE_UNUSUAL_STOP; return RE_UNUSUAL_STOP;
} }
} }
int CDriverMainDlg::Auto_Backward(float speedpar) //去卸货区 int CDriverMainDlg::Auto_Backward(float speedpar) //去卸货区
{ {
int Position_P = 4000; int Position_P = 4000;
if (m_stSensorData.BMagnetism_Valid == 1) //磁条数据有效 if (m_stSensorData.BMagnetism_Valid == 1) //磁条数据有效
{ {
m_stDrvierControl.Diversion_Position = Position_P * m_stSensorData.BMagnetism_Offset; // 计算出比例部分 m_stDrvierControl.Diversion_Position = Position_P * m_stSensorData.BMagnetism_Offset; // 计算出比例部分
m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle - m_stDrvierControl.Diversion_Position; m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle - m_stDrvierControl.Diversion_Position;
m_stDrvierControl.Drive_Speed = -2000 * speedpar; m_stDrvierControl.Drive_Speed = -2000 * speedpar;
if (m_stDrvierControl.Diversion_Position < m_stDrvierControl.AutoMIN_Angle) if (m_stDrvierControl.Diversion_Position < m_stDrvierControl.AutoMIN_Angle)
m_stDrvierControl.Diversion_Position = m_stDrvierControl.AutoMIN_Angle; m_stDrvierControl.Diversion_Position = m_stDrvierControl.AutoMIN_Angle;
else if (m_stDrvierControl.Diversion_Position > m_stDrvierControl.AutoMAX_Angle) else if (m_stDrvierControl.Diversion_Position > m_stDrvierControl.AutoMAX_Angle)
m_stDrvierControl.Diversion_Position = m_stDrvierControl.AutoMAX_Angle; m_stDrvierControl.Diversion_Position = m_stDrvierControl.AutoMAX_Angle;
if (m_stSensorData.BMagnetism_ALLTrue) //横向磁条停车 正常停车 if (m_stSensorData.BMagnetism_ALLTrue) //横向磁条停车 正常停车
{ {
m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle; m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle;
m_stDrvierControl.Drive_Speed = 0; m_stDrvierControl.Drive_Speed = 0;
@ -831,7 +911,7 @@ int Speed = 0;
int LastSpeed = 0; int LastSpeed = 0;
void CDriverMainDlg::SendCanControlData(float Vel, float Ang) void CDriverMainDlg::SendCanControlData(float Vel, float Ang)
{ {
UINT32 acId = 0x5A1; //速度 角度帧 UINT32 acId = 0x5A1; //速度 角度帧
char acData[8] = {}; char acData[8] = {};
acData[0] = *((char*)&theApp.m_fVelCal + 0); acData[0] = *((char*)&theApp.m_fVelCal + 0);
acData[1] = *((char*)&theApp.m_fVelCal + 1); acData[1] = *((char*)&theApp.m_fVelCal + 1);
@ -844,6 +924,32 @@ void CDriverMainDlg::SendCanControlData(float Vel, float Ang)
SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, acId, acData); SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, acId, acData);
} }
void CDriverMainDlg::SendCanControlDataFAST(float Vel, float Ang)
{
UINT32 acId = 0x5A1; // 速度 角度帧
char acData[8] = {};
// 使用union结构来处理字节序
union {
float f;
unsigned char bytes[4];
} velUnion, angUnion;
velUnion.f = Vel;
angUnion.f = Ang;
// 按小端字节序存储数据
acData[0] = velUnion.bytes[0];
acData[1] = velUnion.bytes[1];
acData[2] = velUnion.bytes[2];
acData[3] = velUnion.bytes[3];
acData[4] = angUnion.bytes[0];
acData[5] = angUnion.bytes[1];
acData[6] = angUnion.bytes[2];
acData[7] = angUnion.bytes[3];
SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, acId, acData);
}
void CDriverMainDlg::OnBnClickedBtnSendMan() void CDriverMainDlg::OnBnClickedBtnSendMan()
@ -872,12 +978,12 @@ UINT CDriverMainDlg::SendCanThread(LPVOID v)
void CDriverMainDlg::OnBnClickedAutoSend() void CDriverMainDlg::OnBnClickedAutoSend()
{ {
// TODO: 在此添加控件通知处理程序代码 // TODO: 在此添加控件通知处理程序代码
UpdateData(TRUE); UpdateData(TRUE);
if (m_bAutoSend) if (m_bAutoSend)
{ {
m_StopSendFlag = 0; m_StopSendFlag = 0;
//开启接收线程 //开启接收线程
AfxBeginThread(SendCanThread, this); AfxBeginThread(SendCanThread, this);
} }
else else
@ -887,7 +993,7 @@ void CDriverMainDlg::OnBnClickedAutoSend()
void CDriverMainDlg::OnBnClickedCheckAutoRefreshShow() void CDriverMainDlg::OnBnClickedCheckAutoRefreshShow()
{ {
// TODO: 在此添加控件通知处理程序代码 // TODO: 在此添加控件通知处理程序代码
if (m_bAutoRefresh) if (m_bAutoRefresh)
{ {
m_pstVirtualList->StartAutoRefresh(10); m_pstVirtualList->StartAutoRefresh(10);
@ -899,7 +1005,7 @@ void CDriverMainDlg::OnBnClickedCheckAutoRefreshShow()
} }
void CDriverMainDlg::OnBnClickedBtnSendMan2() //电机使能 void CDriverMainDlg::OnBnClickedBtnSendMan2() //电机使能
{ {
/*UINT32 acId = 0x6000001; /*UINT32 acId = 0x6000001;
char acData[8] = { 0x23, 0x0D, 0x20, 0x01, 0x00, 0x00, 0x00, 0x00 }; char acData[8] = { 0x23, 0x0D, 0x20, 0x01, 0x00, 0x00, 0x00, 0x00 };
@ -907,33 +1013,30 @@ void CDriverMainDlg::OnBnClickedBtnSendMan2() //
} }
UINT CDriverMainDlg::SendCanThread2(LPVOID v) UINT CDriverMainDlg::SendCanThread2(LPVOID v)
{ {
CDriverMainDlg *dlg = (CDriverMainDlg*)v; CDriverMainDlg *dlg = (CDriverMainDlg*)v;
while (0 == dlg->m_StopSendFlag2) while (0 == dlg->m_StopSendFlag2)
{ {
// PID参数 // PID参数
float KP_Y = 0.005; // 横向偏差比例系数 float KP_Y = 0.005; // 横向偏差比例系数
float KI_Y = 0.01; // 积分项（消除静态误差） float KI_Y = 0.01; // 积分项（消除静态误差）
float KD_Y = 0.1; // 微分项（抑制振荡） float KD_Y = 0.1; // 微分项（抑制振荡）
float KP_THETA = 0.5; // 角度偏差系数 float KP_THETA = 0.5; // 角度偏差系数
// 死区滤波（忽略微小偏差） float DEADZONE_X = 100; // 2cm内不响应
float DEADZONE_X = 100; // 2cm内不响应 float DEADZONE_THETA = 0.5; // 0.02rad内不响应
float DEADZONE_THETA = 0.5; // 0.02rad内不响应
float Angle = 0; float Angle = 0;
float CrosswiseX = 0; float CrosswiseX = 0;
float UserAng=0;
float UserX=0;
Angle = theApp.m_fAngle; Angle = theApp.m_fAngle;
CrosswiseX = theApp.m_nX; CrosswiseX = theApp.m_nX;
float UserAng=0;
float UserX=0;
if (Angle > 1800) if (Angle > 1800)
Angle -= 3600; Angle -= 3600;
Angle /= 10; Angle /= 10;
if (abs(CrosswiseX) < DEADZONE_X) if (abs(CrosswiseX) < DEADZONE_X)
CrosswiseX = 0; CrosswiseX = 0;
if (abs(Angle) < DEADZONE_THETA) if (abs(Angle) < DEADZONE_THETA)
@ -941,8 +1044,10 @@ UINT CDriverMainDlg::SendCanThread2(LPVOID v)
theApp.m_fVelCal = 0.3; theApp.m_fVelCal = 0.3;
// 倒车时的控制逻辑
if (theApp.m_fVelCal < 0) if (theApp.m_fVelCal < 0)
{ {
//根据速度方向（前进/后退）和当前角度、横向偏差，计算出最终的转向角度控制量 theApp.m_fAngCal。
if (Angle > 0) if (Angle > 0)
{ {
UserAng = -Angle * KP_THETA; UserAng = -Angle * KP_THETA;
@ -980,6 +1085,7 @@ UINT CDriverMainDlg::SendCanThread2(LPVOID v)
} }
} }
} }
// 正向行驶时的控制逻辑
else if (theApp.m_fVelCal > 0) else if (theApp.m_fVelCal > 0)
{ {
if (Angle > 0) if (Angle > 0)
@ -1004,33 +1110,76 @@ UINT CDriverMainDlg::SendCanThread2(LPVOID v)
theApp.m_fAngCal = UserAng + UserX; //(-KP_Y * theApp.m_nX + KP_THETA * theApp.m_fAngle); theApp.m_fAngCal = UserAng + UserX; //(-KP_Y * theApp.m_nX + KP_THETA * theApp.m_fAngle);
//下发运动控制参数 //下发运动控制参数
dlg->SendCanControlData(theApp.m_fVelCal, theApp.m_fAngCal); dlg->SendCanControlData(theApp.m_fVelCal, theApp.m_fAngCal);
Sleep(10); Sleep(10);
} }
return 0; return 0;
} }
//author: caixiang
//总结
//• 该线程每 10ms 计算一次运动控制参数（速度、角度），并通过 CAN 总线实时下发，实现 AGV / 机器人自动循迹或导航控制。
//• 代码中包含了 PID 控制思想、死区滤波、前进 / 后退不同控制策略等，适合实际工程应用。
UINT CDriverMainDlg::SendCanThreadForFast(LPVOID v)
{
CDriverMainDlg* dlg = (CDriverMainDlg*)v;
while (0 == dlg->m_StopSendFlagForFast)
{
theApp.m_fVelCalForFast = 0.3f;
//因为theApp.m_fAngCalForFast 不是指针变量 所以不是判是否为nullptr的
if (std::isnan(theApp.m_fAngCalForFast)) {
// 尚未初始化
theApp.m_fAngCalForFast = 0.0f;
}
//theApp.m_fAngCalForFast = UserAng;
//
//下发运动控制参数
//dlg->SendCanControlDataFAST(theApp.m_fVelCalForFast, theApp.m_fAngCalForFast);
LogOutToFile("(inner)SendCanThreadForFast: Vel = %f, Ang = %f", theApp.m_fVelCalForFast, theApp.m_fAngCalForFast);
Sleep(10);
}
return 0;
}
void CDriverMainDlg::OnBnClickedAutoSend2() void CDriverMainDlg::OnBnClickedAutoSend2()
{ {
// TODO: 在此添加控件通知处理程序代码 // TODO: 在此添加控件通知处理程序代码
UpdateData(TRUE); UpdateData(TRUE);
if (m_bAutoSend2) if (m_bAutoSend2)
{ {
m_StopSendFlag2 = 0; m_StopSendFlag2 = 0;
//开启发送线程 //开启发送线程
AfxBeginThread(SendCanThread2, this); AfxBeginThread(SendCanThread2, this);
} }
else else
m_StopSendFlag2 = 1; m_StopSendFlag2 = 1;
} }
void CDriverMainDlg::OnBnClickedAutoSend3()
{
// TODO: 在此添加控件通知处理程序代码
UpdateData(TRUE);
if (m_bAutoSendForFast)
{
m_StopSendFlagForFast = 0;
//开启发送线程
AfxBeginThread(SendCanThreadForFast, this);
}
else
m_StopSendFlagForFast = 1;
}
int before_trafficstop_action; int before_trafficstop_action;
int before_safestop_action; int before_safestop_action;
void CDriverMainDlg::OnBnClickedButtonAgvstop() void CDriverMainDlg::OnBnClickedButtonAgvstop()
{ {
// TODO: 在此添加控件通知处理程序代码 // TODO: 在此添加控件通知处理程序代码
m_nAgvAction = A_STOP; m_nAgvAction = A_STOP;
before_trafficstop_action = -1; before_trafficstop_action = -1;
before_safestop_action = -1; before_safestop_action = -1;
@ -1039,7 +1188,7 @@ void CDriverMainDlg::OnBnClickedButtonAgvstop()
void CDriverMainDlg::OnBnClickedButtonAgvestop() void CDriverMainDlg::OnBnClickedButtonAgvestop()
{ {
// TODO: 在此添加控件通知处理程序代码 // TODO: 在此添加控件通知处理程序代码
m_nAgvAction = E_STOP; m_nAgvAction = E_STOP;
before_trafficstop_action = -1; before_trafficstop_action = -1;
before_safestop_action = -1; before_safestop_action = -1;
@ -1048,13 +1197,25 @@ void CDriverMainDlg::OnBnClickedButtonAgvestop()
void CDriverMainDlg::OnBnClickedButtonAgvforward() void CDriverMainDlg::OnBnClickedButtonAgvforward()
{ {
// TODO: 在此添加控件通知处理程序代码 // TODO: 在此添加控件通知处理程序代码
m_nAgvAction = FORWARD; m_nAgvAction = FORWARD;
} }
void CDriverMainDlg::OnBnClickedButtonAgvbackward() void CDriverMainDlg::OnBnClickedButtonAgvbackward()
{ {
// TODO: 在此添加控件通知处理程序代码 // TODO: 在此添加控件通知处理程序代码
m_nAgvAction = BACKWARD; m_nAgvAction = BACKWARD;
} }
void CDriverMainDlg::OnEnChangeEdit1()
{
// TODO: 如果该控件是 RICHEDIT 控件，它将不
// 发送此通知，除非重写 CDialogEx::OnInitDialog()
// 函数并调用 CRichEditCtrl().SetEventMask()，
// 同时将 ENM_CHANGE 标志“或”运算到掩码中。
// TODO: 在此添加控件通知处理程序代码
}

12
Plugin/Driver/DriverMainDlg.h
View File

@ -94,12 +94,15 @@ public:
BOOL m_bAutoRefresh; BOOL m_bAutoRefresh;
BOOL m_bAutoSend; BOOL m_bAutoSend;
BOOL m_bAutoSend2; BOOL m_bAutoSend2;
BOOL m_bAutoSendForFast;
CString m_strSendID; CString m_strSendID;
CString m_strSendData; CString m_strSendData;
BOOL m_StopSendFlag; BOOL m_StopSendFlag;
BOOL m_StopSendFlag2; BOOL m_StopSendFlag2;
BOOL m_StopSendFlagForFast;
void Drive_enable(); //Çý¶¯µç»úʹÄÜ void Drive_enable(); //Çý¶¯µç»úʹÄÜ
void Drive_disable(); //ʧÄÜ void Drive_disable(); //ʧÄÜ
@ -113,6 +116,8 @@ public:
void ReadConfigFromIni(); void ReadConfigFromIni();
void InitVirtualList(); void InitVirtualList();
BOOL OpenCanDevice(); BOOL OpenCanDevice();
float NormalizeAngle(float angle);
float CorrectAngle(float currentAngle, float targetAngle);
void UpdateCanStatue(BOOL bStatue); void UpdateCanStatue(BOOL bStatue);
void ProcessPipeMsg(int lMsgId, char* pData, int lLen); void ProcessPipeMsg(int lMsgId, char* pData, int lLen);
void SendCanData(int nDevIdx, int nCanIdx, int nFrameType, int nFrameFormat, UINT32 acFrameId, char acFrameData[8]); void SendCanData(int nDevIdx, int nCanIdx, int nFrameType, int nFrameFormat, UINT32 acFrameId, char acFrameData[8]);
@ -123,6 +128,8 @@ public:
static UINT ReceiveCanThread(LPVOID v); static UINT ReceiveCanThread(LPVOID v);
static UINT SendCanThread(LPVOID v); static UINT SendCanThread(LPVOID v);
static UINT SendCanThread2(LPVOID v); static UINT SendCanThread2(LPVOID v);
static UINT SendCanThreadForFast(LPVOID v);
void WriteVirtualList(CString strFrameId, CString strFrameData, int nTxRx = 0, int nCanIdx = 0, int nFrameType = 0, int nFrameFormat = 0); void WriteVirtualList(CString strFrameId, CString strFrameData, int nTxRx = 0, int nCanIdx = 0, int nFrameType = 0, int nFrameFormat = 0);
public: public:
@ -147,6 +154,9 @@ public:
void AnalysiseCanData(int nCanIndex, VCI_CAN_OBJ objCan); void AnalysiseCanData(int nCanIndex, VCI_CAN_OBJ objCan);
void CalculateAutoCanParam(); void CalculateAutoCanParam();
void SendCanControlData(float Vel, float Ang); void SendCanControlData(float Vel, float Ang);
void SendCanControlDataFAST(float Vel, float Ang);
void GetRoadwayInfo(int nReqPosId); void GetRoadwayInfo(int nReqPosId);
afx_msg void OnBnClickedButtonAgvstop(); afx_msg void OnBnClickedButtonAgvstop();
@ -156,4 +166,6 @@ public:
CPositionView m_PositionView; CPositionView m_PositionView;
CStatic Mil_info; CStatic Mil_info;
CStatic TAG; CStatic TAG;
afx_msg void OnEnChangeEdit1();
afx_msg void OnBnClickedAutoSend3();
}; };

5
Plugin/Driver/PluginDriver.cpp
View File

@ -58,6 +58,11 @@ BOOL CPluginDriver::InitInstance()
dir[i] = '\0'; dir[i] = '\0';
m_strModulePath = dir; m_strModulePath = dir;
//给fast 相关运动参数 初始化
m_fVelCalForFast = 0.0f;
m_fAngleForFast = 0.0f;
m_fAngCalForFast = 0.0f;
// 如果一个运行在 Windows XP 上的应用程序清单指定要 // 如果一个运行在 Windows XP 上的应用程序清单指定要

6
Plugin/Driver/PluginDriver.h
View File

@ -51,6 +51,12 @@ public:
float m_fAngCal; float m_fAngCal;
float m_fAngleForFast;
float m_fVelCalForFast;
float m_fAngCalForFast;
// 实现 // 实现
CString SendMsg2Platform(CString strReceiver, int nMsgType, Json::Value param = NULL); CString SendMsg2Platform(CString strReceiver, int nMsgType, Json::Value param = NULL);

75
Plugin/Driver/pid_controller.cpp Normal file
View File

@ -0,0 +1,75 @@
#include "pid_controller.h"
#include <cmath>
PIDController::PIDController(float proportionalGain, float integralGain, float derivativeGain,
float iLimitLow, float iLimitHigh, float oLimitLow, float oLimitHigh, float timeStep)
: kp(proportionalGain), ki(integralGain), kd(derivativeGain),
integralLimitLow(iLimitLow), integralLimitHigh(iLimitHigh),
outputLimitLow(oLimitLow), outputLimitHigh(oLimitHigh),
dt(timeStep), prevError(0.0f), integral(0.0f), prevMeasurement(0.0f), firstRun(true) {
}
void PIDController::reset() {
prevError = 0.0f;
integral = 0.0f;
prevMeasurement = 0.0f;
firstRun = true;
}
float PIDController::compute(float setpoint, float measurement) {
// 计算误差（期望角度 - 当前角度）
float error = setpoint - measurement;
// 比例项
float proportionalTerm = kp * error;
// 积分项（带抗积分饱和）
integral += error * dt;
// 积分限幅，防止积分饱和
if (integral > integralLimitHigh) {
integral = integralLimitHigh;
}
else if (integral < integralLimitLow) {
integral = integralLimitLow;
}
float integralTerm = ki * integral;
// 微分项（使用测量值的变化率而非误差变化率，以减少噪声影响）
float derivativeTerm = 0.0f;
if (!firstRun) {
// 使用测量值的变化率计算微分
float dMeasurement = measurement - prevMeasurement;
derivativeTerm = -kd * (dMeasurement / dt);
}
firstRun = false;
// 保存当前测量值用于下次计算
prevMeasurement = measurement;
// 计算总输出
float output = proportionalTerm + integralTerm + derivativeTerm;
// 输出限幅
if (output > outputLimitHigh) {
output = outputLimitHigh;
}
else if (output < outputLimitLow) {
output = outputLimitLow;
}
return output;
}
void PIDController::getTunings(float& p, float& i, float& d) const {
p = kp;
i = ki;
d = kd;
}
void PIDController::setTunings(float p, float i, float d) {
kp = p;
ki = i;
kd = d;
}

44
Plugin/Driver/pid_controller.h Normal file
View File

@ -0,0 +1,44 @@
#ifndef PID_CONTROLLER_H
#define PID_CONTROLLER_H
class PIDController {
private:
// PID参数
float kp; // 比例系数
float ki; // 积分系数
float kd; // 微分系数
// 积分项和输出限制
float integralLimitLow;
float integralLimitHigh;
float outputLimitLow;
float outputLimitHigh;
// 状态变量
float prevError; // 上一次误差
float integral; // 积分项
float prevMeasurement; // 上一次测量值
// 时间相关
float dt; // 时间步长
bool firstRun; // 是否首次运行
public:
// 构造函数，初始化PID参数和限制
PIDController(float proportionalGain, float integralGain, float derivativeGain,
float iLimitLow, float iLimitHigh, float oLimitLow, float oLimitHigh, float timeStep);
// 重置控制器状态
void reset();
// 计算PID输出
float compute(float setpoint, float measurement);
// 获取当前PID参数
void getTunings(float& p, float& i, float& d) const;
// 设置PID参数
void setTunings(float p, float i, float d);
};
#endif // PID_CONTROLLER_H

BIN
Plugin/Driver/resource.h
View File

Binary file not shown.

2
Plugin/Protocol.h
View File

@ -32,6 +32,8 @@ typedef enum
GUIDE_QRCODE = 21, //QR插件往Driver发送识别的二维码信息 GUIDE_QRCODE = 21, //QR插件往Driver发送识别的二维码信息
GUIDE_FAST = 22, //FAST插件往Driver发送 视觉识别到的偏移角度信息
}EM_MSG_TYPE; }EM_MSG_TYPE;

4
Plugin/QrGuide/QrMainDialog.cpp
View File

@ -86,7 +86,6 @@ void CQrMainDialog::AddLog2Edit(CString strMsg)
str += "\r\n"; str += "\r\n";
m_EditMultiLine.SetWindowText(str); m_EditMultiLine.SetWindowText(str);
m_EditMultiLine.LineScroll(m_EditMultiLine.GetLineCount()); m_EditMultiLine.LineScroll(m_EditMultiLine.GetLineCount());
} }
@ -142,9 +141,8 @@ void CQrMainDialog::ProcessPipeMsg(int lMsgId, char* pData, int lLen)
BOOL CQrMainDialog::OnInitDialog() BOOL CQrMainDialog::OnInitDialog()
{ {
CDialogEx::OnInitDialog(); CDialogEx::OnInitDialog();
CString strPipe; CString strPipe;
for (int i = 0; i < __argc; i++) for (int i = 0; i < __argc; i++)
{ {
if (0 == strcmp(__argv[i], "-pipe") && i + 1 < __argc) if (0 == strcmp(__argv[i], "-pipe") && i + 1 < __argc)