From 692b77cc3b1bb030ed83416d76b561d11a57d1e0 Mon Sep 17 00:00:00 2001 From: We-unite <3205135446@qq.com> Date: Sat, 22 Jun 2024 22:36:45 +0800 Subject: Fix bugs, now it can deal with curpose rightly --- serial/serial.c | 20 ++++++++++++----- serial/serial.h | 7 ++++-- serial/usbdev.c | 65 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ serial/wheel.c | 10 +++++++-- udp2lcm/udp2lcm.c | 45 +++++++++++++++++++++++--------------- udp2lcm/udp2lcm.h | 4 +++- 6 files changed, 123 insertions(+), 28 deletions(-) create mode 100644 serial/usbdev.c diff --git a/serial/serial.c b/serial/serial.c index 57b432c..61be0aa 100644 --- a/serial/serial.c +++ b/serial/serial.c @@ -3,7 +3,7 @@ lcm_t *lcm; int curStatus = -1, curSpeed = 0; // 这里的速度指百分比速度 -double curOmega; +double curOmega = 0.0; // 临界区数据 pthread_mutex_t curPoseMutex; clock_t lastTime; @@ -20,6 +20,7 @@ int main() { } // 订阅来自手机端的轮控消息 + lastTime = clock(); path_ctrl_t_subscribe(lcm, "wheel_ctrl", parseCmd, NULL); // 订阅来自算法模块的路径规划消息 path_t_subscribe(lcm, "PATH", parsePath, NULL); @@ -45,6 +46,8 @@ err: 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; } @@ -124,12 +127,17 @@ void sendCurPose() { while (true) { renewCurPose(); pthread_mutex_lock(&curPoseMutex); - pose.pos[0] = curPose[0]; - pose.pos[1] = curPose[1]; - pose.pos[2] = curPose[2]; + // pose.pos[0] = curPose[0]; + // pose.pos[1] = curPose[1]; + // pose.pos[2] = curPose[2]; + pose.pos[0] = 0.0; + pose.pos[1] = 0.0; + pose.pos[2] = 0.0; pthread_mutex_unlock(&curPoseMutex); pose_t_publish(lcm, "POSE", &pose); - usleep(16); + // printf("sent (%lf, %lf, %lf)\n", pose.pos[0], pose.pos[1], + // pose.pos[2]); + usleep(15 * 1000); } } @@ -142,6 +150,6 @@ void renewCurPose() { curPose[0] += speed * cos(curPose[2]) * dt; curPose[1] += speed * sin(curPose[2]) * dt; curPose[2] += curOmega * dt; - lastTime = curTime; + lastTime = clock(); pthread_mutex_unlock(&curPoseMutex); } \ No newline at end of file diff --git a/serial/serial.h b/serial/serial.h index 8061b24..7f09bf8 100644 --- a/serial/serial.h +++ b/serial/serial.h @@ -17,11 +17,14 @@ #define MAX_BUFFER_SIZE 1024 #define DEFAULT_SPEED 0x15 // 默认速度，百分比 // TODO: 两个待测数据 -#define RADIUS 0.3 // 两轮之间的距离的一半，米 +#define RADIUS 0.26 // 两轮之间的距离的一半，米 #define FULLSPEED 1.5 // 轮子全速，m/s #define fabs(x) ((x) > 0 ? (x) : -(x)) typedef unsigned char byte; +int identify_device(const char *port); +const char *findUSBDev(const char *device_type); + 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, @@ -29,7 +32,7 @@ void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel, 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); + const pose_t *msg, void *userdata); void renewCurPose(); void sendCurPose(); diff --git a/serial/usbdev.c b/serial/usbdev.c new file mode 100644 index 0000000..2a543fa --- /dev/null +++ b/serial/usbdev.c @@ -0,0 +1,65 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#define LIDAR_SERIAL "dce74d177fdc724ba5757727edc269bf" +#define WHEEL_SERIAL "0001" + +enum usbDevice { lidar, wheel, other }; +char device_path[256]; + +int identify_device(const char *port) { + char cmd[256]; + snprintf(cmd, sizeof(cmd), "udevadm info --name=%s | grep 'E: ID_SERIAL='", + port); + FILE *fp = popen(cmd, "r"); + if (!fp) { + perror("popen"); + return other; + } + + static char serial[256]; + if (fgets(serial, sizeof(serial), fp) != NULL) { + if (strstr(serial, WHEEL_SERIAL)) { + pclose(fp); + return wheel; + } else if (strstr(serial, LIDAR_SERIAL)) { + pclose(fp); + return lidar; + } + } + pclose(fp); + return other; +} + +const char *findUSBDev(const char *device_type) { + DIR *dir; + struct dirent *entry; + + dir = opendir("/dev"); + if (dir == NULL) { + perror("opendir"); + return NULL; + } + + while ((entry = readdir(dir)) != NULL) { + if (strncmp(entry->d_name, "ttyUSB", 6) == 0) { + snprintf(device_path, sizeof(device_path), "/dev/%s", + entry->d_name); + int dev_type = identify_device(device_path); + if ((dev_type == lidar && strcmp(device_type, "lidar") == 0) || + (dev_type == wheel && strcmp(device_type, "wheel") == 0)) { + closedir(dir); + return device_path; + } + } + } + + closedir(dir); + return NULL; +} diff --git a/serial/wheel.c b/serial/wheel.c index 936f55c..8155734 100644 --- a/serial/wheel.c +++ b/serial/wheel.c @@ -1,10 +1,16 @@ #include "serial.h" -int fd; // 轮子的串口文件描述符 -char portname[50] = "/dev/ttyUSB0"; // 串口设备名 +int fd; // 轮子的串口文件描述符 +char *portname; // 串口设备名 struct termios tty; bool whellInit() { + portname = findUSBDev("wheel"); + if (portname == NULL) { + fprintf(stderr, "Error: Failed to find wheel serial port\n"); + return false; + } + printf("Wheel serial port: %s\n", portname); // 打开串口 fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC); if (fd < 0) { diff --git a/udp2lcm/udp2lcm.c b/udp2lcm/udp2lcm.c index 76f41b0..9ac380f 100644 --- a/udp2lcm/udp2lcm.c +++ b/udp2lcm/udp2lcm.c @@ -30,6 +30,7 @@ int main() { pthread_mutex_init(&heartBeatMutex, NULL); // 开启udp接收线程 pthread_create(&udpRecv, NULL, udpRecvHandler, NULL); + pose_t_subscribe(lcm, "CURRENTPOSE", poseHandler, NULL); if ((httpServerPid = fork()) == 0) { // 拉起服务器进程，手机端需要申请的文件/data/test/defaultMap.txt.txt // ATTENTION: 这里的路径需要根据实际存储地图的路径来修改 @@ -43,7 +44,8 @@ int main() { execlp("python", "python", "-m", "http.server", NULL); } while (true) { - pause(); + // pause(); + lcm_handle(lcm); } return 0; } @@ -91,28 +93,34 @@ void parseCmd(const char *buffer, int bytesReceived) { 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]); + int16_t x, y, sita; + x = swapEndian(*(int16_t *)&heartBeat[3]); + y = swapEndian(*(int16_t *)&heartBeat[5]); + sita = swapEndian(*(int16_t *)&heartBeat[7]); + printf("NOW point: x: %d, y: %d, sita: %d\n", x, y, sita); + robotCtrlData.dparams[4] = (double)x / 20; + robotCtrlData.dparams[5] = (double)y / 20; + robotCtrlData.dparams[6] = (double)sita * M_PI / 180; 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]; + x = swapEndian(*(int16_t *)&buffer[3]); + y = swapEndian(*(int16_t *)&buffer[5]); + sita = 0; + // printf("End point: x: %d, y: %d, sita: %d\n", x, y, sita); + // printf("Original buffer: ", buffer); robotCtrlInit(&robotCtrlData, 0, 20, 0, 3, 0, 0, 0); - robotCtrlData.dparams[0] = x; - robotCtrlData.dparams[1] = y; + robotCtrlData.dparams[0] = (double)x / 20; + robotCtrlData.dparams[1] = (double)y / 20; robotCtrlData.dparams[2] = sita; robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData); freeRobotCtrl(&robotCtrlData); @@ -122,9 +130,9 @@ void parseCmd(const char *buffer, int bytesReceived) { void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel, const pose_t *msg, void *userdata) { int16_t x, y, sita; - x = (int16_t)msg->pos[0]; - y = (int16_t)msg->pos[1]; - sita = (int16_t)msg->pos[2]; + x = (int16_t)((double)msg->pos[0] * 20); + y = (int16_t)((double)msg->pos[1] * 20); + sita = (int16_t)((double)msg->pos[2] * 180 / M_PI); while (sita > 180) sita -= 360; while (sita < -180) @@ -133,10 +141,13 @@ void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel, heartBeat[0] = 0x03; heartBeat[1] = heartBeat[2] = 0; heartBeat[3] = x >> 8; - heartBeat[4] = x; + heartBeat[4] = x & 0xff; heartBeat[5] = y >> 8; - heartBeat[6] = y; + heartBeat[6] = y & 0xff; heartBeat[7] = sita >> 8; heartBeat[8] = sita; pthread_mutex_unlock(&heartBeatMutex); -} \ No newline at end of file + printf("curpose: x: %d, y: %d, sita: %d\n", x, y, sita); +} + +int16_t swapEndian(int16_t val) { return (val << 8) | ((val >> 8) & 0xFF); } \ No newline at end of file diff --git a/udp2lcm/udp2lcm.h b/udp2lcm/udp2lcm.h index 3bab6ac..1ab7cf3 100644 --- a/udp2lcm/udp2lcm.h +++ b/udp2lcm/udp2lcm.h @@ -2,11 +2,12 @@ #define UDP2LCM_H #include "path_ctrl_t.h" -#include "robot_control_t.h" #include "pose_t.h" +#include "robot_control_t.h" #include #include #include +#include #include #include #include @@ -41,5 +42,6 @@ void robotCtrlInit(robot_control_t *robotCtrlData, int64_t utime, void freeRobotCtrl(robot_control_t *robotCtrlData); void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel, const pose_t *msg, void *userdata); +int16_t swapEndian(int16_t val); #endif \ No newline at end of file -- cgit v1.2.3-70-g09d2