cxh/sweeper_e800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Auto commit at 2025-06-12 14:41:07

Browse Source
This commit is contained in:
cxh 2025-06-12 14:41:07 +08:00
parent 2f81d24592
commit 7fc660c798
6 changed files with 166 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/mc/CMakeLists.txt
View File

@ -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)

104
src/mc/include/mc/controlcan.h Executable file
View File

@ -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

BIN
src/mc/lib/libcontrolcan.a Executable file
View File

Binary file not shown.

BIN
src/mc/lib/libcontrolcan.so Executable file
View File

Binary file not shown.

92
src/mc/src/can_driver.cpp
View File

@ -9,6 +9,11 @@
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#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<struct sockaddr *>(&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<bool>(raw_frame.can_id & CAN_EFF_FLAG);
frame.rtr = static_cast<bool>(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);
}
}

5
src/radio_ctrl/src/radio_ctrl.cpp
View File

@ -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(