diff --git a/src/mc/CMakeLists.txt b/src/mc/CMakeLists.txt index ed4d78f..07a65e1 100644 --- a/src/mc/CMakeLists.txt +++ b/src/mc/CMakeLists.txt @@ -5,6 +5,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +link_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib) + # Find dependencies find_package(sweeper_interfaces REQUIRED) find_package(ament_cmake REQUIRED) @@ -19,6 +21,8 @@ add_executable(mc_node ${SRC_FILES}) ament_target_dependencies(mc_node rclcpp std_msgs sweeper_interfaces) +target_link_libraries(mc_node controlcan pthread) + # Set include directories for the target target_include_directories( mc_node @@ -31,6 +35,15 @@ target_compile_features(mc_node PUBLIC c_std_99 cxx_std_17) install(TARGETS mc_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +set(LIB_DIR ${CMAKE_CURRENT_SOURCE_DIR}/lib) + +# 安装libcontrolcan动态库（.so） +install( + FILES + ${LIB_DIR}/libcontrolcan.so + DESTINATION lib +) + # Handle testing if needed if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/src/mc/include/mc/controlcan.h b/src/mc/include/mc/controlcan.h new file mode 100755 index 0000000..93bc5cc --- /dev/null +++ b/src/mc/include/mc/controlcan.h @@ -0,0 +1,104 @@ +#ifndef CONTROLCAN_H +#define CONTROLCAN_H + +////文件版本：v2.02 20190609 +//接口卡类型定义 + +#define VCI_USBCAN1 3 +#define VCI_USBCAN2 4 +#define VCI_USBCAN2A 4 + +#define VCI_USBCAN_E_U 20 +#define VCI_USBCAN_2E_U 21 + +//函数调用返回状态值 +#define STATUS_OK 1 +#define STATUS_ERR 0 + +#define USHORT unsigned short int +#define BYTE unsigned char +#define CHAR char +#define UCHAR unsigned char +#define UINT unsigned int +#define DWORD unsigned int +#define PVOID void* +#define ULONG unsigned int +#define INT int +#define UINT32 UINT +#define LPVOID void* +#define BOOL BYTE +#define TRUE 1 +#define FALSE 0 + +//1.ZLGCAN系列接口卡信息的数据类型。 +typedef struct _VCI_BOARD_INFO{ + USHORT hw_Version; + USHORT fw_Version; + USHORT dr_Version; + USHORT in_Version; + USHORT irq_Num; + BYTE can_Num; + CHAR str_Serial_Num[20]; + CHAR str_hw_Type[40]; + USHORT Reserved[4]; +} VCI_BOARD_INFO,*PVCI_BOARD_INFO; + +//2.定义CAN信息帧的数据类型。 +typedef struct _VCI_CAN_OBJ{ + UINT ID; + UINT TimeStamp; + BYTE TimeFlag; + BYTE SendType; + BYTE RemoteFlag;//是否是远程帧 + BYTE ExternFlag;//是否是扩展帧 + BYTE DataLen; + BYTE Data[8]; + BYTE Reserved[3]; +}VCI_CAN_OBJ,*PVCI_CAN_OBJ; + +//3.定义初始化CAN的数据类型 +typedef struct _INIT_CONFIG{ + DWORD AccCode; + DWORD AccMask; + DWORD Reserved; + UCHAR Filter; + UCHAR Timing0; + UCHAR Timing1; + UCHAR Mode; +}VCI_INIT_CONFIG,*PVCI_INIT_CONFIG; + +///////// new add struct for filter ///////// +typedef struct _VCI_FILTER_RECORD{ + DWORD ExtFrame; //是否为扩展帧 + DWORD Start; + DWORD End; +}VCI_FILTER_RECORD,*PVCI_FILTER_RECORD; + +#ifdef __cplusplus +#define EXTERN_C extern "C" +#else +#define EXTERN_C +#endif + +EXTERN_C DWORD VCI_OpenDevice(DWORD DeviceType,DWORD DeviceInd,DWORD Reserved); +EXTERN_C DWORD VCI_CloseDevice(DWORD DeviceType,DWORD DeviceInd); +EXTERN_C DWORD VCI_InitCAN(DWORD DeviceType, DWORD DeviceInd, DWORD CANInd, PVCI_INIT_CONFIG pInitConfig); + +EXTERN_C DWORD VCI_ReadBoardInfo(DWORD DeviceType,DWORD DeviceInd,PVCI_BOARD_INFO pInfo); + +EXTERN_C DWORD VCI_SetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData); + +EXTERN_C ULONG VCI_GetReceiveNum(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd); +EXTERN_C DWORD VCI_ClearBuffer(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd); + +EXTERN_C DWORD VCI_StartCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd); +EXTERN_C DWORD VCI_ResetCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd); + +EXTERN_C ULONG VCI_Transmit(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pSend,UINT Len); +EXTERN_C ULONG VCI_Receive(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pReceive,UINT Len,INT WaitTime); + +EXTERN_C DWORD VCI_UsbDeviceReset(DWORD DevType,DWORD DevIndex,DWORD Reserved); +EXTERN_C DWORD VCI_FindUsbDevice2(PVCI_BOARD_INFO pInfo); + +#endif + diff --git a/src/mc/lib/libcontrolcan.a b/src/mc/lib/libcontrolcan.a new file mode 100755 index 0000000..bb8aee0 Binary files /dev/null and b/src/mc/lib/libcontrolcan.a differ diff --git a/src/mc/lib/libcontrolcan.so b/src/mc/lib/libcontrolcan.so new file mode 100755 index 0000000..75a24ab Binary files /dev/null and b/src/mc/lib/libcontrolcan.so differ diff --git a/src/mc/src/can_driver.cpp b/src/mc/src/can_driver.cpp index 3af1f9f..d6d4ffb 100644 --- a/src/mc/src/can_driver.cpp +++ b/src/mc/src/can_driver.cpp @@ -9,6 +9,11 @@ #include #include #include +#include "mc/controlcan.h" + +DWORD deviceType = VCI_USBCAN2; // 或 VCI_USBCAN_2E_U 等 +DWORD deviceIndex = 0; // 通常为 0（第一个设备） +DWORD channelIndex = 0; // 通道号：0 表示 CAN1，1 表示 CAN2 CANDriver::CANDriver() = default; @@ -22,39 +27,39 @@ bool CANDriver::open(const std::string &interface) if (running) return false; - sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW); - if (sockfd < 0) + // 参数可解析 interface 设定 channelIndex，例如 interface="can0" => channelIndex=0 + deviceType = VCI_USBCAN2; + deviceIndex = 0; + channelIndex = (interface == "can1") ? 1 : 0; + + if (VCI_OpenDevice(deviceType, deviceIndex, 0) != STATUS_OK) { - perror("socket"); + std::cerr << "VCI_OpenDevice failed" << std::endl; return false; } - struct ifreq ifr; - std::strncpy(ifr.ifr_name, interface.c_str(), IFNAMSIZ - 1); - ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + VCI_INIT_CONFIG config{}; + config.AccCode = 0; + config.AccMask = 0xFFFFFFFF; + config.Filter = 0; // 接收所有 + config.Timing0 = 0x00; // 500Kbps 示例（需查文档确认 Timing） + config.Timing1 = 0x1C; + config.Mode = 0; // 正常模式 - if (ioctl(sockfd, SIOCGIFINDEX, &ifr) < 0) + if (VCI_InitCAN(deviceType, deviceIndex, channelIndex, &config) != STATUS_OK) { - perror("ioctl"); - ::close(sockfd); + std::cerr << "VCI_InitCAN failed" << std::endl; + VCI_CloseDevice(deviceType, deviceIndex); return false; } - struct sockaddr_can addr{}; - addr.can_family = AF_CAN; - addr.can_ifindex = ifr.ifr_ifindex; - - if (bind(sockfd, reinterpret_cast(&addr), sizeof(addr)) < 0) + if (VCI_StartCAN(deviceType, deviceIndex, channelIndex) != STATUS_OK) { - perror("bind"); - ::close(sockfd); + std::cerr << "VCI_StartCAN failed" << std::endl; + VCI_CloseDevice(deviceType, deviceIndex); return false; } - // 设置为非阻塞 - int flags = fcntl(sockfd, F_GETFL, 0); - fcntl(sockfd, F_SETFL, flags | O_NONBLOCK); - running = true; receiveThread = std::thread(&CANDriver::receiveThreadFunc, this); return true; @@ -69,11 +74,8 @@ void CANDriver::close() if (receiveThread.joinable()) receiveThread.join(); - if (sockfd >= 0) - { - ::close(sockfd); - sockfd = -1; - } + VCI_ResetCAN(deviceType, deviceIndex, channelIndex); + VCI_CloseDevice(deviceType, deviceIndex); } bool CANDriver::sendFrame(const CANFrame &frame) @@ -81,18 +83,17 @@ bool CANDriver::sendFrame(const CANFrame &frame) if (!running) return false; - struct can_frame raw_frame{}; - raw_frame.can_id = frame.id; - if (frame.ext) - raw_frame.can_id |= CAN_EFF_FLAG; - if (frame.rtr) - raw_frame.can_id |= CAN_RTR_FLAG; - raw_frame.can_dlc = frame.dlc; - std::memcpy(raw_frame.data, frame.data, frame.dlc); + VCI_CAN_OBJ obj{}; + obj.ID = frame.id; + obj.RemoteFlag = frame.rtr ? 1 : 0; + obj.ExternFlag = frame.ext ? 1 : 0; + obj.SendType = 0; // 普通发送 + obj.DataLen = frame.dlc; + std::memcpy(obj.Data, frame.data, frame.dlc); - if (write(sockfd, &raw_frame, sizeof(raw_frame)) != sizeof(raw_frame)) + if (VCI_Transmit(deviceType, deviceIndex, channelIndex, &obj, 1) != 1) { - perror("write"); + std::cerr << "VCI_Transmit failed" << std::endl; return false; } @@ -149,25 +150,24 @@ bool CANDriver::applyFilters() void CANDriver::receiveThreadFunc() { - struct can_frame raw_frame; while (running) { - ssize_t nbytes = read(sockfd, &raw_frame, sizeof(raw_frame)); - if (nbytes < 0) + VCI_CAN_OBJ rec[100]; + ULONG len = VCI_Receive(deviceType, deviceIndex, channelIndex, rec, 100, 10); + if (len == 0) { - if (errno != EAGAIN) - perror("read"); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); continue; } - if (nbytes == sizeof(raw_frame) && callback) + for (ULONG i = 0; i < len && callback; ++i) { CANFrame frame; - frame.id = raw_frame.can_id & CAN_EFF_MASK; - frame.ext = static_cast(raw_frame.can_id & CAN_EFF_FLAG); - frame.rtr = static_cast(raw_frame.can_id & CAN_RTR_FLAG); - frame.dlc = raw_frame.can_dlc; - std::memcpy(frame.data, raw_frame.data, raw_frame.can_dlc); + frame.id = rec[i].ID; + frame.ext = rec[i].ExternFlag != 0; + frame.rtr = rec[i].RemoteFlag != 0; + frame.dlc = rec[i].DataLen; + std::memcpy(frame.data, rec[i].Data, frame.dlc); callback(frame, userData); } } diff --git a/src/radio_ctrl/src/radio_ctrl.cpp b/src/radio_ctrl/src/radio_ctrl.cpp index c3017d7..367b302 100644 --- a/src/radio_ctrl/src/radio_ctrl.cpp +++ b/src/radio_ctrl/src/radio_ctrl.cpp @@ -8,7 +8,8 @@ namespace sweeperMsg = sweeper_interfaces::msg; -constexpr float EPS_GEAR_RATIO = 16.5f; +constexpr float DEG_PER_SEC_TO_RPM = 1.0f / 6.0f; +constexpr float GEAR_RATIO = 7.0f; constexpr float DELTA_T = 0.02f; // 20ms const uint16_t speed[3] = {192, 992, 1792}; @@ -128,7 +129,7 @@ private: float angle_speed = fabs(target_angle - current_feedback_angle) / DELTA_T; // 电机转速（单位：rpm） - float motor_rpm = angle_speed * EPS_GEAR_RATIO; + float motor_rpm = angle_speed * DEG_PER_SEC_TO_RPM * GEAR_RATIO; // 限制电机转速到 [120, 1500] 范围，防止过小/过大 uint16_t can_speed = std::clamp(