update

5 files changed, 261 insertions, 110 deletions

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 <errno.h>
-#include <fcntl.h>
-#include <stdbool.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <termios.h>
-#include <unistd.h>
+#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 <errno.h>
+#include <fcntl.h>
+#include <pthread.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <termios.h>
+#include <unistd.h>
+
+#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 <sys/signal.h>
 
 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);
     }
 }