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

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

Browse Source
This commit is contained in:
李广豪
2025-06-16 13:31:34 +08:00
parent 7e13599f5c 4be62027c6
commit 6733dfe597
12 changed files with 414 additions and 106 deletions
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 : ʵ<EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD>
// 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 <EFBFBD>Ի<EFBFBD><EFBFBD><EFBFBD>
//<EFBFBD>ؼ<EFBFBD>IDֻ<EFBFBD><EFBFBD>Ҫ<EFBFBD>ڵ<EFBFBD>ǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ψһ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>дһ<EFBFBD><EFBFBD><EFBFBD>ͺ<EFBFBD>
// 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()
//һλʮ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>Ϊʮ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//一位十六进制转换为十进制
int CDriverMainDlg::HexChar(char c)
{
if ((c >= '0') && (c <= '9'))
@@ -94,7 +103,7 @@ int CDriverMainDlg::HexChar(char c)
return 0x10;
}
//<EFBFBD><EFBFBD>λʮ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>Ϊʮ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//两位十六进制数转换为十进制
int CDriverMainDlg::Str2Hex(CString str)
{
int len = str.GetLength();
@@ -123,7 +132,7 @@ int CDriverMainDlg::Str2Hex(CString str)
void CDriverMainDlg::InitVirtualList()
{
//<EFBFBD><EFBFBD>Ϣ<EFBFBD><EFBFBD>ʾ<EFBFBD>б<EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
//信息显示列表初始化
m_pstVirtualList->InsertColumn(0, "Seq");
m_pstVirtualList->SetColumnWidth(0, 110);
m_pstVirtualList->InsertColumn(1, "Time");
@@ -153,7 +162,7 @@ void CDriverMainDlg::UpdatePositionView()
InvalidateRect(rect);
}
//<EFBFBD><EFBFBD><EFBFBD>³<EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>nPosition<EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>̣<EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//更新车辆位置，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: <EFBFBD>ڴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>/<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĭ<EFBFBD><C4AC>ֵ
// TODO: 在此添加消息处理程序代码和/或调用默认值
static int n = 0;
if (TIMER_UPDATE_POS == nIDEvent)
{
//<EFBFBD><EFBFBD>ʱˢ<EFBFBD>³<EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>
//定时刷新车辆位置
m_PositionView.m_nPosition = n++ * 600;
UpdatePositionView();
}
else
{
//<EFBFBD>Զ<EFBFBD><EFBFBD>嶨ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD>OnTimer<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>ᱻǿ<EFBFBD>ƽ<EFBFBD><EFBFBD><EFBFBD>
//自定义定时器，无需调用基类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)
{
//<EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD><EFBFBD>и<EFBFBD>ʽ<EFBFBD><EFBFBD>
//数据按行格式化
char acTemp[DATA_LINE_SIZE + 1] = { 0 };
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD>б<EFBFBD><EFBFBD><EFBFBD>ʾ
//发送信息列表显示
CString strTime;
SYSTEMTIME systime;
GetLocalTime(&systime);
char acId[11] = { 0 };
memset(acId, 32, 10); //<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>Ϊ<EFBFBD>ո<EFBFBD><EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD>
memset(acId, 32, 10); //初始化为空格字符串
memcpy(acId, strFrameId.GetBuffer(), strFrameId.GetLength());
char acData[33] = { 0 };
memset(acData, 32, 32); //<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>Ϊ<EFBFBD>ո<EFBFBD><EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD>
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);
//<EFBFBD><EFBFBD><EFBFBD>ýӿ<EFBFBD>д<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//调用接口写入数据
m_pQueue->WriteItem(acTemp);
}
//<EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨѶ<EFBFBD>Ĺܵ<EFBFBD><EFBFBD>ص<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//和模块进程通讯的管道回调函数：
void g_PipeCallBack(void* pObj, int lMsgId, WPARAM wparam, LPARAM lparam)
{
CDriverMainDlg* pModule = (CDriverMainDlg*)pObj;
if (CEXPIPE_CONNECT_OK == lMsgId)
{
//<EFBFBD><EFBFBD><EFBFBD>ӳɹ<EFBFBD>
//连接成功
//if (FALSE == pModule->PostMessage(WM_PLATFORM_CONNECT_OK, NULL, NULL)) { LogOutToFile("g_PipeCallBack PostMessage error[%d]", lMsgId); }
LogOutToFile("<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>̹ܵ<EFBFBD>");
LogOutToFile("已连接父进程管道");
//<EFBFBD><EFBFBD>WMS<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
//向WMS服务器请求设备配置信息
//theApp.SendMsg2Platform("WMS", DEVICE_CONFIG_REQ);
}
else if (CEXPIPE_DIS_CLIENT == lMsgId)
{
//<EFBFBD>ܵ<EFBFBD><EFBFBD>Ͽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//管道断开，结束
#ifndef _DEBUG
LogOutToFile("<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӹܵ<EFBFBD><EFBFBD>Ͽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD><EFBFBD>˳<EFBFBD>");
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;
//ƽ̨ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
//平台转发给插件的消息
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();
//<EFBFBD>ӿƴϿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><EFBFBD>л<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><EFBFBD><EFBFBD>wms<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩ<EFBFBD><EFBFBD>
//从科聪控制器收到切换导航指令，向wms请求获取通道信息，包括标签等
if (nMsgType == CHANGE_GUIDE_TYPE)
{
AfxMessageBox("AGV<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD>л<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ");
AfxMessageBox("AGV到达请求点，切换导航方式");
GetRoadwayInfo(0);
}
//<EFBFBD><EFBFBD>wms<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//从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();
}
//<EFBFBD><EFBFBD>QR<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><EFBFBD><EFBFBD>ά<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
//从QR插件收到二维码信息
else if (nMsgType == GUIDE_QRCODE)
{
Json::Value data = root["params"];
@@ -317,16 +328,37 @@ void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen)
}
//<EFBFBD><EFBFBD>ȡ<EFBFBD>豸<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//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) //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵĹ<EFBFBD><EFBFBD><EFBFBD>ģʽ
else if (nMsgType == SET_TRANS_MODE_REQ && m_bDeviceInit) //设置输送线的工作模式
{
}
else if (nMsgType == GET_TRANS_STATE_REQ)//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬
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 <20><>Ϣ<EFBFBD><CFA2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//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";
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>
//中文名称，仅显示用
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; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD> FALSE <EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>֪<EFBFBD><EFBFBD>δ<EFBFBD>ɹ<EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
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);//<EFBFBD>б<EFBFBD>10<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˢ<EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>
//m_pstVirtualList->StartAutoRefresh(10);//列表10毫秒刷新一次
CDialogEx::OnInitDialog();
@@ -410,7 +482,7 @@ BOOL CDriverMainDlg::OnInitDialog()
if (m_bCanRxEn)
{
StopFlag = 0;
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߳<EFBFBD>
//开启接收线程
AfxBeginThread(ReceiveCanThread, this);
}
else
@@ -419,9 +491,16 @@ BOOL CDriverMainDlg::OnInitDialog()
if (m_bAutoSend2)
{
m_StopSendFlag2 = 0;
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߳<EFBFBD>
//开启发送线程
AfxBeginThread(SendCanThread2, this);
}
if (m_bAutoSendForFast)
{
m_StopSendFlagForFast = 0;
//开启发送线程
AfxBeginThread(SendCanThreadForFast, this);
}
else
m_StopSendFlag2 = 1;
@@ -436,12 +515,12 @@ BOOL CDriverMainDlg::OnInitDialog()
}
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>豸
//打开设备
BOOL CDriverMainDlg::OpenCanDevice()
{
DWORD Reserved = 0;
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>豸
//打开设备
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;
//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>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);
//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>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;
}
//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>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;
//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>ͨ<EFBFBD><EFBFBD>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("<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>");
GetDlgItem(IDC_STATUS_CAN)->SetWindowText("已连接");
}
else
{
GetDlgItem(IDC_STATUS_CAN)->SetWindowText("<EFBFBD>ѹر<EFBFBD>");
GetDlgItem(IDC_STATUS_CAN)->SetWindowText("已关闭");
}
}
void CDriverMainDlg::ReadConfigFromIni()
{
CString strIniPath = theApp.m_strModulePath + "\\config.ini";
// <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// 初始化设备配置
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
// <EFBFBD>쳣: OCX <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҳӦ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> FALSE
// 异常: OCX 属性页应返回 FALSE
}
//void CDriverMainDlg::Drive_enable() //ʹ<EFBFBD><EFBFBD>
//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() //ʧ<EFBFBD><EFBFBD>
//void CDriverMainDlg::Drive_disable() //失能
//{
// char can2_buff[8] = { 0 };
// can2_buff[0] = 0x23;
@@ -560,15 +639,16 @@ void CDriverMainDlg::ReadConfigFromIni()
/*
param0: can<EFBFBD>豸<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
param1: can<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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])
{
//<EFBFBD>ӽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
//从界面获取发送信息
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];
}*/
/****************************************************************************/
/******************************<EFBFBD>ӽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>***********************/
/******************************从界面获取发送信息完毕***********************/
/****************************************************************************/
static bool snederr_flag = false;
//<EFBFBD><EFBFBD><EFBFBD>ö<EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD>ӿⷢ<EFBFBD>ͺ<EFBFBD><EFBFBD><EFBFBD>
//调用动态链接库发送函数
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<EFBFBD>豸<EFBFBD>ѶϿ<EFBFBD>
//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;
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߳<EFBFBD>
//开启接收线程
AfxBeginThread(ReceiveCanThread, this);
}
else
@@ -657,11 +737,11 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
for (int kCanIndex = 0; kCanIndex<2; kCanIndex++)
{
//<EFBFBD><EFBFBD><EFBFBD>ö<EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>պ<EFBFBD><EFBFBD><EFBFBD>
//调用动态链接看接收函数
int NumValue = VCI_Receive(pThis->m_DevType, pThis->m_DevIndex, kCanIndex, pCanObj, 200, 0);
if (NumValue >0)
{
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD>б<EFBFBD><EFBFBD><EFBFBD>ʾ
//接收信息列表显示
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++) //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
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);
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ﴦ<EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><EFBFBD><EFBFBD>CAN<EFBFBD><EFBFBD>Ϣ
//在这里处理收到的CAN消息
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//解析定位引导数据
pThis->AnalysiseCanData(kCanIndex, pCanObj[num]);
/*if (MANUAL_MODE ==pThis->m_pMainWnd->m_nAgvMode)
@@ -692,7 +772,7 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
}
else
{
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ʋ<EFBFBD><EFBFBD><EFBFBD>(<28>Զ<EFBFBD>ģʽ),<2C><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>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<EFBFBD>豸<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڻ<EFBFBD>USB<EFBFBD><EFBFBD><EFBFBD>ߣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD><EFBFBD><EFBFBD>VCI_CloseDevice<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>VCI_OpenDevice<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˿<EFBFBD><EFBFBD>ԴﵽUSB-CAN<41>豸<EFBFBD>Ȳ<EFBFBD><C8B2>ε<EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
// USB-CAN设备不存在或USB掉线，可以调用VCI_CloseDevice并重新VCI_OpenDevice。如此可以达到USB-CAN设备热插拔的效果。
pThis->UpdateCanStatue(FALSE);
pThis->OpenCanDevice();
}
@@ -723,7 +803,7 @@ UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
}
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><EFBFBD><EFBFBD>CAN<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//解析接收到的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)
{
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//里程数据
m_stSensorData.Mile_info = (float *)objCan.Data;
CString nEdit;
nEdit.Format(_T("%f"), *m_stSensorData.Mile_info); // %x-16<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ; %d-10<31><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ
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<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ; %d-10<31><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ
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) //ȥ<EFBFBD>ػ<EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>״<EFBFBD><D7B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD>ٶ<EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
int CDriverMainDlg::Auto_Forward(float speedpar) //去载货区 进入雷达减速区通过速度系数调整速度
{
int Position_P = 4000; //ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (m_stSensorData.FMagnetism_Valid == 1) //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч
int Position_P = 4000; //转向比例
if (m_stSensorData.FMagnetism_Valid == 1) //磁条数据有效
{
m_stDrvierControl.Diversion_Position = Position_P * m_stSensorData.FMagnetism_Offset; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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) //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
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 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD>쳣ͣ<EFBFBD><EFBFBD>
else //磁条无效异常停车
{
m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle;
m_stDrvierControl.Drive_Speed = 0;
return RE_UNUSUAL_STOP;
}
}
int CDriverMainDlg::Auto_Backward(float speedpar) //ȥж<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int CDriverMainDlg::Auto_Backward(float speedpar) //去卸货区
{
int Position_P = 4000;
if (m_stSensorData.BMagnetism_Valid == 1) //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч
if (m_stSensorData.BMagnetism_Valid == 1) //磁条数据有效
{
m_stDrvierControl.Diversion_Position = Position_P * m_stSensorData.BMagnetism_Offset; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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) //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
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; //<EFBFBD>ٶ<EFBFBD> <20>Ƕ<EFBFBD>֡
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: <EFBFBD>ڴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӿؼ<EFBFBD>֪ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// TODO: 在此添加控件通知处理程序代码
UpdateData(TRUE);
if (m_bAutoSend)
{
m_StopSendFlag = 0;
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߳<EFBFBD>
//开启接收线程
AfxBeginThread(SendCanThread, this);
}
else
@@ -887,7 +993,7 @@ void CDriverMainDlg::OnBnClickedAutoSend()
void CDriverMainDlg::OnBnClickedCheckAutoRefreshShow()
{
// TODO: <EFBFBD>ڴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӿؼ<EFBFBD>֪ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// TODO: 在此添加控件通知处理程序代码
if (m_bAutoRefresh)
{
m_pstVirtualList->StartAutoRefresh(10);
@@ -899,7 +1005,7 @@ void CDriverMainDlg::OnBnClickedCheckAutoRefreshShow()
}
void CDriverMainDlg::OnBnClickedBtnSendMan2() //<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD>
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<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float KP_Y = 0.005; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>
float KI_Y = 0.01; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><EFBFBD><EFBFBD>
float KD_Y = 0.1; // ΢<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>񵴣<EFBFBD>
float KP_THETA = 0.5; // <EFBFBD>Ƕ<EFBFBD>ƫ<EFBFBD><EFBFBD>ϵ<EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>΢Сƫ<D0A1>
float DEADZONE_X = 100; // 2cm<63>ڲ<EFBFBD><DAB2><EFBFBD>Ӧ
float DEADZONE_THETA = 0.5; // 0.02rad<EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD>Ӧ
// 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);
//<EFBFBD>·<EFBFBD><EFBFBD>˶<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ʋ<EFBFBD><EFBFBD><EFBFBD>
//下发运动控制参数
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: <EFBFBD>ڴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӿؼ<EFBFBD>֪ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// TODO: 在此添加控件通知处理程序代码
UpdateData(TRUE);
if (m_bAutoSend2)
{
m_StopSendFlag2 = 0;
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߳<EFBFBD>
//开启发送线程
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: <EFBFBD>ڴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӿؼ<EFBFBD>֪ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// TODO: 在此添加控件通知处理程序代码
m_nAgvAction = A_STOP;
before_trafficstop_action = -1;
before_safestop_action = -1;
@@ -1039,7 +1188,7 @@ void CDriverMainDlg::OnBnClickedButtonAgvstop()
void CDriverMainDlg::OnBnClickedButtonAgvestop()
{
// TODO: <EFBFBD>ڴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӿؼ<EFBFBD>֪ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// TODO: 在此添加控件通知处理程序代码
m_nAgvAction = E_STOP;
before_trafficstop_action = -1;
before_safestop_action = -1;
@@ -1048,13 +1197,25 @@ void CDriverMainDlg::OnBnClickedButtonAgvestop()
void CDriverMainDlg::OnBnClickedButtonAgvforward()
{
// TODO: <EFBFBD>ڴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӿؼ<EFBFBD>֪ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// TODO: 在此添加控件通知处理程序代码
m_nAgvAction = FORWARD;
}
void CDriverMainDlg::OnBnClickedButtonAgvbackward()
{
// TODO: <EFBFBD>ڴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӿؼ<EFBFBD>֪ͨ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// 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(); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>
void Drive_disable(); //ʧ<><CAA7>
@@ -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;
//<2F><>fast <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD> <20><>ʼ<EFBFBD><CABC>
m_fVelCalForFast = 0.0f;
m_fAngleForFast = 0.0f;
m_fAngCalForFast = 0.0f;
// <20><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> Windows XP <20>ϵ<EFBFBD>Ӧ<EFBFBD>ó<EFBFBD><C3B3><EFBFBD><EFBFBD>嵥ָ<E5B5A5><D6B8>Ҫ

6
Plugin/Driver/PluginDriver.h
View File

@@ -51,6 +51,12 @@ public:
float m_fAngCal;
float m_fAngleForFast;
float m_fVelCalForFast;
float m_fAngCalForFast;
// ʵ<><CAB5>
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) {
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EEA3A8><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD> - <20><>ǰ<EFBFBD>Ƕȣ<C7B6>
float error = setpoint - measurement;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float proportionalTerm = kp * error;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EEA3A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>ͣ<EFBFBD>
integral += error * dt;
// <20><><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>
if (integral > integralLimitHigh) {
integral = integralLimitHigh;
}
else if (integral < integralLimitLow) {
integral = integralLimitLow;
}
float integralTerm = ki * integral;
// ΢<><CEA2><EFBFBD>ʹ<EEA3A8>ò<EFBFBD><C3B2><EFBFBD>ֵ<EFBFBD>ı仯<C4B1>ʶ<EFBFBD><CAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>仯<EFBFBD>ʣ<EFBFBD><CAA3>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӱ<EFBFBD>죩
float derivativeTerm = 0.0f;
if (!firstRun) {
// ʹ<>ò<EFBFBD><C3B2><EFBFBD>ֵ<EFBFBD>ı仯<C4B1>ʼ<EFBFBD><CABC><EFBFBD>΢<EFBFBD><CEA2>
float dMeasurement = measurement - prevMeasurement;
derivativeTerm = -kd * (dMeasurement / dt);
}
firstRun = false;
// <20><><EFBFBD>浱ǰ<E6B5B1><C7B0><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>´μ<C2B4><CEBC><EFBFBD>
prevMeasurement = measurement;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float output = proportionalTerm + integralTerm + derivativeTerm;
// <20><><EFBFBD><EFBFBD><EFBFBD>޷<EFBFBD>
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<49><44><EFBFBD><EFBFBD>
float kp; // <20><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>
float ki; // <20><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>
float kd; // ΢<><CEA2>ϵ<EFBFBD><CFB5>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float integralLimitLow;
float integralLimitHigh;
float outputLimitLow;
float outputLimitHigh;
// ״̬<D7B4><CCAC><EFBFBD><EFBFBD>
float prevError; // <20><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float integral; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float prevMeasurement; // <20><>һ<EFBFBD>β<EFBFBD><CEB2><EFBFBD>ֵ
// ʱ<><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float dt; // ʱ<>䲽<EFBFBD><E4B2BD>
bool firstRun; // <20>Ƿ<EFBFBD><C7B7>״<EFBFBD><D7B4><EFBFBD><EFBFBD><EFBFBD>
public:
// <20><><EFBFBD>캯<EFBFBD><ECBAAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
PIDController(float proportionalGain, float integralGain, float derivativeGain,
float iLimitLow, float iLimitHigh, float oLimitLow, float oLimitHigh, float timeStep);
// <20><><EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD>״̬
void reset();
// <20><><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD>
float compute(float setpoint, float measurement);
// <20><>ȡ<EFBFBD><C8A1>ǰPID<49><44><EFBFBD><EFBFBD>
void getTunings(float& p, float& i, float& d) const;
// <20><><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD>
void setTunings(float p, float i, float d);
};
#endif // PID_CONTROLLER_H

BIN
Plugin/Driver/resource.h
View File

Binary file not shown.