Merge branch 'master' of http://git.picaiba.com/agv/agv-control

2025-06-16 13:31:34 +08:00 · 2025-06-16 13:31:34 +08:00 · 6733dfe597
commit 6733dfe597
parent 7e13599f5c 4be62027c6
12 changed files with 414 additions and 106 deletions
--- a/Plugin/Driver/Driver.rc
+++ b/Plugin/Driver/Driver.rc
--- a/Plugin/Driver/Driver.vcxproj
+++ b/Plugin/Driver/Driver.vcxproj
@ -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">
--- a/Plugin/Driver/Driver.vcxproj.filters
+++ b/Plugin/Driver/Driver.vcxproj.filters
@ -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" />
--- a/Plugin/Driver/DriverMainDlg.cpp
+++ b/Plugin/Driver/DriverMainDlg.cpp
@ -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:  在此添加控件通知处理程序代码
+}
+
+
--- a/Plugin/Driver/DriverMainDlg.h
+++ b/Plugin/Driver/DriverMainDlg.h
@ -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();
 };
--- a/Plugin/Driver/PluginDriver.cpp
+++ b/Plugin/Driver/PluginDriver.cpp
@ -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 上的应用程序清单指定要
--- a/Plugin/Driver/PluginDriver.h
+++ b/Plugin/Driver/PluginDriver.h
@ -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);

--- a/Plugin/Driver/pid_controller.cpp
+++ b/Plugin/Driver/pid_controller.cpp
@ -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;
+}
--- a/Plugin/Driver/pid_controller.h
+++ b/Plugin/Driver/pid_controller.h
@ -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
--- a/Plugin/Driver/resource.h
+++ b/Plugin/Driver/resource.h
--- a/Plugin/Protocol.h
+++ b/Plugin/Protocol.h
@ -32,6 +32,8 @@ typedef enum

 	GUIDE_QRCODE         = 21,       //QR插件往Driver发送识别的二维码信息

+	GUIDE_FAST           = 22,       //FAST插件往Driver发送 视觉识别到的偏移角度信息
+

 }EM_MSG_TYPE;

--- a/Plugin/QrGuide/QrMainDialog.cpp
+++ b/Plugin/QrGuide/QrMainDialog.cpp
@ -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)