From 4be62027c6f0132df3320432707dd44d6fb0c2b3 Mon Sep 17 00:00:00 2001 From: CaiXiang <939387484@qq.com> Date: Mon, 16 Jun 2025 11:16:20 +0800 Subject: [PATCH] =?UTF-8?q?=E5=BC=95=E5=85=A5PID=E6=8E=A7=E5=88=B6?= =?UTF-8?q?=E5=99=A8=E5=B9=B6=E6=9B=B4=E6=96=B0=E7=9B=B8=E5=85=B3=E5=8A=9F?= =?UTF-8?q?=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 在多个文件中进行了重要更改： - 更新 `Driver.rc` 和 `resource.h` 的二进制文件。 - 在 `Driver.vcxproj` 中添加了多线程调试DLL的运行库设置。 - 引入 `pid_controller.h` 和 `pid_controller.cpp`，实现PID控制器功能。 - 在 `DriverMainDlg.cpp` 中添加了对PID控制的支持，包括新成员变量和方法。 - 增加了自动发送和车辆位置更新的功能。 - 在 `Protocol.h` 中添加了新的消息类型 `GUIDE_FAST`。 --- Plugin/Driver/Driver.rc | Bin 25824 -> 26060 bytes Plugin/Driver/Driver.vcxproj | 3 + Plugin/Driver/Driver.vcxproj.filters | 2 + Plugin/Driver/DriverMainDlg.cpp | 365 +++++++++++++++++++-------- Plugin/Driver/DriverMainDlg.h | 14 +- Plugin/Driver/PluginDriver.cpp | 5 + Plugin/Driver/PluginDriver.h | 6 + Plugin/Driver/pid_controller.cpp | 75 ++++++ Plugin/Driver/pid_controller.h | 44 ++++ Plugin/Driver/resource.h | Bin 18022 -> 18114 bytes Plugin/Protocol.h | 2 + Plugin/QrGuide/QrMainDialog.cpp | 4 +- 12 files changed, 414 insertions(+), 106 deletions(-) create mode 100644 Plugin/Driver/pid_controller.cpp create mode 100644 Plugin/Driver/pid_controller.h diff --git a/Plugin/Driver/Driver.rc b/Plugin/Driver/Driver.rc index 2af3a36f4baad6e6182a61f01b6f1eae2e46af7e..d4199211eff19bd61165cc1994d901730103e67d 100644 GIT binary patch delta 61 zcmV-D0K)&^$pOsE0kA?3lZaBVv+fU2G?T1D2$NnIB9jh1Fq2{-B$K`@ED16IIRG*M TEC4l=Z8tTO7G1ux{6yp*_WINDOWS;_DEBUG;%(PreprocessorDefinitions) false $(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) + MultiThreadedDebugDLL Windows @@ -202,6 +203,7 @@ + @@ -216,6 +218,7 @@ + diff --git a/Plugin/Driver/Driver.vcxproj.filters b/Plugin/Driver/Driver.vcxproj.filters index 5275caf..b2437bf 100644 --- a/Plugin/Driver/Driver.vcxproj.filters +++ b/Plugin/Driver/Driver.vcxproj.filters @@ -17,6 +17,7 @@ + @@ -31,6 +32,7 @@ + diff --git a/Plugin/Driver/DriverMainDlg.cpp b/Plugin/Driver/DriverMainDlg.cpp index f1f7bca..99a649d 100644 --- 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 +#include + 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(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_CloseDeviceVCI_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: 在此添加控件通知处理程序代码 +} + + diff --git a/Plugin/Driver/DriverMainDlg.h b/Plugin/Driver/DriverMainDlg.h index 6e22816..df45126 100644 --- 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(); }; diff --git a/Plugin/Driver/PluginDriver.cpp b/Plugin/Driver/PluginDriver.cpp index a42ad09..9c708f4 100644 --- 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 ϵӦó嵥ָҪ diff --git a/Plugin/Driver/PluginDriver.h b/Plugin/Driver/PluginDriver.h index fedac39..fef4746 100644 --- 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); diff --git a/Plugin/Driver/pid_controller.cpp b/Plugin/Driver/pid_controller.cpp new file mode 100644 index 0000000..8ee23c9 --- /dev/null +++ b/Plugin/Driver/pid_controller.cpp @@ -0,0 +1,75 @@ +#include "pid_controller.h" +#include + +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; +} diff --git a/Plugin/Driver/pid_controller.h b/Plugin/Driver/pid_controller.h new file mode 100644 index 0000000..26b8cfe --- /dev/null +++ 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 diff --git a/Plugin/Driver/resource.h b/Plugin/Driver/resource.h index 88944aec2d5b6020618b9907a56f40c07a1aacba..cdd5d3a4d4c1e65a16e7baca00cff562ed01d031 100644 GIT binary patch delta 40 wcmaFX!+5Bdal;wy$sf$PID;8n8T=Sr7>p-RwAGv(psg@jL4a%XHSGyf04t#lcK`qY delta 22 ecmX@q%lNE^al;wy$qE8ola(wDHYe#MNC5z93J4JZ diff --git a/Plugin/Protocol.h b/Plugin/Protocol.h index 0f357e7..568690e 100644 --- a/Plugin/Protocol.h +++ b/Plugin/Protocol.h @@ -32,6 +32,8 @@ typedef enum GUIDE_QRCODE = 21, //QRDriverʶĶάϢ + GUIDE_FAST = 22, //FASTDriver Ӿʶ𵽵ƫƽǶϢ + }EM_MSG_TYPE; diff --git a/Plugin/QrGuide/QrMainDialog.cpp b/Plugin/QrGuide/QrMainDialog.cpp index 8e988d3..0324d26 100644 --- 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)