Fix bugs, now it can deal with curpose rightly

6 files changed, 123 insertions, 28 deletions

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 <dirent.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <termios.h>
+#include <unistd.h>
+
+#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 <arpa/inet.h>
 #include <errno.h>
 #include <fcntl.h>
+#include <math.h>
 #include <netinet/in.h>
 #include <poll.h>
 #include <pthread.h>
@@ -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