优化 CAN 数据发送逻辑和 PID 控制器设置

在 `DriverMainDlg.cpp` 中添加了对 `<cstdint>` 的引用，以支持固定宽度整数类型。修改了 `CorrectAngle` 函数中的 PID 控制器时间步长，并添加了相关注释。删除了 `ReadConfigFromIni` 中的多余返回语句，并在 `SendCanData` 中增加了对 CAN 设备状态的检查。

新增 `SendCanControlDataForKDS` 函数以处理柯蒂斯控制器的 CAN 数据发送，更新了 `SendCanThreadForFast` 函数以调用新的数据发送函数，并调整了休眠时间。同时，在 `DriverMainDlg.h` 中声明了新函数，并在日志中记录了发送的 CAN 数据以便调试。

Plugin/Driver/DriverMainDlg.cpp

@@ -9,6 +9,7 @@
 #include "pid_controller.h"
 #include <iostream>
 #include <cmath>
+#include <cstdint>
 
 
 
@@ -391,6 +392,7 @@ float CDriverMainDlg::CorrectAngle(float currentAngle, float targetAngle) {
 	// 创建PID控制器实例
 	// 参数：比例系数、积分系数、微分系数、积分限幅、积分上限、输出下限、输出上限、时间步长(秒)
 	// 这些参数 是要根据实际情况进行调整的
+	//pid 对象在第一次调用 CorrectAngle 函数时创建，并且在程序的整个运行期间不会被销毁。即使函数执行结束，pid 对象仍然存在，其内部状态（如积分项 integral、上一次误差 prevError 等）会被持久化保存
 	static PIDController pid(
 		0.5f,  //比例系数
 		0.2f,  //积分系数
@@ -399,7 +401,7 @@ float CDriverMainDlg::CorrectAngle(float currentAngle, float targetAngle) {
 		10.0f, //积分上限
 		-45.0f, //输出下限
 		45.0f, //输出上限
-		0.01f   //时间步长(秒)
+		0.023f   //时间步长(秒)
 	);
 	
 
@@ -599,7 +601,6 @@ void CDriverMainDlg::ReadConfigFromIni()
 	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
 }
@@ -679,7 +680,6 @@ void CDriverMainDlg::SendCanData(int nDevIdx, int nCanIdx, int nFrameType, int n
 			//m_pMainWnd->InsertLog(LOG_ERROR, "Failed- Device Not Open!");
 			OpenCanDevice();
 			UpdateCanStatue(FALSE);
-
 		}
 		else if (flag == 0)
 		{
@@ -924,6 +924,7 @@ void CDriverMainDlg::SendCanControlData(float Vel, float Ang)
 	SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, acId, acData);
 }
 
+//TODO caixiang; can -> 科聪控制器
 void CDriverMainDlg::SendCanControlDataFAST(float Vel, float Ang)
 {
 	UINT32 acId = 0x5A1;  // 速度 角度帧
@@ -947,7 +948,75 @@ void CDriverMainDlg::SendCanControlDataFAST(float Vel, float Ang)
 	acData[5] = angUnion.bytes[1];
 	acData[6] = angUnion.bytes[2];
 	acData[7] = angUnion.bytes[3];
-	SendCanData(0, 0, m_nSendFrameTypeIdx0, m_nSendFrameFormatIdx0, acId, acData);
+	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);
 }
 
 
@@ -1135,12 +1204,10 @@ UINT CDriverMainDlg::SendCanThreadForFast(LPVOID v)
 			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);
+		dlg->SendCanControlDataForKDS(theApp.m_fVelCalForFast, theApp.m_fAngCalForFast);
+		Sleep(20);
 	}
 	return 0;
 }

Plugin/Driver/DriverMainDlg.h

@@ -156,6 +156,7 @@ public:
 	void SendCanControlData(float Vel, float Ang);
 
 	void SendCanControlDataFAST(float Vel, float Ang);
+	void SendCanControlDataForKDS(float Vel, float Ang);
 
 	void GetRoadwayInfo(int nReqPosId);
 	

Plugin/Driver/葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺a

@@ -0,0 +1,2 @@
+[06-17 09:24:50.073]  (inner)SendCanThreadForFast: Vel = 0.300000, Ang = 0.000000
+