#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <string>
#include <cstring>
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>
#include <cmath>
class UARTController {
public:
int setUART(int fd, int nSpeed, int nBits, char nEvent, int nStop);
uint8_t CRC8(const uint8_t *data, uint16_t length);
void DDSMIDSet(uint8_t id);
void DDSMModeSet(uint8_t id, uint8_t mode);
void DDSMShache(uint8_t id);
void DDSM_run(uint8_t id1, uint8_t id2,int16_t ctrl_value1,int16_t ctrl_value2, uint8_t speedUp_Time1,uint8_t speedUp_Time2, uint8_t lock_Car1,uint8_t lock_Car2);
int serverIOCTL_Init(const std::string& gpioIndex);
int serverIOCTL_On(const std::string& gpioIndex);
int serverIOCTL_Off(const std::string& gpioIndex);
int serverIOCTL_Exit(const std::string& gpioIndex);
int RS485Fa(uint8_t id, const uint8_t *data, size_t length);
void clearInputBuffer();
void configureUARTAndGPIO(int fd, const std::string& gpioIndex);
void receiveData(int fd, const std::string& gpioIndex, std::string& receivedData);
private:
static constexpr const char* SERVER_GPIO_INDEX_1 = "119"; // RS485-1 流控引脚编号
static constexpr const char* SERVER_GPIO_INDEX_2 = "120"; // RS485-2 流控引脚编号
static constexpr const char* defaultPath1 = "/dev/ttyS4"; // RS485-1 流控引脚编号
static constexpr const char* defaultPath2 = "/dev/ttyS7"; // RS485-2 流控引脚编号
// 辅助函数:RAII 文件管理器
struct FileCloser {
int fd;
explicit FileCloser(int f) : fd(f) {}
~FileCloser() { if (fd >= 0) close(fd); }
operator bool() const { return fd >= 0; } // 允许隐式转换为布尔值
int get() const { return fd; } // 获取文件描述符
};
std::unique_ptr<FileCloser> openFile(const std::string& path, int flags) {
int fd = open(path.c_str(), flags);
if (fd < 0) {
throw std::runtime_error("Failed to open file: " + path);
}
return std::make_unique<FileCloser>(fd);
}
};
const double WHEEL_RADIUS = 0.05; // 轮子半径 (米)
const double WHEEL_BASE = 0.25; // 两轮之间的距离 (米)
const double RPM_TO_RAD_PER_SEC = 2 * M_PI / 60; // 将rpm转换为rad/s的比例因子
// 实现CRC8计算
uint8_t UARTController::CRC8(const uint8_t *data, uint16_t length) {
unsigned int i;
uint8_t crc = 0;
while(length--)
{
crc ^= *data++;
for (i = 0; i < 8; i++)
{
if (crc & 0x01)
crc = (crc >> 1) ^ 0x8C;
else
crc >>= 1;
}
}
return crc;
}
// 设置UART参数
int UARTController::setUART(int fd, int nSpeed, int nBits, char nEvent, int nStop) {
struct termios opt;
// 清除输入输出缓冲区
if (tcflush(fd, TCIOFLUSH) == -1) {
std::cerr << "Failed to flush buffers" << std::endl;
return -1;
}
// 获取当前串口属性
if (tcgetattr(fd, &opt) == -1) {
std::cerr << "Failed to get attributes" << std::endl;
return -1;
}
// 清除波特率、校验位等设置
opt.c_cflag &= (~CBAUD);
opt.c_cflag &= (~PARENB);
opt.c_iflag &= (~ICRNL); // 禁用 CR 到 NL 的转换
opt.c_lflag &= (~ECHO); // 禁用回显
// 设置波特率
switch (nSpeed) {
case 2400: cfsetspeed(&opt, B2400); break;
case 4800: cfsetspeed(&opt, B4800); break;
case 9600: cfsetspeed(&opt, B9600); break;
case 38400: cfsetspeed(&opt, B38400); break;
case 115200: cfsetspeed(&opt, B115200); break;
default:
std::cerr << "Unsupported baud rate" << std::endl;
return -1;
}
// 设置数据位
switch (nBits) {
case 7: opt.c_cflag |= CS7; break;
case 8: opt.c_cflag |= CS8; break;
default:
std::cerr << "Unsupported data bits" << std::endl;
return -1;
}
// 设置校验位
switch (nEvent) {
case 'n': case 'N': opt.c_cflag &= (~PARENB); break; // 无校验
case 'o': case 'O': opt.c_cflag |= PARODD; break; // 奇校验
case 'e': case 'E':
opt.c_cflag |= PARENB;
opt.c_cflag &= (~PARODD);
break; // 偶校验
default:
std::cerr << "Unsupported parity" << std::endl;
return -1;
}
// 设置停止位
switch (nStop) {
case 1: opt.c_cflag &= ~CSTOPB; break; // 1 停止位
case 2: opt.c_cflag |= CSTOPB; break; // 2 停止位
default:
std::cerr << "Unsupported stop bits" << std::endl;
return -1;
}
// 应用新的串口属性
if (tcsetattr(fd, TCSANOW, &opt) == -1) {
std::cerr << "Failed to set attributes" << std::endl;
return -1;
}
return 0;
}
// 初始化GPIO流控引脚
int UARTController::serverIOCTL_Init(const std::string& gpioIndex) {
try {
auto exportFile = openFile("/sys/class/gpio/export", O_WRONLY);
write(exportFile->get(), gpioIndex.c_str(), gpioIndex.size());
std::string directionPath = "/sys/class/gpio/gpio" + gpioIndex + "/direction";
auto directionFile = openFile(directionPath, O_WRONLY);
write(directionFile->get(), "out", strlen("out"));
return 0;
} catch (const std::runtime_error& e) {
std::cerr << e.what() << std::endl;
return 1;
}
}
// 打开GPIO
int UARTController::serverIOCTL_On(const std::string& gpioIndex) {
try {
std::string valuePath = "/sys/class/gpio/gpio" + gpioIndex + "/value";
auto valueFile = openFile(valuePath, O_WRONLY);
ssize_t result = write(valueFile->get(), "1", 1);
if (result == -1) {
std::cerr << "Error in write: " << strerror(errno) << std::endl; // 处理错误[^4]
}
return 0;
} catch (const std::runtime_error& e) {
std::cerr << e.what() << std::endl;
return 1;
}
}
// 关闭GPIO
int UARTController::serverIOCTL_Off(const std::string& gpioIndex) {
try {
std::string valuePath = "/sys/class/gpio/gpio" + gpioIndex + "/value";
auto valueFile = openFile(valuePath, O_WRONLY);
write(valueFile->get(), "0", 1);
return 0;
} catch (const std::runtime_error& e) {
std::cerr << e.what() << std::endl;
return 1;
}
}
// 退出GPIO
int UARTController::serverIOCTL_Exit(const std::string& gpioIndex) {
try {
auto unexportFile = openFile("/sys/class/gpio/unexport", O_WRONLY);
write(unexportFile->get(), gpioIndex.c_str(), gpioIndex.size());
return 0;
} catch (const std::runtime_error& e) {
std::cerr << e.what() << std::endl;
return 1;
}
}
// 清除输入缓冲区
void UARTController::clearInputBuffer() {
tcflush(0, TCIFLUSH);
}
// 配置UART和GPIO
void UARTController::configureUARTAndGPIO(int fd, const std::string &gpioIndex)
{
setUART(fd, 115200, 8, 'n', 1);
serverIOCTL_Init(gpioIndex);
serverIOCTL_Off(gpioIndex); // 设置为接收模式
}
// 接收数据
void UARTController::receiveData(int fd, const std::string &gpioIndex, std::string &receivedData)
{
char buffer[1024] = {0};
ssize_t bytesRead = read(fd, buffer, sizeof(buffer));
if (bytesRead > 0) {
receivedData.assign(buffer, bytesRead);
std::cout << "Received data (" << bytesRead << " bytes): " << receivedData << std::endl;
} else {
std::cout << "No data received." << std::endl;
}
}
//两路rs485发送函数
int UARTController::RS485Fa(uint8_t id, const uint8_t *data, size_t length)
{
const char* defaultPath;
const char* gpioIndex;
if(id==1)
{
defaultPath=defaultPath1;
gpioIndex=SERVER_GPIO_INDEX_1;
}
if(id==2)
{
defaultPath=defaultPath2;
gpioIndex=SERVER_GPIO_INDEX_2;
}
try {
auto file = openFile(defaultPath, O_RDWR);
setUART(file->get(), 115200, 8, 'n', 1);
if (serverIOCTL_Init(gpioIndex) != 0) {
throw std::runtime_error("GPIO initialization failed");
}
if (serverIOCTL_On(gpioIndex) != 0) {
throw std::runtime_error("Failed to enable GPIO");
}
write(file->get(), data, length);
usleep(100000); // 等待100ms
serverIOCTL_Exit(gpioIndex);
} catch (const std::exception& e) {
std::cerr << "Errwor in RS485Fa: " << e.what() << std::endl;
return -1;
}
return 0;
}
//电机id设置,设置ID时请保证总线上只有一个电机,每次上电只允许设置一次,电机接收到5次ID设置指令后进行设置。
void UARTController::DDSMIDSet(uint8_t id)
{
const char* defaultPath = (id == 1) ? defaultPath1 : defaultPath2;
const char* gpioIndex = (id == 1) ? SERVER_GPIO_INDEX_1 : SERVER_GPIO_INDEX_2;
try {
auto file = openFile(defaultPath, O_RDWR);
if (!file) throw std::runtime_error("Failed to open file");
if (setUART(file->get(), 115200, 8, 'n', 1) != 0) {
throw std::runtime_error("Failed to set UART parameters");
}
if (serverIOCTL_Init(gpioIndex) != 0) {
throw std::runtime_error("GPIO initialization failed");
}
if (serverIOCTL_On(gpioIndex) != 0) {
throw std::runtime_error("Failed to enable GPIO");
}
uint8_t data[10] = {0xAA, 0x55, 0x53, id, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
data[9] = CRC8(data, 9); // 计算并设置CRC校验值
for (int i = 0; i < 5; ++i) {
if (RS485Fa(file->get(), data, sizeof(data)) != 0) {
throw std::runtime_error("Failed to send data over RS485");
}
std::cout << "Sent data: ";
for (size_t j = 0; j < sizeof(data); ++j) {
std::cout << "0x" << std::hex << (int)data[j] << " ";
}
std::cout << std::dec << std::endl;
usleep(100000); // 等待10ms
}
serverIOCTL_Off(gpioIndex);
serverIOCTL_Exit(gpioIndex);
} catch (const std::exception& e) {
std::cerr << "Error: " << e.what() << std::endl;
}
}
//电机模式设置 0x01:设定为电流环 0x02:设定为速度环 0x03:设定为位置环
void UARTController::DDSMModeSet(uint8_t id, uint8_t mode)
{
uint8_t data[10]={id,0xA0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,mode};
RS485Fa(id,data,sizeof(data));
}
//电机刹车
void UARTController::DDSMShache(uint8_t id)
{
uint8_t data1[9]={id,0x64,0x00,0x00,0x00,0x00,0x00,0XFF,0x00};
uint8_t crc;
crc=CRC8(data1,sizeof(data1));
uint8_t data[10]={id,0x64,0x00,0x00,0x00,0x00,0x00,0XFF,0x00,crc};
RS485Fa(id,data,sizeof(data));
}
//电机运行,id:电机编号 ctrl_value:在当面模式下的控制值 speedUp_Time:加速时间 lock_Car:刹车 0xff生效 速度环模式下使用
void UARTController::DDSM_run(uint8_t id1, uint8_t id2,int16_t ctrl_value1,int16_t ctrl_value2, uint8_t speedUp_Time1,uint8_t speedUp_Time2, uint8_t lock_Car1,uint8_t lock_Car2)
{
uint8_t crc1;
uint8_t crc2;
uint8_t data1[9]={id1,0x64,(uint8_t)(ctrl_value1>>8),(uint8_t)(ctrl_value1),0x00,0x00,speedUp_Time1,lock_Car1,0x00};
crc1=CRC8(data1,sizeof(data1));
uint8_t data2[10]={id1,0x64,(uint8_t)(ctrl_value1>>8),(uint8_t)(ctrl_value1),0x00,0x00,speedUp_Time1,lock_Car1,0x00,crc1};
uint8_t data3[9]={id2,0x64,(uint8_t)(-ctrl_value2>>8),(uint8_t)(-ctrl_value2),0x00,0x00,speedUp_Time2,lock_Car2,0x00};
crc2=CRC8(data3,sizeof(data3));
uint8_t data4[10]={id2,0x64,(uint8_t)(-ctrl_value2>>8),(uint8_t)(-ctrl_value2),0x00,0x00,speedUp_Time2,lock_Car2,0x00,crc2};
RS485Fa(id1,data2,sizeof(data2));
RS485Fa(id2,data4,sizeof(data4));
}
// 函数:逆运动学计算
void inverseKinematics(double linearVelocity, double angularVelocity, double &leftWheelSpeed, double &rightWheelSpeed) {
// 计算左右轮速度
leftWheelSpeed = linearVelocity - (angularVelocity * WHEEL_BASE )/ 2;
rightWheelSpeed = linearVelocity + (angularVelocity * WHEEL_BASE ) /2;
}
//将线速度转换成rpm
double lcr(double lineSpeed){
double angularspeed = lineSpeed / WHEEL_RADIUS;
double motorRpm = angularspeed / RPM_TO_RAD_PER_SEC;
if (motorRpm < -255)
{
motorRpm = -255;
}
if (motorRpm > 255)
{
motorRpm = 255;
}
return motorRpm;
}
//回调函数
void callback(const geometry_msgs::Twist::ConstPtr& twist)
{
UARTController controller;
double linearVelocity,angularVelocity,leftWheelSpeed,rightWheelSpeed,leftspeed,rightspeed,lastleft,lastright;
linearVelocity = twist->linear.x;
angularVelocity = twist->angular.z;
inverseKinematics(linearVelocity,angularVelocity,leftWheelSpeed,rightWheelSpeed);
leftspeed = lcr(leftWheelSpeed);
rightspeed = lcr(rightWheelSpeed);
if(leftspeed== rightspeed)
{
lastleft = leftspeed;
lastright = rightspeed;
controller.DDSM_run(1,2,leftspeed,rightspeed ,0,0,0,0);
}else if(leftspeed != rightspeed)
{
controller.DDSM_run(1,2,leftspeed,rightspeed ,0,0,0,0);
usleep(1000000);
controller.DDSM_run(1,2,lastleft,lastright,0,0,0,0);
}
}
int main(int argc, char *argv[])
{
UARTController controller;
ros::init(argc,argv,"action_subscriber");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/cmd_vel",10,callback);
ros::spin();
controller.DDSM_run(1,2,0,0,0,0,0,0);
return 0;
}
该代码在运行中为什么不能访问gpio