diff --git a/Plugin/Driver/DriverMainDlg.cpp b/Plugin/Driver/DriverMainDlg.cpp index 99a649d..92c75ac 100644 --- a/Plugin/Driver/DriverMainDlg.cpp +++ b/Plugin/Driver/DriverMainDlg.cpp @@ -9,6 +9,7 @@ #include "pid_controller.h" #include #include +#include @@ -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(speedInRPM); + if (speedValue > 5000) { + speedValue = 5000; + } + char speedLowByte = static_cast(speedValue & 0xFF); + char speedHighByte = static_cast((speedValue >> 8) & 0xFF); + + // 处理角度（假设 Ang 单位为度，范围 -180° - 180°，转换为 -18000 - 17999） + int16_t angleValue = static_cast(Ang * 100); + if (angleValue < -18000) { + angleValue = -18000; + } + if (angleValue > 17999) { + angleValue = 17999; + } + char angleLowByte = static_cast(angleValue & 0xFF); + char angleHighByte = static_cast((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; } diff --git a/Plugin/Driver/DriverMainDlg.h b/Plugin/Driver/DriverMainDlg.h index df45126..1b8bf28 100644 --- a/Plugin/Driver/DriverMainDlg.h +++ b/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); diff --git a/Plugin/Driver/葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺a b/Plugin/Driver/葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺a new file mode 100644 index 0000000..cca8f15 --- /dev/null +++ b/Plugin/Driver/葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺葺a @@ -0,0 +1,2 @@ +[06-17 09:24:50.073] (inner)SendCanThreadForFast: Vel = 0.300000, Ang = 0.000000 +