From 80b5d9e1c8fa0fec8f3806da492049b10dfc96f8 Mon Sep 17 00:00:00 2001 From: We-unite <3205135446@qq.com> Date: Wed, 19 Jun 2024 13:43:36 +0800 Subject: update --- serial/CMakeLists.txt | 6 +- serial/serial.c | 203 +++++++++++++++++++++++++++----------------------- serial/serial.h | 36 +++++++++ serial/wheel.c | 70 +++++++++++++++++ udp2lcm/udp2lcm.c | 56 ++++++++++---- 5 files changed, 261 insertions(+), 110 deletions(-) create mode 100644 serial/serial.h create mode 100644 serial/wheel.c diff --git a/serial/CMakeLists.txt b/serial/CMakeLists.txt index 55415f3..5ded98c 100644 --- a/serial/CMakeLists.txt +++ b/serial/CMakeLists.txt @@ -1,7 +1,11 @@ cmake_minimum_required(VERSION 3.10) project(serial) -add_executable(serial serial.c ${CMAKE_SOURCE_DIR}/lcmtype/path_ctrl_t.c) +# 设置源文件为本文件夹下的所有.c .cpp文件以及lcmtype文件夹下的所有.c .cpp文件 +aux_source_directory(. DIR_SRCS) +aux_source_directory(${CMAKE_SOURCE_DIR}/lcmtype DIR_SRCS) + +add_executable(serial ${DIR_SRCS}) # 添加链接库 target_link_libraries(serial lcm) \ No newline at end of file diff --git a/serial/serial.c b/serial/serial.c index db45fac..57b432c 100644 --- a/serial/serial.c +++ b/serial/serial.c @@ -1,130 +1,147 @@ -#include "path_ctrl_t.h" -#include -#include -#include -#include -#include -#include -#include -#include +#include "serial.h" -typedef unsigned char byte; -#define MAX_BUFFER_SIZE 1024 -#define DEFAULT_SPEED 0x15 +lcm_t *lcm; -int fd; // 轮子的串口文件描述符 -char portname[50] = "/dev/ttyUSB0"; // 串口设备名 -char clientIP[20] = ""; -struct termios tty; - -bool whellInit(); -bool wheelSend(byte a, byte a_v, byte b, byte b_v); -void speedControl(byte status, byte speed); -void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel, - const path_ctrl_t *msg, void *userdata); +int curStatus = -1, curSpeed = 0; // 这里的速度指百分比速度 +double curOmega; +// 临界区数据 +pthread_mutex_t curPoseMutex; +clock_t lastTime; +double curPose[3] = {0, 0, 0}; int main() { if (!whellInit()) { goto err; } - lcm_t *path_lcm = lcm_create(NULL); - if (!path_lcm) { + lcm = lcm_create(NULL); + if (!lcm) { fprintf(stderr, "Failed to create LCM\n"); goto err; } - path_ctrl_t cmd; - path_ctrl_t_subscribe(path_lcm, "wheel_ctrl", parseCmd, &cmd); + + // 订阅来自手机端的轮控消息 + path_ctrl_t_subscribe(lcm, "wheel_ctrl", parseCmd, NULL); + // 订阅来自算法模块的路径规划消息 + path_t_subscribe(lcm, "PATH", parsePath, NULL); + // 来自算法模块的当前位置信息 + pthread_mutex_init(&curPoseMutex, NULL); + pose_t_subscribe(lcm, "CURRENTPOSE", setCurPose, NULL); + // 新线程，定时发送当前位置信息 + pthread_t thread; + if (pthread_create(&thread, NULL, sendCurPose, NULL) != 0) { + fprintf(stderr, "Error: Failed to create thread\n"); + goto err; + } while (true) { - lcm_handle(path_lcm); + lcm_handle(lcm); } err: - close(fd); + // close(fd); return 0; } -bool whellInit() { - // 打开串口 - fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC); - if (fd < 0) { - fprintf(stderr, "Error opening %s: %s\n", portname, strerror(errno)); - return false; - } - - // 设置串口参数 - memset(&tty, 0, sizeof tty); - if (tcgetattr(fd, &tty) != 0) { - fprintf(stderr, "Error from tcgetattr: %s\n", strerror(errno)); - return false; - } - - cfsetospeed(&tty, B115200); // 设置输出波特率为115200 - cfsetispeed(&tty, B115200); // 设置输入波特率为115200 - - tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars - tty.c_iflag &= ~IGNBRK; // ignore break signal - tty.c_lflag = 0; // no signaling chars, no echo, - // no canonical processing - tty.c_oflag = 0; // no remapping, no delays - tty.c_cc[VMIN] = 0; // read doesn't block - tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout - - tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl - tty.c_cflag |= (CLOCAL | CREAD); // ignore modem controls, - // enable reading - tty.c_cflag &= ~(PARENB | PARODD); // shut off parity - tty.c_cflag |= 0; - tty.c_cflag &= ~CSTOPB; - tty.c_cflag &= ~CRTSCTS; - - if (tcsetattr(fd, TCSANOW, &tty) != 0) { - fprintf(stderr, "Error from tcsetattr: %s\n", strerror(errno)); - return false; - } - return true; -} - -bool wheelSend(byte a, byte a_v, byte b, byte b_v) { - unsigned char data[7] = {0x53, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00}; - byte checksum = 0; - data[2] = a; - data[3] = a_v; - data[4] = b; - data[5] = b_v; - - for (int i = 0; i < 6; i++) { - checksum ^= data[i]; - } - data[6] = checksum; - - // 发送数据 - if (write(fd, data, 7) != 7) { - fprintf(stderr, "Failed to write to the serial port\n"); - return false; - } - - printf("Data %02X %02X %02X %02X %02X %02X %02X wheelSend successfully!\n", - data[0], data[1], data[2], data[3], data[4], data[5], data[6]); - return true; -} - void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel, const path_ctrl_t *msg, void *userdata) { byte status = msg->cmd, speed = msg->speed; + if (curStatus == status && curSpeed == speed) { + return; + } + + // 状态改变，计算新的位置 + renewCurPose(); + + // 下发新的指令 switch (status) { case 1: wheelSend(0x01, speed, 0x01, speed); + curSpeed = speed; + curOmega = 0; break; case 2: wheelSend(0x02, DEFAULT_SPEED, 0x01, DEFAULT_SPEED); + curOmega = (double)DEFAULT_SPEED * FULLSPEED / 100 / RADIUS; + curSpeed = 0; break; case 3: wheelSend(0x01, DEFAULT_SPEED, 0x02, DEFAULT_SPEED); + curOmega = (double)-DEFAULT_SPEED * FULLSPEED / 100 / RADIUS; + curSpeed = 0; break; case 0: default: wheelSend(0x00, 0x00, 0x00, 0x00); + curOmega = 0; + curSpeed = 0; break; } + // 更新状态 + curStatus = status; +} + +void parsePath(const lcm_recv_buf_t *rbuf, const char *channel, + const path_t *msg, void *userdata) { + int16_t length = msg->length; + double v, w; + int8_t vWheels[2]; + int8_t bWheels[2]; + if (length <= 0) { + fprintf(stderr, "Error: Invalid path\n"); + return; + } + v = msg->xyr[0][0]; + w = msg->xyr[0][1]; + if (fabs(v) <= 0.01 && fabs(w) <= 0.01) { + wheelSend(0x00, 0x00, 0x00, 0x00); + renewCurPose(); + curSpeed = 0; + curOmega = 0; + } else { + vWheels[0] = (int8_t)((double)(v - w * RADIUS) / FULLSPEED * 100); + vWheels[1] = (int8_t)((double)(v + w * RADIUS) / FULLSPEED * 100); + bWheels[0] = vWheels[0] > 0 ? 0x01 : 0x02; + bWheels[1] = vWheels[1] > 0 ? 0x01 : 0x02; + wheelSend(bWheels[0], fabs(vWheels[0]), bWheels[1], fabs(vWheels[1])); + renewCurPose(); + curSpeed = (int8_t)((double)v / FULLSPEED * 100); + curOmega = (int8_t)w; + } +} + +void setCurPose(const lcm_recv_buf_t *rbuf, const char *channel, + const pose_t *msg, void *userdata) { + pthread_mutex_lock(&curPoseMutex); + curPose[0] = msg->pos[0]; + curPose[1] = msg->pos[1]; + curPose[2] = msg->pos[2]; + lastTime = clock(); + pthread_mutex_unlock(&curPoseMutex); +} + +void sendCurPose() { + pose_t pose; + while (true) { + renewCurPose(); + pthread_mutex_lock(&curPoseMutex); + pose.pos[0] = curPose[0]; + pose.pos[1] = curPose[1]; + pose.pos[2] = curPose[2]; + pthread_mutex_unlock(&curPoseMutex); + pose_t_publish(lcm, "POSE", &pose); + usleep(16); + } +} + +void renewCurPose() { + clock_t curTime = clock(); + double dt = (double)(curTime - lastTime) / CLOCKS_PER_SEC; + double speed = (double)curSpeed * FULLSPEED / 100; + + pthread_mutex_lock(&curPoseMutex); + curPose[0] += speed * cos(curPose[2]) * dt; + curPose[1] += speed * sin(curPose[2]) * dt; + curPose[2] += curOmega * dt; + lastTime = curTime; + pthread_mutex_unlock(&curPoseMutex); } \ No newline at end of file diff --git a/serial/serial.h b/serial/serial.h new file mode 100644 index 0000000..8061b24 --- /dev/null +++ b/serial/serial.h @@ -0,0 +1,36 @@ +#ifndef SERIAL_H +#define SERIAL_H + +#include "path_ctrl_t.h" +#include "path_t.h" +#include "pose_t.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define MAX_BUFFER_SIZE 1024 +#define DEFAULT_SPEED 0x15 // 默认速度，百分比 +// TODO: 两个待测数据 +#define RADIUS 0.3 // 两轮之间的距离的一半，米 +#define FULLSPEED 1.5 // 轮子全速，m/s +#define fabs(x) ((x) > 0 ? (x) : -(x)) +typedef unsigned char byte; + +bool whellInit(); +bool wheelSend(byte a, byte a_v, byte b, byte b_v); +void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel, + const path_ctrl_t *msg, void *userdata); +void parsePath(const lcm_recv_buf_t *rbuf, const char *channel, + const path_t *msg, void *userdata); +void setCurPose(const lcm_recv_buf_t *rbuf, const char *channel, + const pose_t *msg, void *userdata); +void renewCurPose(); +void sendCurPose(); + +#endif \ No newline at end of file diff --git a/serial/wheel.c b/serial/wheel.c new file mode 100644 index 0000000..936f55c --- /dev/null +++ b/serial/wheel.c @@ -0,0 +1,70 @@ +#include "serial.h" + +int fd; // 轮子的串口文件描述符 +char portname[50] = "/dev/ttyUSB0"; // 串口设备名 +struct termios tty; + +bool whellInit() { + // 打开串口 + fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC); + if (fd < 0) { + fprintf(stderr, "Error opening %s: %s\n", portname, strerror(errno)); + return false; + } + + // 设置串口参数 + memset(&tty, 0, sizeof tty); + if (tcgetattr(fd, &tty) != 0) { + fprintf(stderr, "Error from tcgetattr: %s\n", strerror(errno)); + return false; + } + + cfsetospeed(&tty, B115200); // 设置输出波特率为115200 + cfsetispeed(&tty, B115200); // 设置输入波特率为115200 + + tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars + tty.c_iflag &= ~IGNBRK; // ignore break signal + tty.c_lflag = 0; // no signaling chars, no echo, + // no canonical processing + tty.c_oflag = 0; // no remapping, no delays + tty.c_cc[VMIN] = 0; // read doesn't block + tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout + + tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl + tty.c_cflag |= (CLOCAL | CREAD); // ignore modem controls, + // enable reading + tty.c_cflag &= ~(PARENB | PARODD); // shut off parity + tty.c_cflag |= 0; + tty.c_cflag &= ~CSTOPB; + tty.c_cflag &= ~CRTSCTS; + + if (tcsetattr(fd, TCSANOW, &tty) != 0) { + fprintf(stderr, "Error from tcsetattr: %s\n", strerror(errno)); + return false; + } + return true; +} + +bool wheelSend(byte a, byte a_v, byte b, byte b_v) { + unsigned char data[7] = {0x53, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00}; + byte checksum = 0; + data[2] = a; + data[3] = a_v; + data[4] = b; + data[5] = b_v; + + for (int i = 0; i < 6; i++) { + checksum ^= data[i]; + } + data[6] = checksum; + + // 发送数据 + if (write(fd, data, 7) != 7) { + fprintf(stderr, "Failed to write to the serial port\n"); + return false; + } + + printf("Data %02X %02X %02X %02X %02X %02X %02X wheelSend successfully!\n", + data[0], data[1], data[2], data[3], data[4], data[5], data[6]); + return true; +} \ No newline at end of file diff --git a/udp2lcm/udp2lcm.c b/udp2lcm/udp2lcm.c index fdf32c8..76f41b0 100644 --- a/udp2lcm/udp2lcm.c +++ b/udp2lcm/udp2lcm.c @@ -1,16 +1,6 @@ // Encoded in UTF-8 - -/* - * struct path_ctrl_t - * { - * int8_t cmd; - * int8_t speed; - * } - * 这是与轮控模块通信的数据结构，speed为0-100表示速度 - * cmd为0停，1前，2左，3右 - */ - #include "udp2lcm.h" +#include pthread_t udpRecv, udpSend; int8_t heartBeat[9] = {0}; @@ -19,7 +9,19 @@ struct sockaddr_in clientAddr; char clientIP[20] = ""; lcm_t *lcm; +pid_t httpServerPid = -1; + +void sigIntHandler(int sig) { + if (httpServerPid != -1) { + kill(httpServerPid, SIGINT); + } + printf("Exiting...\n"); + exit(sig); +} + int main() { + // 当遇到Ctrl-C时，递归地杀死所有子进程 + signal(SIGINT, sigIntHandler); if (!LCMInit()) { fprintf(stderr, "Error: Failed to initialize LCM\n"); return -1; @@ -28,11 +30,10 @@ int main() { pthread_mutex_init(&heartBeatMutex, NULL); // 开启udp接收线程 pthread_create(&udpRecv, NULL, udpRecvHandler, NULL); - pid_t httpServerPid = -1; if ((httpServerPid = fork()) == 0) { - // 拉起服务器进程，手机端需要申请的文件/mnt/cf/mapfile/defaultMap.txt + // 拉起服务器进程，手机端需要申请的文件/data/test/defaultMap.txt.txt // ATTENTION: 这里的路径需要根据实际存储地图的路径来修改 - chdir("/mnt/cf/mapfile/"); + chdir("/data/test"); /* * execlp使用系统调用exec()执行一个程序 * 第一个参数是文件名，将在PATH中查找，最后一个参数必须是NULL @@ -48,7 +49,8 @@ int main() { } bool LCMInit() { - if ((lcm = lcm_create("udpm://239.255.76.67:7667?ttl=1")) == NULL) { + // 多播地址范围：224.0.0.0~239.255.255.255 + if ((lcm = lcm_create(NULL)) == NULL) { fprintf(stderr, "Error: Failed to create Path Ctrl LCM\n"); return false; } @@ -86,12 +88,34 @@ void parseCmd(const char *buffer, int bytesReceived) { path.speed = 0; path_ctrl_t_publish(lcm, "wheel_ctrl", &path); + printf("Save Map!\n"); robotCtrlInit(&robotCtrlData, 0, 32, 0, 0, 0, 0, 0); robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData); - pose_t_subscribe(lcm, "CURRENTPOSE", poseHandler, NULL); + freeRobotCtrl(&robotCtrlData); + + sleep(2); + + robotCtrlInit(&robotCtrlData, 0, 10, 0, 7, 0, 0, 0); + pthread_mutex_lock(&heartBeatMutex); + robotCtrlData.dparams[4] = (double)(heartBeat[3] << 8 + heartBeat[4]); + robotCtrlData.dparams[5] = (double)(heartBeat[5] << 8 + heartBeat[6]); + robotCtrlData.dparams[6] = (double)(heartBeat[7] << 8 + heartBeat[8]); + pthread_mutex_unlock(&heartBeatMutex); + robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData); + freeRobotCtrl(&robotCtrlData); } else if (buffer[0] == 3) { // 手机发来的终点坐标，转发给算法模块 + int x, y, sita; + x = buffer[3] << 8 | buffer[4]; + y = buffer[5] << 8 | buffer[6]; + sita = buffer[7] << 8 | buffer[8]; + robotCtrlInit(&robotCtrlData, 0, 20, 0, 3, 0, 0, 0); + robotCtrlData.dparams[0] = x; + robotCtrlData.dparams[1] = y; + robotCtrlData.dparams[2] = sita; + robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData); + freeRobotCtrl(&robotCtrlData); } } -- cgit v1.2.3-70-g09d2