5b092de045
在 `DriverMainDlg.cpp` 中添加了对 `<cstdint>` 的引用，以支持固定宽度整数类型。修改了 `CorrectAngle` 函数中的 PID 控制器时间步长，并添加了相关注释。删除了 `ReadConfigFromIni` 中的多余返回语句，并在 `SendCanData` 中增加了对 CAN 设备状态的检查。 新增 `SendCanControlDataForKDS` 函数以处理柯蒂斯控制器的 CAN 数据发送，更新了 `SendCanThreadForFast` 函数以调用新的数据发送函数，并调整了休眠时间。同时，在 `DriverMainDlg.h` 中声明了新函数，并在日志中记录了发送的 CAN 数据以便调试。
1289 lines
34 KiB
C++
1289 lines
34 KiB
C++
// CanDeviceDlg.cpp : 实现文件
|
||
//
|
||
|
||
#include "stdafx.h"
|
||
#include "DriverMainDlg.h"
|
||
#include "resource.h"
|
||
#include "afxdialogex.h"
|
||
#include "PluginDriver.h"
|
||
#include "pid_controller.h"
|
||
#include <iostream>
|
||
#include <cmath>
|
||
#include <cstdint>
|
||
|
||
|
||
|
||
int StopFlag = 0;
|
||
unsigned long nextrow;
|
||
#define TIMER_UPDATE_POS (19999)
|
||
|
||
// CCanDeviceDlg 对话框
|
||
//控件ID只需要在当前窗口中唯一，这里随便写一个就好
|
||
#define ID_VIRTUAL_LIST_CTRL (20000)
|
||
#define TIMER_ID_ADD_TEST_DATA (2024)
|
||
IMPLEMENT_DYNAMIC(CDriverMainDlg, CDialogEx)
|
||
|
||
CDriverMainDlg::CDriverMainDlg(CWnd* pParent /*=NULL*/)
|
||
: CDialogEx(IDD_DEV_CAN_DLG, pParent)
|
||
{
|
||
m_bOpenCan = FALSE;
|
||
m_DevIndex = 0;
|
||
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;
|
||
m_nSendCanIndex = 0;
|
||
m_nRecvCanIndex = 1;
|
||
m_pstVirtualList = new CEXVirtualListCtrl();
|
||
m_pQueue = NULL;
|
||
m_iniPath = theApp.m_strModulePath + "\\config.ini";
|
||
ReadConfigFromIni();
|
||
|
||
}
|
||
|
||
CDriverMainDlg::~CDriverMainDlg()
|
||
{
|
||
}
|
||
|
||
void CDriverMainDlg::DoDataExchange(CDataExchange* pDX)
|
||
{
|
||
CDialogEx::DoDataExchange(pDX);
|
||
DDX_CBIndex(pDX, IDC_COMBO_SENDFRAMETYPE_IDX0, m_nSendFrameTypeIdx0);
|
||
DDX_CBIndex(pDX, IDC_COMBO_SENDFRAMEFORMAT_IDX0, m_nSendFrameFormatIdx0);
|
||
DDX_CBIndex(pDX, IDC_COMBO_SENDFRAMETYPE_IDX1, m_nSendFrameTypeIdx1);
|
||
DDX_CBIndex(pDX, IDC_COMBO_SENDFRAMEFORMAT_IDX1, m_nSendFrameFormatIdx1);
|
||
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);
|
||
DDX_Control(pDX, IDC_STATIC_POSITION, m_PositionView);
|
||
DDX_Control(pDX, IDC_EDIT1, Mil_info);
|
||
DDX_Control(pDX, IDC_EDIT2, TAG);
|
||
}
|
||
|
||
|
||
BEGIN_MESSAGE_MAP(CDriverMainDlg, CDialogEx)
|
||
ON_BN_CLICKED(IDC_CHECK_CANRX_EN, OnCheckCanrxEn)
|
||
ON_BN_CLICKED(IDC_BTN_SEND_MAN, &CDriverMainDlg::OnBnClickedBtnSendMan)
|
||
ON_BN_CLICKED(IDC_AUTO_SEND, &CDriverMainDlg::OnBnClickedAutoSend)
|
||
ON_BN_CLICKED(IDC_CHECK_AUTO_REFRESH_SHOW, &CDriverMainDlg::OnBnClickedCheckAutoRefreshShow)
|
||
ON_BN_CLICKED(IDC_BTN_SEND_MAN2, &CDriverMainDlg::OnBnClickedBtnSendMan2)
|
||
ON_BN_CLICKED(IDC_AUTO_SEND2, &CDriverMainDlg::OnBnClickedAutoSend2)
|
||
ON_BN_CLICKED(IDC_BUTTON_AGVSTOP, &CDriverMainDlg::OnBnClickedButtonAgvstop)
|
||
ON_BN_CLICKED(IDC_BUTTON_AGVESTOP, &CDriverMainDlg::OnBnClickedButtonAgvestop)
|
||
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'))
|
||
return c - 0x30;
|
||
else if ((c >= 'A') && (c <= 'F'))
|
||
return c - 'A' + 10;
|
||
else if ((c >= 'a') && (c <= 'f'))
|
||
return c - 'a' + 10;
|
||
else
|
||
return 0x10;
|
||
}
|
||
|
||
//两位十六进制数转换为十进制
|
||
int CDriverMainDlg::Str2Hex(CString str)
|
||
{
|
||
int len = str.GetLength();
|
||
if (len == 2)
|
||
{
|
||
int a = HexChar(str[0]);
|
||
int b = HexChar(str[1]);
|
||
if (a == 16 || b == 16)
|
||
{
|
||
AfxMessageBox("Format error");
|
||
return 256;
|
||
}
|
||
else
|
||
{
|
||
return a * 16 + b;
|
||
|
||
}
|
||
|
||
}
|
||
else
|
||
{
|
||
AfxMessageBox("input length must be 2");
|
||
return 256;
|
||
}
|
||
}
|
||
|
||
void CDriverMainDlg::InitVirtualList()
|
||
{
|
||
//信息显示列表初始化
|
||
m_pstVirtualList->InsertColumn(0, "Seq");
|
||
m_pstVirtualList->SetColumnWidth(0, 110);
|
||
m_pstVirtualList->InsertColumn(1, "Time");
|
||
m_pstVirtualList->SetColumnWidth(1, 110);
|
||
m_pstVirtualList->InsertColumn(2, "CANIndex");
|
||
m_pstVirtualList->SetColumnWidth(2, 100);
|
||
m_pstVirtualList->InsertColumn(3, "Tx-Rx");
|
||
m_pstVirtualList->SetColumnWidth(3, 80);
|
||
m_pstVirtualList->InsertColumn(4, " ID ");
|
||
m_pstVirtualList->SetColumnWidth(4, 100);
|
||
m_pstVirtualList->InsertColumn(5, "Frame");
|
||
m_pstVirtualList->SetColumnWidth(5, 80);
|
||
m_pstVirtualList->InsertColumn(6, "Type");
|
||
m_pstVirtualList->SetColumnWidth(6, 90);
|
||
m_pstVirtualList->InsertColumn(7, "DLC");
|
||
m_pstVirtualList->SetColumnWidth(7, 60);
|
||
m_pstVirtualList->InsertColumn(8, "Data");
|
||
m_pstVirtualList->SetColumnWidth(8, 250);
|
||
}
|
||
|
||
void CDriverMainDlg::UpdatePositionView()
|
||
{
|
||
m_PositionView.Invalidate();
|
||
CRect rect;
|
||
GetDlgItem(IDC_STATIC_POS_LABEL)->GetWindowRect(&rect);
|
||
ScreenToClient(rect);
|
||
InvalidateRect(rect);
|
||
}
|
||
|
||
//更新车辆位置，nPosition记录从巷道口的里程，从0开始计算
|
||
void CDriverMainDlg::UpdateVehiclePosition(int nPosition)
|
||
{
|
||
m_PositionView.m_nPosition = nPosition;
|
||
UpdatePositionView();
|
||
}
|
||
|
||
void CDriverMainDlg::OnTimer(UINT_PTR nIDEvent)
|
||
{
|
||
// TODO: 在此添加消息处理程序代码和/或调用默认值
|
||
|
||
static int n = 0;
|
||
if (TIMER_UPDATE_POS == nIDEvent)
|
||
{
|
||
//定时刷新车辆位置
|
||
m_PositionView.m_nPosition = n++ * 600;
|
||
UpdatePositionView();
|
||
}
|
||
else
|
||
{
|
||
//自定义定时器，无需调用基类OnTimer，否则定时器会被强制结束
|
||
CDialog::OnTimer(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); //初始化为空格字符串
|
||
memcpy(acId, strFrameId.GetBuffer(), strFrameId.GetLength());
|
||
|
||
|
||
char acData[33] = { 0 };
|
||
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(),
|
||
systime.wHour, systime.wMinute, systime.wSecond, systime.wMilliseconds,
|
||
nCanIdx,
|
||
nTxRx,
|
||
acId,
|
||
nFrameFormat,
|
||
nFrameType,
|
||
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("已连接父进程管道");
|
||
|
||
//向WMS服务器请求设备配置信息
|
||
//theApp.SendMsg2Platform("WMS", DEVICE_CONFIG_REQ);
|
||
}
|
||
else if (CEXPIPE_DIS_CLIENT == lMsgId)
|
||
{
|
||
//管道断开，结束
|
||
#ifndef _DEBUG
|
||
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
|
||
}
|
||
else if (CEXPIPE_NEW_DATA == lMsgId)
|
||
{
|
||
pModule->ProcessPipeMsg(lMsgId, (char*)wparam, (int)lparam);
|
||
}
|
||
else
|
||
{
|
||
;
|
||
}
|
||
}
|
||
|
||
void CDriverMainDlg::GetRoadwayInfo(int nReqPosId)
|
||
{
|
||
Json::Value param;
|
||
|
||
param["request_pos"] = nReqPosId;
|
||
param["map_idx"] = 18;
|
||
param["agv_id"] = 1;
|
||
|
||
theApp.SendMsg2Platform("WMS", ROADWAY_CONFIG_REQ, param);
|
||
}
|
||
|
||
|
||
//处理 平台各个模块 传过来的消息
|
||
void CDriverMainDlg::ProcessPipeMsg(int lMsgId, char* pData, int lLen)
|
||
{
|
||
if (lLen == 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
PIPE_DATA_STRUCT* pstData = (PIPE_DATA_STRUCT*)pData;
|
||
|
||
//平台转发给插件的消息
|
||
if (pstData->lMsgId == MAIN_2_MODULE_WMS && pstData->lDataLen > 0)
|
||
{
|
||
Json::Reader reader;
|
||
Json::Value root;
|
||
if (reader.parse((char*)pstData->acData, root))
|
||
{
|
||
CString strReceiver = root["receiver"].asString().c_str();
|
||
CString strSender = root["sender"].asString().c_str();
|
||
int nMsgType = root["type"].asInt();
|
||
|
||
|
||
//从科聪控制器收到切换导航指令，向wms请求获取通道信息，包括标签等
|
||
if (nMsgType == CHANGE_GUIDE_TYPE)
|
||
{
|
||
AfxMessageBox("AGV到达请求点，切换导航方式");
|
||
GetRoadwayInfo(0);
|
||
}
|
||
//从wms获取巷道信息返回
|
||
else if (nMsgType == ROADWAY_CONFIG_RET)
|
||
{
|
||
Json::Value data = root["params"];
|
||
theApp.m_nRoadwayId = data["id"].asInt();
|
||
theApp.m_nRoadwayLen = data["length"].asInt();
|
||
theApp.m_strRoadwayName = data["name"].asString().c_str();
|
||
|
||
Json::Value labels = data["labels"];
|
||
theApp.m_nLabelCount = labels.size();
|
||
|
||
for (int i = 0; i < theApp.m_nLabelCount; i++)
|
||
{
|
||
theApp.m_GuideLabels[i].nIndex = labels[i]["label_index"].asInt();
|
||
theApp.m_GuideLabels[i].nTag = labels[i]["label_id"].asInt();
|
||
theApp.m_GuideLabels[i].nOffset = labels[i]["label_offset"].asInt();
|
||
}
|
||
m_PositionView.Update();
|
||
UpdatePositionView();
|
||
}
|
||
|
||
//从QR插件收到二维码信息
|
||
else if (nMsgType == GUIDE_QRCODE)
|
||
{
|
||
Json::Value data = root["params"];
|
||
theApp.m_fAngle = data["angle"].asInt();
|
||
theApp.m_nX = data["x"].asInt();
|
||
theApp.m_nY = data["y"].asInt();
|
||
theApp.m_nTag = data["tag"].asInt();
|
||
|
||
}
|
||
|
||
//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 == GET_TRANS_STATE_REQ)//请求获取输送线状态
|
||
{
|
||
|
||
}*/
|
||
|
||
}
|
||
}
|
||
else if (pstData->lMsgId == MAIN_2_MODULE_SHOWWINDOW)
|
||
{
|
||
//PostMessage(WM_COMMAND, MAKEWPARAM(ID_TRAY_SHOW, 0), 0);
|
||
AfxGetApp()->m_pMainWnd->ShowWindow(SW_SHOWNORMAL);
|
||
SetForegroundWindow();
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
//auth : caixiang
|
||
// 基于PID的角度纠正函数
|
||
float CDriverMainDlg::CorrectAngle(float currentAngle, float targetAngle) {
|
||
// 创建PID控制器实例
|
||
// 参数：比例系数、积分系数、微分系数、积分限幅、积分上限、输出下限、输出上限、时间步长(秒)
|
||
// 这些参数 是要根据实际情况进行调整的
|
||
//pid 对象在第一次调用 CorrectAngle 函数时创建，并且在程序的整个运行期间不会被销毁。即使函数执行结束，pid 对象仍然存在，其内部状态（如积分项 integral、上一次误差 prevError 等）会被持久化保存
|
||
static PIDController pid(
|
||
0.5f, //比例系数
|
||
0.2f, //积分系数
|
||
0.1f, //微分系数
|
||
-10.0f, //积分限幅
|
||
10.0f, //积分上限
|
||
-45.0f, //输出下限
|
||
45.0f, //输出上限
|
||
0.023f //时间步长(秒)
|
||
);
|
||
|
||
|
||
// 规范化角度，确保角度在-180°到180°之间
|
||
currentAngle = NormalizeAngle(currentAngle);
|
||
targetAngle = NormalizeAngle(targetAngle);
|
||
|
||
// 计算纠正角度，然后通过can协议，下发给电机。
|
||
float correction = pid.compute(targetAngle, currentAngle);
|
||
|
||
return correction;
|
||
}
|
||
|
||
|
||
|
||
|
||
// CCanDeviceDlg 消息处理程序
|
||
BOOL CDriverMainDlg::OnInitDialog()
|
||
{
|
||
|
||
CString strPipe;
|
||
for (int i = 0; i < __argc; i++)
|
||
{
|
||
if (0 == strcmp(__argv[i], "-pipe") && i + 1 < __argc)
|
||
{
|
||
strPipe = __argv[i + 1];
|
||
}
|
||
}
|
||
|
||
if (strPipe.IsEmpty())
|
||
{
|
||
char acPipe[256] = "";
|
||
|
||
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;
|
||
}
|
||
g_pstPipeClient->RegisterCall(g_PipeCallBack, this);
|
||
g_pstPipeClient->Connect(strPipe.GetBuffer());
|
||
|
||
|
||
|
||
m_strDataDir = theApp.m_strModulePath + "\\data";
|
||
CreateDirectory(m_strDataDir.GetString(), NULL);
|
||
|
||
|
||
if (!m_pstVirtualList->Create(WS_CHILD | WS_VISIBLE | LVS_REPORT | LVS_OWNERDATA | LVS_EX_DOUBLEBUFFER, CRect(0, 0, 0, 0), this, ID_VIRTUAL_LIST_CTRL)) {
|
||
TRACE0("Failed to create MyVirtualListCtrl\n");
|
||
delete m_pstVirtualList;
|
||
m_pstVirtualList = nullptr;
|
||
return FALSE; // 返回 FALSE 以使框架知道未成功初始化
|
||
}
|
||
|
||
InitVirtualList();
|
||
OpenCanDevice();
|
||
|
||
CRect rectList;
|
||
GetDlgItem(IDC_STATIC_LIST)->GetWindowRect(&rectList);
|
||
ScreenToClient(rectList);
|
||
m_pstVirtualList->MoveWindow(rectList.left, rectList.top, rectList.Width(), rectList.Height());
|
||
|
||
m_pQueue = new CEXMemFileQueue(m_strDataDir.GetString());
|
||
m_pstVirtualList->SetData(m_pQueue);
|
||
//m_pstVirtualList->StartAutoRefresh(10);//列表10毫秒刷新一次
|
||
|
||
CDialogEx::OnInitDialog();
|
||
|
||
if (TRUE == m_bAutoRefresh)
|
||
{
|
||
m_pstVirtualList->StartAutoRefresh(10);
|
||
}
|
||
else
|
||
{
|
||
m_pstVirtualList->StopAutoRefresh();
|
||
}
|
||
|
||
if (m_bCanRxEn)
|
||
{
|
||
StopFlag = 0;
|
||
//开启接收线程
|
||
AfxBeginThread(ReceiveCanThread, this);
|
||
}
|
||
else
|
||
StopFlag = 1;
|
||
|
||
if (m_bAutoSend2)
|
||
{
|
||
m_StopSendFlag2 = 0;
|
||
//开启发送线程
|
||
AfxBeginThread(SendCanThread2, this);
|
||
}
|
||
if (m_bAutoSendForFast)
|
||
{
|
||
m_StopSendFlagForFast = 0;
|
||
//开启发送线程
|
||
AfxBeginThread(SendCanThreadForFast, this);
|
||
}
|
||
|
||
else
|
||
m_StopSendFlag2 = 1;
|
||
|
||
|
||
//SetTimer(TIMER_UPDATE_POS, 1000, NULL);
|
||
|
||
|
||
GetRoadwayInfo(1);
|
||
|
||
return TRUE;
|
||
|
||
|
||
}
|
||
|
||
//打开设备
|
||
BOOL CDriverMainDlg::OpenCanDevice()
|
||
{
|
||
|
||
DWORD Reserved = 0;
|
||
//打开设备
|
||
if (VCI_OpenDevice(VCI_USBCAN2, 0, Reserved) != 1)
|
||
{
|
||
//MessageBox("open failed");
|
||
//m_pMainWnd->InsertLog(LOG_ERROR, "Open Can Device Failed");
|
||
return FALSE;
|
||
}
|
||
VCI_INIT_CONFIG InitInfo[1];
|
||
InitInfo->Timing0 = 0x01; //250K
|
||
InitInfo->Timing1 = 0x1C;
|
||
InitInfo->Filter = 0;
|
||
InitInfo->AccCode = 0x80000000;
|
||
InitInfo->AccMask = 0xFFFFFFFF;
|
||
InitInfo->Mode = 0; //Data ;Remote;
|
||
//初始化通道0
|
||
if (VCI_InitCAN(VCI_USBCAN2, 0, 0, InitInfo) != 1) //can-0
|
||
{
|
||
//MessageBox("Init-CAN failed!");
|
||
//m_pMainWnd->InsertLog(LOG_ERROR, "Init-CAN-Index0 Failed!");
|
||
return FALSE;
|
||
}
|
||
Sleep(100);
|
||
//初始化通道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
|
||
//if (m_nDevType == 1)
|
||
{
|
||
if (VCI_InitCAN(VCI_USBCAN2, 0, 1, InitInfo) != 1) //can-1
|
||
{
|
||
//MessageBox("Init-CAN failed!");
|
||
//m_pMainWnd->InsertLog(LOG_ERROR, "Init-CAN-Index1 Failed!");
|
||
return FALSE;
|
||
}
|
||
}
|
||
Sleep(100);
|
||
|
||
InitInfo->Mode = 0; //Data ;Remote;
|
||
//初始化通道1
|
||
if (VCI_StartCAN(VCI_USBCAN2, 0, 1) != 1) //can-0
|
||
{
|
||
//MessageBox("Start-CAN failed!");
|
||
//m_pMainWnd->InsertLog(LOG_ERROR, "Start-CAN-Index1 Failed!");
|
||
return FALSE;
|
||
}
|
||
|
||
//MessageBox("Open successfule!\n Start CAN OK!");
|
||
UpdateCanStatue(TRUE);
|
||
//InsertLog(LOG_INFO, "Open The CAN Device Successfule!");
|
||
//KillTimer(TIMER_RECONNECT_CAN);
|
||
return TRUE;
|
||
|
||
}
|
||
|
||
void CDriverMainDlg::UpdateCanStatue(BOOL bStatue)
|
||
{
|
||
if (TRUE == bStatue)
|
||
{
|
||
GetDlgItem(IDC_STATUS_CAN)->SetWindowText("已连接");
|
||
}
|
||
else
|
||
{
|
||
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);
|
||
m_nSendFrameTypeIdx1 = GetPrivateProfileInt("CAN", "FRAME_TYPE_IDX1", 0, strIniPath);
|
||
|
||
return; // return TRUE unless you set the focus to a control
|
||
// 异常: OCX 属性页应返回 FALSE
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
//void CDriverMainDlg::Drive_enable() //使能
|
||
//{
|
||
// char can2_buff[8] = { 0 };
|
||
// can2_buff[0] = 0x23;
|
||
// can2_buff[1] = 0x0D;
|
||
// can2_buff[2] = 0x20;
|
||
// can2_buff[3] = 0x01;
|
||
// can2_buff[4] = 0;
|
||
// can2_buff[5] = 0;
|
||
// can2_buff[6] = 0;
|
||
// can2_buff[7] = 0;
|
||
// SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, 0x600001, can2_buff);
|
||
// SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, 0x600002, can2_buff);
|
||
//}
|
||
//void CDriverMainDlg::Drive_disable() //失能
|
||
//{
|
||
// char can2_buff[8] = { 0 };
|
||
// can2_buff[0] = 0x23;
|
||
// can2_buff[1] = 0x0C;
|
||
// can2_buff[2] = 0x20;
|
||
// can2_buff[3] = 0x01;
|
||
// can2_buff[4] = 0;
|
||
// can2_buff[5] = 0;
|
||
// can2_buff[6] = 0;
|
||
// can2_buff[7] = 0;
|
||
// SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, 0x600001, can2_buff);
|
||
// SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, 0x600002, can2_buff);
|
||
//}
|
||
|
||
|
||
/*
|
||
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;
|
||
sendbuf->ExternFlag = nFrameType;
|
||
sendbuf->DataLen = datanum;
|
||
sendbuf->RemoteFlag = nFrameFormat;
|
||
sendbuf->ID = acFrameId;
|
||
|
||
memcpy(sendbuf->Data, acFrameData, datanum);
|
||
/*for (int i = 0; i < datanum; i++)
|
||
{
|
||
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设备已断开
|
||
//MessageBox("failed- device not open\n");
|
||
//m_pMainWnd->InsertLog(LOG_ERROR, "Failed- Device Not Open!");
|
||
OpenCanDevice();
|
||
UpdateCanStatue(FALSE);
|
||
}
|
||
else if (flag == 0)
|
||
{
|
||
//MessageBox("send error\n");
|
||
//m_pMainWnd->InsertLog(LOG_ERROR, "Send Can Mesaage Error!");
|
||
UpdateCanStatue(FALSE);
|
||
}
|
||
|
||
if (false == snederr_flag)
|
||
{
|
||
snederr_flag = true;
|
||
UpdateCanStatue(FALSE);
|
||
}
|
||
return;
|
||
|
||
}
|
||
snederr_flag = false;
|
||
BYTE data;
|
||
CString strFrameId, strTmp, strFrameData;
|
||
strFrameId.Format("%08X", acFrameId);
|
||
|
||
for (int i = 0; i<sendbuf->DataLen; i++)
|
||
{
|
||
data = sendbuf->Data[i];
|
||
strTmp.Format("%02X", data);
|
||
strFrameData += strTmp + " ";
|
||
}
|
||
|
||
WriteVirtualList(strFrameId, strFrameData, SEND_MSG, nCanIdx, nFrameType, nFrameFormat);
|
||
|
||
}
|
||
|
||
void CDriverMainDlg::OnCheckCanrxEn()
|
||
{
|
||
UpdateData(TRUE);
|
||
if (m_bCanRxEn)
|
||
{
|
||
StopFlag = 0;
|
||
//开启接收线程
|
||
AfxBeginThread(ReceiveCanThread, this);
|
||
}
|
||
else
|
||
StopFlag = 1;
|
||
}
|
||
|
||
|
||
int Control_Mode_Switch;
|
||
UINT CDriverMainDlg::ReceiveCanThread(LPVOID v)
|
||
{
|
||
CDriverMainDlg *pThis = (CDriverMainDlg*)v;
|
||
|
||
while (1)
|
||
{
|
||
VCI_CAN_OBJ pCanObj[200];
|
||
|
||
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);
|
||
strTime.Format("%02d:%02d:%02d:%03d", systime.wHour, systime.wMinute, systime.wSecond, systime.wMilliseconds);
|
||
|
||
|
||
for (int num = 0; num < NumValue; num++)
|
||
{
|
||
CString strFrameId, strFrameData, strTmp;
|
||
strFrameId.Format("%08X", pCanObj[num].ID);
|
||
|
||
for (int i = 0; i < (pCanObj[num].DataLen); i++) //数据信息
|
||
{
|
||
strTmp.Format("%02X ", pCanObj[num].Data[i]);
|
||
strFrameData += strTmp;
|
||
}
|
||
|
||
pThis->WriteVirtualList(strFrameId, strFrameData, 1, kCanIndex);
|
||
|
||
//在这里处理收到的CAN消息
|
||
|
||
//解析定位引导数据
|
||
pThis->AnalysiseCanData(kCanIndex, pCanObj[num]);
|
||
|
||
/*if (MANUAL_MODE ==pThis->m_pMainWnd->m_nAgvMode)
|
||
{
|
||
|
||
}
|
||
else
|
||
{
|
||
//计算运动控制参数(自动模式),需要考虑m_nAgvAction值
|
||
if (Control_Mode_Switch)
|
||
{
|
||
Control_Mode_Switch = 0;
|
||
}
|
||
pThis->CalculateAutoCanParam();
|
||
}*/
|
||
|
||
|
||
|
||
}
|
||
}
|
||
else if (NumValue == -1)
|
||
{
|
||
// USB-CAN设备不存在或USB掉线，可以调用VCI_CloseDevice并重新VCI_OpenDevice。如此可以达到USB-CAN设备热插拔的效果。
|
||
pThis->UpdateCanStatue(FALSE);
|
||
pThis->OpenCanDevice();
|
||
}
|
||
|
||
}
|
||
Sleep(10);
|
||
|
||
if (StopFlag == 1)
|
||
{
|
||
return 0;
|
||
}
|
||
}
|
||
return 1;
|
||
}
|
||
|
||
|
||
//解析接收到的CAN数据
|
||
void CDriverMainDlg::AnalysiseCanData(int nCanIndex, VCI_CAN_OBJ objCan)
|
||
{
|
||
CString nEdit;
|
||
if ( 0 == nCanIndex)
|
||
{
|
||
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进制显示
|
||
Mil_info.SetWindowText(nEdit);
|
||
|
||
nEdit.Format(_T("%d"), theApp.m_nTag); // %x-16进制显示; %d-10进制显示
|
||
TAG.SetWindowText(nEdit);
|
||
}
|
||
}
|
||
else if (1 == nCanIndex)
|
||
{
|
||
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
void CDriverMainDlg::CalculateAutoCanParam()
|
||
{
|
||
switch (m_nAgvAction)
|
||
{
|
||
case A_STOP:
|
||
|
||
break;
|
||
case FORWARD:
|
||
m_nAgvReturnState = Auto_Forward(m_stDrvierControl.SPEEDPAR);
|
||
break;
|
||
case BACKWARD:
|
||
m_nAgvReturnState = Auto_Backward(m_stDrvierControl.SPEEDPAR);
|
||
break;
|
||
case E_STOP:
|
||
|
||
break;
|
||
default:
|
||
|
||
break;
|
||
}
|
||
}
|
||
|
||
int CDriverMainDlg::Auto_Forward(float speedpar) //去载货区 进入雷达减速区通过速度系数调整速度
|
||
{
|
||
int Position_P = 4000; //转向比例
|
||
if (m_stSensorData.FMagnetism_Valid == 1) //磁条数据有效
|
||
{
|
||
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) //横向磁条停车 正常停车
|
||
{
|
||
m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle;
|
||
m_stDrvierControl.Drive_Speed = 0;
|
||
return RE_POINT_STOP;
|
||
}
|
||
return RE_NORMAL_RUN;
|
||
}
|
||
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 Position_P = 4000;
|
||
if (m_stSensorData.BMagnetism_Valid == 1) //磁条数据有效
|
||
{
|
||
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) //横向磁条停车 正常停车
|
||
{
|
||
m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle;
|
||
m_stDrvierControl.Drive_Speed = 0;
|
||
return RE_POINT_STOP;
|
||
}
|
||
return RE_NORMAL_RUN;
|
||
}
|
||
else
|
||
{
|
||
m_stDrvierControl.Diversion_Position = m_stDrvierControl.Zero_Angle;
|
||
m_stDrvierControl.Drive_Speed = 0;
|
||
return RE_UNUSUAL_STOP;
|
||
}
|
||
}
|
||
|
||
int Speed = 0;
|
||
int LastSpeed = 0;
|
||
void CDriverMainDlg::SendCanControlData(float Vel, float Ang)
|
||
{
|
||
UINT32 acId = 0x5A1; //速度 角度帧
|
||
char acData[8] = {};
|
||
acData[0] = *((char*)&theApp.m_fVelCal + 0);
|
||
acData[1] = *((char*)&theApp.m_fVelCal + 1);
|
||
acData[2] = *((char*)&theApp.m_fVelCal + 2);
|
||
acData[3] = *((char*)&theApp.m_fVelCal + 3);
|
||
acData[4] = *((char*)&theApp.m_fAngCal + 0);
|
||
acData[5] = *((char*)&theApp.m_fAngCal + 1);
|
||
acData[6] = *((char*)&theApp.m_fAngCal + 2);
|
||
acData[7] = *((char*)&theApp.m_fAngCal + 3);
|
||
SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, acId, acData);
|
||
}
|
||
|
||
//TODO caixiang; can -> 科聪控制器
|
||
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
|
||
);
|
||
}
|
||
|
||
|
||
//TODO caixiang; can -> 柯蒂斯控制器
|
||
// 手动定义圆周率的值
|
||
constexpr double PI = 3.14159265358979323846;
|
||
// 发送 CAN 控制数据的函数
|
||
void CDriverMainDlg::SendCanControlDataForKDS(float Vel, float Ang) {
|
||
const float wheelRadius = 0.3; // 轮子半径，单位：米
|
||
// 将速度从 m/s 转换为 RPM，取绝对值因为方向由单独的位控制
|
||
float speedInRPM = std::abs(Vel * 60) / (2 * PI * wheelRadius);
|
||
|
||
// 处理速度（假设范围 0 - 5000 RPM）
|
||
uint16_t speedValue = static_cast<uint16_t>(speedInRPM);
|
||
if (speedValue > 5000) {
|
||
speedValue = 5000;
|
||
}
|
||
char speedLowByte = static_cast<char>(speedValue & 0xFF);
|
||
char speedHighByte = static_cast<char>((speedValue >> 8) & 0xFF);
|
||
|
||
// 处理角度（假设 Ang 单位为度，范围 -180° - 180°，转换为 -18000 - 17999）
|
||
int16_t angleValue = static_cast<int16_t>(Ang * 100);
|
||
if (angleValue < -18000) {
|
||
angleValue = -18000;
|
||
}
|
||
if (angleValue > 17999) {
|
||
angleValue = 17999;
|
||
}
|
||
char angleLowByte = static_cast<char>(angleValue & 0xFF);
|
||
char angleHighByte = static_cast<char>((angleValue >> 8) & 0xFF);
|
||
|
||
// 构建 0x226 的 CAN 报文
|
||
char canData226[8] = { 0 };
|
||
// 自动模式 Interlock 设为 On（1）
|
||
canData226[0] = 0b00000001;
|
||
if (Vel > 0) {
|
||
// 速度大于 0，行走前进设为 On（1）
|
||
canData226[0] |= 0b00000010;
|
||
}
|
||
else if (Vel < 0) {
|
||
// 速度小于 0，行走后退设为 On（1）
|
||
canData226[0] |= 0b00000100;
|
||
}
|
||
canData226[1] = speedLowByte;
|
||
canData226[2] = speedHighByte;
|
||
// 假设加速率、减速率、最大转速加速率、最大转速减速率都为 1（0.1s），根据实际情况调整
|
||
canData226[3] = 5;
|
||
canData226[4] = 5;
|
||
canData226[5] = 5;
|
||
canData226[6] = 5;
|
||
//sendCanMessage(0x226, canData226);
|
||
LogOutToFile("(inner)SendCanControlDataForKDS: canData226 = %c,%c,%c,%c,%c,%c,%c,%c", canData226[0], canData226[1], canData226[2], canData226[3], canData226[4], canData226[5], canData226[6]);
|
||
SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, 0x226, canData226);
|
||
|
||
// 构建 0x326 的 CAN 报文
|
||
char canData326[8] = { 0 };
|
||
canData326[0] = angleLowByte;
|
||
canData326[1] = angleHighByte;
|
||
//sendCanMessage(0x326, canData326);
|
||
LogOutToFile("(inner)SendCanControlDataForKDS: canData326 = %c,%c,%c,%c,%c,%c,%c,%c", canData326[0], canData326[1], canData326[2], canData326[3], canData326[4], canData326[5], canData326[6]);
|
||
//SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, 0x326, canData326);
|
||
}
|
||
|
||
|
||
|
||
void CDriverMainDlg::OnBnClickedBtnSendMan()
|
||
{
|
||
UINT32 acId = 0x023;
|
||
char acData[8] = { 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08 };
|
||
SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, acId, acData);
|
||
}
|
||
|
||
|
||
|
||
UINT CDriverMainDlg::SendCanThread(LPVOID v)
|
||
{
|
||
|
||
CDriverMainDlg *dlg = (CDriverMainDlg*)v;
|
||
while ( 0 == dlg->m_StopSendFlag)
|
||
{
|
||
UINT32 acId = 0x11;
|
||
char acData[8] = { 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08 };
|
||
dlg->SendCanData(0, 1, dlg->m_nSendFrameTypeIdx0, dlg->m_nSendFrameFormatIdx0, acId, acData);
|
||
Sleep(10);
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
|
||
void CDriverMainDlg::OnBnClickedAutoSend()
|
||
{
|
||
// TODO: 在此添加控件通知处理程序代码
|
||
UpdateData(TRUE);
|
||
if (m_bAutoSend)
|
||
{
|
||
m_StopSendFlag = 0;
|
||
//开启接收线程
|
||
AfxBeginThread(SendCanThread, this);
|
||
}
|
||
else
|
||
m_StopSendFlag = 1;
|
||
}
|
||
|
||
|
||
void CDriverMainDlg::OnBnClickedCheckAutoRefreshShow()
|
||
{
|
||
// TODO: 在此添加控件通知处理程序代码
|
||
if (m_bAutoRefresh)
|
||
{
|
||
m_pstVirtualList->StartAutoRefresh(10);
|
||
}
|
||
else
|
||
{
|
||
m_pstVirtualList->StopAutoRefresh();
|
||
}
|
||
}
|
||
|
||
|
||
void CDriverMainDlg::OnBnClickedBtnSendMan2() //电机使能
|
||
{
|
||
/*UINT32 acId = 0x6000001;
|
||
char acData[8] = { 0x23, 0x0D, 0x20, 0x01, 0x00, 0x00, 0x00, 0x00 };
|
||
SendCanData(0, 1, m_nSendFrameTypeIdx1, m_nSendFrameFormatIdx1, acId, acData);*/
|
||
}
|
||
|
||
|
||
|
||
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内不响应
|
||
|
||
float Angle = 0;
|
||
float CrosswiseX = 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;
|
||
if (CrosswiseX > 0)
|
||
{
|
||
UserX = -CrosswiseX * 0.008;
|
||
}
|
||
else if(CrosswiseX < 0)
|
||
{
|
||
UserX = -CrosswiseX * 0.008;
|
||
}
|
||
else
|
||
{
|
||
}
|
||
}
|
||
else if (Angle < 0)
|
||
{
|
||
UserAng = -Angle * KP_THETA;
|
||
UserX = CrosswiseX * 0.008;
|
||
if (CrosswiseX > 0)
|
||
{
|
||
|
||
}
|
||
}
|
||
else
|
||
{
|
||
UserX = -CrosswiseX * 0.005;
|
||
if (CrosswiseX > 0)
|
||
{
|
||
|
||
}
|
||
else if (CrosswiseX < 0)
|
||
{
|
||
|
||
}
|
||
}
|
||
}
|
||
// 正向行驶时的控制逻辑
|
||
else if (theApp.m_fVelCal > 0)
|
||
{
|
||
if (Angle > 0)
|
||
{
|
||
UserAng = Angle * KP_THETA;
|
||
UserX = -CrosswiseX * 0.005;
|
||
}
|
||
else if (Angle < 0)
|
||
{
|
||
UserAng = Angle * KP_THETA;
|
||
UserX = -CrosswiseX * 0.005;
|
||
if (CrosswiseX > 0)
|
||
{
|
||
|
||
}
|
||
}
|
||
else
|
||
{
|
||
UserX = -CrosswiseX * 0.005;
|
||
}
|
||
}
|
||
|
||
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;
|
||
//下发运动控制参数
|
||
LogOutToFile("(inner)SendCanThreadForFast: Vel = %f, Ang = %f", theApp.m_fVelCalForFast, theApp.m_fAngCalForFast);
|
||
dlg->SendCanControlDataForKDS(theApp.m_fVelCalForFast, theApp.m_fAngCalForFast);
|
||
Sleep(20);
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
void CDriverMainDlg::OnBnClickedAutoSend2()
|
||
{
|
||
// 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: 在此添加控件通知处理程序代码
|
||
m_nAgvAction = A_STOP;
|
||
before_trafficstop_action = -1;
|
||
before_safestop_action = -1;
|
||
}
|
||
|
||
|
||
void CDriverMainDlg::OnBnClickedButtonAgvestop()
|
||
{
|
||
// TODO: 在此添加控件通知处理程序代码
|
||
m_nAgvAction = E_STOP;
|
||
before_trafficstop_action = -1;
|
||
before_safestop_action = -1;
|
||
}
|
||
|
||
|
||
void CDriverMainDlg::OnBnClickedButtonAgvforward()
|
||
{
|
||
// TODO: 在此添加控件通知处理程序代码
|
||
m_nAgvAction = FORWARD;
|
||
}
|
||
|
||
|
||
void CDriverMainDlg::OnBnClickedButtonAgvbackward()
|
||
{
|
||
// TODO: 在此添加控件通知处理程序代码
|
||
m_nAgvAction = BACKWARD;
|
||
}
|
||
|
||
void CDriverMainDlg::OnEnChangeEdit1()
|
||
{
|
||
// TODO: 如果该控件是 RICHEDIT 控件，它将不
|
||
// 发送此通知，除非重写 CDialogEx::OnInitDialog()
|
||
// 函数并调用 CRichEditCtrl().SetEventMask()，
|
||
// 同时将 ENM_CHANGE 标志“或”运算到掩码中。
|
||
|
||
// TODO: 在此添加控件通知处理程序代码
|
||
}
|
||
|
||
|