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>
<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>
<RuntimeLibrary>MultiThreadedDebugDLL</RuntimeLibrary>
</ClCompile>
<Link>
<SubSystem>Windows</SubSystem>
@ -202,6 +203,7 @@
<ClInclude Include="ColoredListCtrl.h" />
<ClInclude Include="DriverMainDlg.h" />
<ClInclude Include="Map.h" />
<ClInclude Include="pid_controller.h" />
<ClInclude Include="PluginDriver.h" />
<ClInclude Include="PositionView.h" />
<ClInclude Include="Resource.h" />
@ -216,6 +218,7 @@
<ClCompile Include="CEXVirtualListCtrl.cpp" />
<ClCompile Include="ColoredListCtrl.cpp" />
<ClCompile Include="DriverMainDlg.cpp" />
<ClCompile Include="pid_controller.cpp" />
<ClCompile Include="PluginDriver.cpp" />
<ClCompile Include="PositionView.cpp" />
<ClCompile Include="stdafx.cpp">

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

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

365
Plugin/Driver/DriverMainDlg.cpp
View File

@ -1,4 +1,4 @@
// CanDeviceDlg.cpp : 实现文件
// CanDeviceDlg.cpp : 实现文件
//
#include "stdafx.h"
@ -6,14 +6,18 @@
#include "resource.h"
#include "afxdialogex.h"
#include "PluginDriver.h"
#include "pid_controller.h"
#include <iostream>
#include <cmath>
int StopFlag = 0;
unsigned long nextrow;
#define TIMER_UPDATE_POS (19999)
// CCanDeviceDlg 对话框
//控件ID只需要在当前窗口中唯一，这里随便写一个就好
// CCanDeviceDlg 对话框
//控件ID只需要在当前窗口中唯一，这里随便写一个就好
#define ID_VIRTUAL_LIST_CTRL (20000)
#define TIMER_ID_ADD_TEST_DATA (2024)
IMPLEMENT_DYNAMIC(CDriverMainDlg, CDialogEx)
@ -26,11 +30,13 @@ CDriverMainDlg::CDriverMainDlg(CWnd* pParent /*=NULL*/)
m_bCanRxEn = TRUE;
m_bAutoRefresh = TRUE;
m_bAutoSend2 = TRUE;
m_bAutoSendForFast= TRUE;
m_strSendID = "00 00 00 88";
m_strSendData = "01 02 03 04 05 06 07 08";
//m_pMainWnd = (CPluginMainDialog*) pParent;
m_StopSendFlag = 0;
m_StopSendFlag2 = 0;
m_StopSendFlagForFast = 0;
m_DevType = VCI_USBCAN2;
m_nAgvAction = 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_AUTO_SEND, m_bAutoSend);
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_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_AGVBACKWARD, &CDriverMainDlg::OnBnClickedButtonAgvbackward)
ON_WM_TIMER()
ON_EN_CHANGE(IDC_EDIT1, &CDriverMainDlg::OnEnChangeEdit1)
ON_BN_CLICKED(IDC_AUTO_SEND3, &CDriverMainDlg::OnBnClickedAutoSend3)
END_MESSAGE_MAP()
//一位十六进制转换为十进制
//一位十六进制转换为十进制
int CDriverMainDlg::HexChar(char c)
{
if ((c >= '0') && (c <= '9'))
@ -94,7 +103,7 @@ int CDriverMainDlg::HexChar(char c)
return 0x10;
}
//两位十六进制数转换为十进制
//两位十六进制数转换为十进制
int CDriverMainDlg::Str2Hex(CString str)
{
int len = str.GetLength();
@ -123,7 +132,7 @@ int CDriverMainDlg::Str2Hex(CString str)
void CDriverMainDlg::InitVirtualList()
{
//信息显示列表初始化
//信息显示列表初始化
m_pstVirtualList->InsertColumn(0, "Seq");
m_pstVirtualList->SetColumnWidth(0, 110);
m_pstVirtualList->InsertColumn(1, "Time");
@ -153,7 +162,7 @@ void CDriverMainDlg::UpdatePositionView()
InvalidateRect(rect);
}
//更新车辆位置，nPosition记录从巷道口的里程，从0开始计算
//更新车辆位置，nPosition记录从巷道口的里程，从0开始计算
void CDriverMainDlg::UpdateVehiclePosition(int nPosition)
{
m_PositionView.m_nPosition = nPosition;
@ -162,18 +171,18 @@ void CDriverMainDlg::UpdateVehiclePosition(int nPosition)
void CDriverMainDlg::OnTimer(UINT_PTR nIDEvent)
{
// TODO: 在此添加消息处理程序代码和/或调用默认值
// TODO: 在此添加消息处理程序代码和/或调用默认值
static int n = 0;
if (TIMER_UPDATE_POS == nIDEvent)
{
//定时刷新车辆位置
//定时刷新车辆位置
m_PositionView.m_nPosition = n++ * 600;
UpdatePositionView();
}
else
{
//自定义定时器，无需调用基类OnTimer，否则定时器会被强制结束
//自定义定时器，无需调用基类OnTimer，否则定时器会被强制结束
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)
{
//数据按行格式化
//数据按行格式化
char acTemp[DATA_LINE_SIZE + 1] = { 0 };
//发送信息列表显示
//发送信息列表显示
CString strTime;
SYSTEMTIME systime;
GetLocalTime(&systime);
char acId[11] = { 0 };
memset(acId, 32, 10); //初始化为空格字符串
memset(acId, 32, 10); //初始化为空格字符串
memcpy(acId, strFrameId.GetBuffer(), strFrameId.GetLength());
char acData[33] = { 0 };
memset(acData, 32, 32); //初始化为空格字符串
memset(acData, 32, 32); //初始化为空格字符串
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(),
@ -208,31 +217,31 @@ void CDriverMainDlg::WriteVirtualList(CString strFrameId, CString strFrameData,
8,
acData);
//调用接口写入数据
//调用接口写入数据
m_pQueue->WriteItem(acTemp);
}
//和模块进程通讯的管道回调函数：
//和模块进程通讯的管道回调函数：
void g_PipeCallBack(void* pObj, int lMsgId, WPARAM wparam, LPARAM lparam)
{
CDriverMainDlg* pModule = (CDriverMainDlg*)pObj;
if (CEXPIPE_CONNECT_OK == 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);
}
else if (CEXPIPE_DIS_CLIENT == lMsgId)
{
//管道断开，结束
//管道断开，结束
#ifndef _DEBUG
LogOutToFile("父进程连接管道断开，本模块自动退出");
LogOutToFile("父进程连接管道断开，本模块自动退出");
//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);
#endif
@ -258,6 +267,8 @@ void CDriverMainDlg::GetRoadwayInfo(int nReqPosId)
theApp.SendMsg2Platform("WMS", ROADWAY_CONFIG_REQ, param);
}
//处理 平台各个模块 传过来的消息
void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen)
{
if (lLen == 0)
@ -267,7 +278,7 @@ void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen)
PIPE_DATA_STRUCT* pstData = (PIPE_DATA_STRUCT*)pData;
//平台转发给插件的消息
//平台转发给插件的消息
if (pstData->lMsgId == MAIN_2_MODULE_WMS && pstData->lDataLen > 0)
{
Json::Reader reader;
@ -279,13 +290,13 @@ void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen)
int nMsgType = root["type"].asInt();
//从科聪控制器收到切换导航指令，向wms请求获取通道信息，包括标签等
//从科聪控制器收到切换导航指令，向wms请求获取通道信息，包括标签等
if (nMsgType == CHANGE_GUIDE_TYPE)
{
AfxMessageBox("AGV到达请求点，切换导航方式");
AfxMessageBox("AGV到达请求点，切换导航方式");
GetRoadwayInfo(0);
}
//从wms获取巷道信息返回
//从wms获取巷道信息返回
else if (nMsgType == ROADWAY_CONFIG_RET)
{
Json::Value data = root["params"];
@ -306,7 +317,7 @@ void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen)
UpdatePositionView();
}
//从QR插件收到二维码信息
//从QR插件收到二维码信息
else if (nMsgType == GUIDE_QRCODE)
{
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)
{
}
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");
}
// 角度规范化函数，将角度限制在-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()
{
@ -364,7 +436,7 @@ BOOL CDriverMainDlg::OnInitDialog()
CString iniPath = theApp.m_strModulePath.Left(theApp.m_strModulePath.ReverseFind('\\') + 1);
//m_strModulePath = theApp.m_strModulePath.ReverseFind("\\");
iniPath = iniPath + "pipe.ini";
//中文名称，仅显示用
//中文名称，仅显示用
GetPrivateProfileString("AGV-MODULE", "PIPE_DRIVER", "", acPipe, 256, iniPath);
strPipe = acPipe;
}
@ -381,7 +453,7 @@ BOOL CDriverMainDlg::OnInitDialog()
TRACE0("Failed to create MyVirtualListCtrl\n");
delete m_pstVirtualList;
m_pstVirtualList = nullptr;
return FALSE; // 返回 FALSE 以使框架知道未成功初始化
return FALSE; // 返回 FALSE 以使框架知道未成功初始化
}
InitVirtualList();
@ -394,7 +466,7 @@ BOOL CDriverMainDlg::OnInitDialog()
m_pQueue = new CEXMemFileQueue(m_strDataDir.GetString());
m_pstVirtualList->SetData(m_pQueue);
//m_pstVirtualList->StartAutoRefresh(10);//列表10毫秒刷新一次
//m_pstVirtualList->StartAutoRefresh(10);//列表10毫秒刷新一次
CDialogEx::OnInitDialog();
@ -410,7 +482,7 @@ BOOL CDriverMainDlg::OnInitDialog()
if (m_bCanRxEn)
{
StopFlag = 0;
//开启接收线程
//开启接收线程
AfxBeginThread(ReceiveCanThread, this);
}
else
@ -419,9 +491,16 @@ BOOL CDriverMainDlg::OnInitDialog()
if (m_bAutoSend2)
{
m_StopSendFlag2 = 0;
//开启发送线程
//开启发送线程
AfxBeginThread(SendCanThread2, this);
}
if (m_bAutoSendForFast)
{
m_StopSendFlagForFast = 0;
//开启发送线程
AfxBeginThread(SendCanThreadForFast, this);
}
else
m_StopSendFlag2 = 1;
@ -436,12 +515,12 @@ BOOL CDriverMainDlg::OnInitDialog()
}
//打开设备
//打开设备
BOOL CDriverMainDlg::OpenCanDevice()
{
DWORD Reserved = 0;
//打开设备
//打开设备
if (VCI_OpenDevice(VCI_USBCAN2, 0, Reserved) != 1)
{
//MessageBox("open failed");
@ -455,7 +534,7 @@ BOOL CDriverMainDlg::OpenCanDevice()
InitInfo->AccCode = 0x80000000;
InitInfo->AccMask = 0xFFFFFFFF;
InitInfo->Mode = 0; //Data ;Remote;
//初始化通道0
//初始化通道0
if (VCI_InitCAN(VCI_USBCAN2, 0, 0, InitInfo) != 1) //can-0
{
//MessageBox("Init-CAN failed!");
@ -463,14 +542,14 @@ BOOL CDriverMainDlg::OpenCanDevice()
return FALSE;
}
Sleep(100);
//初始化通道0
//初始化通道0
if (VCI_StartCAN(VCI_USBCAN2, 0, 0) != 1) //can-0
{
//MessageBox("Start-CAN failed!");
//m_pMainWnd->InsertLog(LOG_ERROR, "Start-CAN-Index0 Failed!");
return FALSE;
}
//初始化通道1
//初始化通道1
//if (m_nDevType == 1)
{
if (VCI_InitCAN(VCI_USBCAN2, 0, 1, InitInfo) != 1) //can-1
@ -483,7 +562,7 @@ BOOL CDriverMainDlg::OpenCanDevice()
Sleep(100);
InitInfo->Mode = 0; //Data ;Remote;
//初始化通道1
//初始化通道1
if (VCI_StartCAN(VCI_USBCAN2, 0, 1) != 1) //can-0
{
//MessageBox("Start-CAN failed!");
@ -499,22 +578,22 @@ BOOL CDriverMainDlg::OpenCanDevice()
}
void CDriverMainDlg::UpdateCanStatue(BOOL bStatue)
void CDriverMainDlg::UpdateCanStatue(BOOL bStatue)
{
if (TRUE == bStatue)
{
GetDlgItem(IDC_STATUS_CAN)->SetWindowText("已连接");
GetDlgItem(IDC_STATUS_CAN)->SetWindowText("已连接");
}
else
{
GetDlgItem(IDC_STATUS_CAN)->SetWindowText("已关闭");
GetDlgItem(IDC_STATUS_CAN)->SetWindowText("已关闭");
}
}
void CDriverMainDlg::ReadConfigFromIni()
{
CString strIniPath = theApp.m_strModulePath + "\\config.ini";
// 初始化设备配置
// 初始化设备配置
m_nSendFrameFormatIdx0 = GetPrivateProfileInt("CAN", "FRAME_FORMAT_IDX0", 0, strIniPath);
m_nSendFrameTypeIdx0 = GetPrivateProfileInt("CAN", "FRAME_TYPE_IDX0", 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
// 异常: OCX 属性页应返回 FALSE
// 异常: OCX 属性页应返回 FALSE
}
//void CDriverMainDlg::Drive_enable() //使能
//void CDriverMainDlg::Drive_enable() //使能
//{
// char can2_buff[8] = { 0 };
// 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, 0x600002, can2_buff);
//}
//void CDriverMainDlg::Drive_disable() //失能
//void CDriverMainDlg::Drive_disable() //失能
//{
// char can2_buff[8] = { 0 };
// can2_buff[0] = 0x23;
@ -560,15 +639,16 @@ void CDriverMainDlg::ReadConfigFromIni()
/*
param0: can设备索引
param1: can口索引
param0: can设备索引
param1: can口索引
param2: standard/extend
param3: data/remote
received can msg
*/
void CDriverMainDlg::SendCanData(int nDevIdx, int nCanIdx, int nFrameType, int nFrameFormat, UINT32 acFrameId, char acFrameData[8])
{
//从界面获取发送信息
//从界面获取发送信息
VCI_CAN_OBJ sendbuf[1];
int datanum = 8;
@ -583,18 +663,18 @@ void CDriverMainDlg::SendCanData(int nDevIdx, int nCanIdx, int nFrameType, int n
sendbuf->Data[i] = acFrameData[i];
}*/
/****************************************************************************/
/******************************从界面获取发送信息完毕***********************/
/******************************从界面获取发送信息完毕***********************/
/****************************************************************************/
static bool snederr_flag = false;
//调用动态链接库发送函数
//调用动态链接库发送函数
int flag = VCI_Transmit(m_DevType, m_DevIndex, nCanIdx, sendbuf, 1);//CAN message send
if (flag<1)
{
//VCI_CloseDevice(m_DevType, m_DevIndex);
if (flag == -1)
{
//Can设备已断开
//Can设备已断开
//MessageBox("failed- device not open\n");
//m_pMainWnd->InsertLog(LOG_ERROR, "Failed- Device Not Open!");
OpenCanDevice();
@ -638,7 +718,7 @@ void CDriverMainDlg::OnCheckCanrxEn()
if (m_bCanRxEn)
{
StopFlag = 0;
//开启接收线程
//开启接收线程
AfxBeginThread(ReceiveCanThread, this);
}
else
@ -657,11 +737,11 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
for (int kCanIndex = 0; kCanIndex<2; kCanIndex++)
{
//调用动态链接看接收函数
//调用动态链接看接收函数
int NumValue = VCI_Receive(pThis->m_DevType, pThis->m_DevIndex, kCanIndex, pCanObj, 200, 0);
if (NumValue >0)
{
//接收信息列表显示
//接收信息列表显示
CString strTime;
SYSTEMTIME systime;
GetLocalTime(&systime);
@ -673,7 +753,7 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
CString strFrameId, strFrameData, strTmp;
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]);
strFrameData += strTmp;
@ -681,9 +761,9 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
pThis->WriteVirtualList(strFrameId, strFrameData, 1, kCanIndex);
//在这里处理收到的CAN消息
//在这里处理收到的CAN消息
//解析定位引导数据
//解析定位引导数据
pThis->AnalysiseCanData(kCanIndex, pCanObj[num]);
/*if (MANUAL_MODE ==pThis->m_pMainWnd->m_nAgvMode)
@ -692,7 +772,7 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
}
else
{
//计算运动控制参数(自动模式),需要考虑m_nAgvAction值
//计算运动控制参数(自动模式),需要考虑m_nAgvAction值
if (Control_Mode_Switch)
{
Control_Mode_Switch = 0;
@ -706,7 +786,7 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
}
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->OpenCanDevice();
}
@ -723,7 +803,7 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
}
//解析接收到的CAN数据
//解析接收到的CAN数据
void CDriverMainDlg::AnalysiseCanData(int nCanIndex, VCI_CAN_OBJ objCan)
{
CString nEdit;
@ -731,13 +811,13 @@ void CDriverMainDlg::AnalysiseCanData(int nCanIndex, VCI_CAN_OBJ objCan)
{
if (objCan.ID == 0x5B1)
{
//里程数据
//里程数据
m_stSensorData.Mile_info = (float *)objCan.Data;
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);
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);
}
}
@ -772,19 +852,19 @@ void CDriverMainDlg::CalculateAutoCanParam()
}
}
int CDriverMainDlg::Auto_Forward(float speedpar) //去载货区 进入雷达减速区通过速度系数调整速度
int CDriverMainDlg::Auto_Forward(float speedpar) //去载货区 进入雷达减速区通过速度系数调整速度
{
int Position_P = 4000; //转向比例
if (m_stSensorData.FMagnetism_Valid == 1) //磁条数据有效
int Position_P = 4000; //转向比例
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.Drive_Speed = 3000 * speedpar;
if (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)
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.Drive_Speed = 0;
@ -792,26 +872,26 @@ int CDriverMainDlg::Auto_Forward(float speedpar) //ȥ
}
return RE_NORMAL_RUN;
}
else //磁条无效异常停车
else //磁条无效异常停车
{
m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle;
m_stDrvierControl.Drive_Speed = 0;
return RE_UNUSUAL_STOP;
}
}
int CDriverMainDlg::Auto_Backward(float speedpar) //去卸货区
int CDriverMainDlg::Auto_Backward(float speedpar) //去卸货区
{
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.Drive_Speed = -2000 * speedpar;
if (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)
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.Drive_Speed = 0;
@ -831,7 +911,7 @@ int Speed = 0;
int LastSpeed = 0;
void CDriverMainDlg::SendCanControlData(float Vel, float Ang)
{
UINT32 acId = 0x5A1; //速度 角度帧
UINT32 acId = 0x5A1; //速度 角度帧
char acData[8] = {};
acData[0] = *((char*)&theApp.m_fVelCal + 0);
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);
}
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()
@ -872,12 +978,12 @@ UINT CDriverMainDlg::SendCanThread(LPVOID v)
void CDriverMainDlg::OnBnClickedAutoSend()
{
// TODO: 在此添加控件通知处理程序代码
// TODO: 在此添加控件通知处理程序代码
UpdateData(TRUE);
if (m_bAutoSend)
{
m_StopSendFlag = 0;
//开启接收线程
//开启接收线程
AfxBeginThread(SendCanThread, this);
}
else
@ -887,7 +993,7 @@ void CDriverMainDlg::OnBnClickedAutoSend()
void CDriverMainDlg::OnBnClickedCheckAutoRefreshShow()
{
// TODO: 在此添加控件通知处理程序代码
// TODO: 在此添加控件通知处理程序代码
if (m_bAutoRefresh)
{
m_pstVirtualList->StartAutoRefresh(10);
@ -899,7 +1005,7 @@ void CDriverMainDlg::OnBnClickedCheckAutoRefreshShow()
}
void CDriverMainDlg::OnBnClickedBtnSendMan2() //电机使能
void CDriverMainDlg::OnBnClickedBtnSendMan2() //电机使能
{
/*UINT32 acId = 0x6000001;
char acData[8] = { 0x23, 0x0D, 0x20, 0x01, 0x00, 0x00, 0x00, 0x00 };
@ -907,42 +1013,41 @@ void CDriverMainDlg::OnBnClickedBtnSendMan2() //
}
UINT CDriverMainDlg::SendCanThread2(LPVOID v)
{
CDriverMainDlg *dlg = (CDriverMainDlg*)v;
while (0 == dlg->m_StopSendFlag2)
{
// PID参数
float KP_Y = 0.005; // 横向偏差比例系数
float KI_Y = 0.01; // 积分项（消除静态误差）
float KD_Y = 0.1; // 微分项（抑制振荡）
float KP_THETA = 0.5; // 角度偏差系数
// 死区滤波（忽略微小偏差）
float DEADZONE_X = 100; // 2cm内不响应
float DEADZONE_THETA = 0.5; // 0.02rad内不响应
// PID参数
float KP_Y = 0.005; // 横向偏差比例系数
float KI_Y = 0.01; // 积分项（消除静态误差）
float KD_Y = 0.1; // 微分项（抑制振荡）
float KP_THETA = 0.5; // 角度偏差系数
float DEADZONE_X = 100; // 2cm内不响应
float DEADZONE_THETA = 0.5; // 0.02rad内不响应
float Angle = 0;
float CrosswiseX = 0;
float UserAng=0;
float UserX=0;
Angle = theApp.m_fAngle;
CrosswiseX = theApp.m_nX;
float UserAng=0;
float UserX=0;
if (Angle > 1800)
Angle -= 3600;
Angle /= 10;
if (abs(CrosswiseX) < DEADZONE_X)
CrosswiseX = 0;
if (abs(Angle) < DEADZONE_THETA)
Angle = 0;
theApp.m_fVelCal = 0.3;
// 倒车时的控制逻辑
if (theApp.m_fVelCal < 0)
{
//根据速度方向（前进/后退）和当前角度、横向偏差，计算出最终的转向角度控制量 theApp.m_fAngCal。
if (Angle > 0)
{
UserAng = -Angle * KP_THETA;
@ -980,6 +1085,7 @@ UINT CDriverMainDlg::SendCanThread2(LPVOID v)
}
}
}
// 正向行驶时的控制逻辑
else if (theApp.m_fVelCal > 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);
//下发运动控制参数
//下发运动控制参数
dlg->SendCanControlData(theApp.m_fVelCal, theApp.m_fAngCal);
Sleep(10);
}
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()
{
// TODO: 在此添加控件通知处理程序代码
// TODO: 在此添加控件通知处理程序代码
UpdateData(TRUE);
if (m_bAutoSend2)
{
m_StopSendFlag2 = 0;
//开启发送线程
//开启发送线程
AfxBeginThread(SendCanThread2, this);
}
else
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_safestop_action;
void CDriverMainDlg::OnBnClickedButtonAgvstop()
{
// TODO: 在此添加控件通知处理程序代码
// TODO: 在此添加控件通知处理程序代码
m_nAgvAction = A_STOP;
before_trafficstop_action = -1;
before_safestop_action = -1;
@ -1039,7 +1188,7 @@ void CDriverMainDlg::OnBnClickedButtonAgvstop()
void CDriverMainDlg::OnBnClickedButtonAgvestop()
{
// TODO: 在此添加控件通知处理程序代码
// TODO: 在此添加控件通知处理程序代码
m_nAgvAction = E_STOP;
before_trafficstop_action = -1;
before_safestop_action = -1;
@ -1048,13 +1197,25 @@ void CDriverMainDlg::OnBnClickedButtonAgvestop()
void CDriverMainDlg::OnBnClickedButtonAgvforward()
{
// TODO: 在此添加控件通知处理程序代码
// TODO: 在此添加控件通知处理程序代码
m_nAgvAction = FORWARD;
}
void CDriverMainDlg::OnBnClickedButtonAgvbackward()
{
// TODO: 在此添加控件通知处理程序代码
// TODO: 在此添加控件通知处理程序代码
m_nAgvAction = BACKWARD;
}
void CDriverMainDlg::OnEnChangeEdit1()
{
// TODO: 如果该控件是 RICHEDIT 控件，它将不
// 发送此通知，除非重写 CDialogEx::OnInitDialog()
// 函数并调用 CRichEditCtrl().SetEventMask()，
// 同时将 ENM_CHANGE 标志“或”运算到掩码中。
// TODO: 在此添加控件通知处理程序代码
}

14
Plugin/Driver/DriverMainDlg.h
View File

@ -94,12 +94,15 @@ public:
BOOL m_bAutoRefresh;
BOOL m_bAutoSend;
BOOL m_bAutoSend2;
BOOL m_bAutoSendForFast;
CString m_strSendID;
CString m_strSendData;
BOOL m_StopSendFlag;
BOOL m_StopSendFlag2;
BOOL m_StopSendFlagForFast;
void Drive_enable(); //Çý¶¯µç»úʹÄÜ
void Drive_disable(); //ʧÄÜ
@ -113,6 +116,8 @@ public:
void ReadConfigFromIni();
void InitVirtualList();
BOOL OpenCanDevice();
float NormalizeAngle(float angle);
float CorrectAngle(float currentAngle, float targetAngle);
void UpdateCanStatue(BOOL bStatue);
void ProcessPipeMsg(int lMsgId, char* pData, int lLen);
void SendCanData(int nDevIdx, int nCanIdx, int nFrameType, int nFrameFormat, UINT32 acFrameId, char acFrameData[8]);
@ -122,7 +127,9 @@ public:
virtual BOOL OnInitDialog();
static UINT ReceiveCanThread(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);
public:
@ -147,6 +154,9 @@ public:
void AnalysiseCanData(int nCanIndex, VCI_CAN_OBJ objCan);
void CalculateAutoCanParam();
void SendCanControlData(float Vel, float Ang);
void SendCanControlDataFAST(float Vel, float Ang);
void GetRoadwayInfo(int nReqPosId);
afx_msg void OnBnClickedButtonAgvstop();
@ -156,4 +166,6 @@ public:
CPositionView m_PositionView;
CStatic Mil_info;
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';
m_strModulePath = dir;
//给fast 相关运动参数 初始化
m_fVelCalForFast = 0.0f;
m_fAngleForFast = 0.0f;
m_fAngCalForFast = 0.0f;
// 如果一个运行在 Windows XP 上的应用程序清单指定要

6
Plugin/Driver/PluginDriver.h
View File

@ -51,6 +51,12 @@ public:
float m_fAngCal;
float m_fAngleForFast;
float m_fVelCalForFast;
float m_fAngCalForFast;
// 实现
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_FAST = 22, //FAST插件往Driver发送 视觉识别到的偏移角度信息
}EM_MSG_TYPE;

4
Plugin/QrGuide/QrMainDialog.cpp
View File

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