1 files changed, 29 insertions, 167 deletions

diff --git a/serial.c b/serial.c
index 43506ec..6251675 100644
--- a/serial.c
+++ b/serial.c
@@ -1,50 +1,46 @@
-/*
- * 在OpenHarmony系统下开发串口通信的应用与在Linux系统中类似，
- * 但需要确保你的设备支持串口操作，并且有适当的驱动支持。
- *
- * 以下是一个用C++编写的示例，展示如何在OpenHarmony中打开串口，
- * 设置波特率为115200，并发送指定的16进制数。
- *   - 打开串口：使用open()函数打开指定的串口设备。
- *   - 配置串口：使用termios结构体配置串口参数如波特率、字符大小、奇偶校验等。
- *   - 发送数据：使用write()函数将数据写入串口。
- *   - 关闭串口：操作完成后，使用close()函数关闭串口。
- * 确保你的OpenHarmony设备有对应的串口设备文件/dev/ttyS1，
- * 并且你的应用有足够的权限来访问这个设备。根据你的硬件和操作系统配置，设备文件的路径可能不同。
- */
-
-#include <arpa/inet.h>
+#include "path/path_ctrl_t.h"
 #include <errno.h>
 #include <fcntl.h>
-#include <netinet/in.h>
-#include <poll.h>
-#include <pthread.h>
 #include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <sys/socket.h>
 #include <termios.h>
 #include <unistd.h>
 
-#define FULL_SPEED 1.0f // 全速前进的速度
-#define R 0.15f         // 轮距的一半
-
 typedef unsigned char byte;
-int fd;
-int port = 5001;
+#define MAX_BUFFER_SIZE 1024
+
+int fd;                             // 轮子的串口文件描述符
 char portname[50] = "/dev/ttyUSB0"; // 串口设备名
 char clientIP[20] = "";
-
 struct termios tty;
-pthread_t udpSend, udpRecv;
-const int MAX_BUFFER_SIZE = 1024;
-struct sockaddr_in clientAddr;
 
-int whellInit() {
+bool whellInit();
+bool wheelSend(byte a, byte a_v, byte b, byte b_v);
+void speedControl(byte status, byte speed);
+void parseCmd(const path_ctrl_t *msg, void *user);
+
+int main() {
+    if (!whellInit()) {
+        goto err;
+    }
+    lcm *path_lcm = lcm_create(NULL);
+    if (!path_lcm) {
+        fprintf(stderr, "Failed to create LCM\n");
+        goto err;
+    }
+    path_ctrl_t_subscribe(path_lcm, "PATH_CTRL", parseCmd);
+
+err:
+    close(fd);
+    return 0;
+}
+
+bool whellInit() {
     // 打开串口
     fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC);
     if (fd < 0) {
-        // C语言报error
         fprintf(stderr, "Error opening %s: %s\n", portname, strerror(errno));
         return false;
     }
@@ -52,7 +48,6 @@ int whellInit() {
     // 设置串口参数
     memset(&tty, 0, sizeof tty);
     if (tcgetattr(fd, &tty) != 0) {
-        // cerr << "Error from tcgetattr: " << strerror(errno) << endl;
         fprintf(stderr, "Error from tcgetattr: %s\n", strerror(errno));
         return false;
     }
@@ -77,7 +72,6 @@ int whellInit() {
     tty.c_cflag &= ~CRTSCTS;
 
     if (tcsetattr(fd, TCSANOW, &tty) != 0) {
-        // cerr << "Error from tcsetattr: " << strerror(errno) << endl;
         fprintf(stderr, "Error from tcsetattr: %s\n", strerror(errno));
         return false;
     }
@@ -108,7 +102,8 @@ bool wheelSend(byte a, byte a_v, byte b, byte b_v) {
     return true;
 }
 
-void speedControl(byte status, byte speed) {
+void parseCmd(const path_ctrl_t *msg, void *user) {
+    byte status = msg->cmd, speed = msg->speed;
     switch (status) {
     case 1:
         wheelSend(0x01, speed, 0x01, speed);
@@ -124,137 +119,4 @@ void speedControl(byte status, byte speed) {
         wheelSend(0x00, 0x00, 0x00, 0x00);
         break;
     }
-}
-
-void udpSendThread() {
-    // 创建UDP套接字
-    int sockfd = -1;
-    struct sockaddr_in serverAddr;
-    if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
-        fprintf(stderr, "Error: Failed to create socket\n");
-        return;
-    }
-
-    memset((char *)&serverAddr, 0, sizeof(serverAddr));
-    serverAddr.sin_family = AF_INET;   // 设置地址族为IPv4
-    serverAddr.sin_port = htons(port); // 设置端口号
-    if (inet_aton(clientIP, &serverAddr.sin_addr) == 0) {
-        fprintf(stderr, "Error: Failed to convert IP address\n");
-        close(sockfd);
-        return;
-    }
-    while (true) {
-        // 每秒向socket发送一次0x30
-        byte a = 0x39;
-        // 发送消息
-        sendto(sockfd, (const char *)&a, 1, 0, (struct sockaddr *)&serverAddr,
-               sizeof(serverAddr));
-        printf("Send: Data %02X udpSend successfully!\n", a);
-        sleep(1);
-    }
-}
-
-void udpRecvThread() {
-    int socketfd = -1;
-    char buffer[MAX_BUFFER_SIZE];
-    struct sockaddr_in serverAddr;
-    socklen_t addrLen = sizeof(serverAddr);
-    int bytesReceived;
-
-    // fd_set readfds;
-    // struct timeval tv;
-    int retval;
-    struct pollfd fds;
-
-    if ((socketfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
-        fprintf(stderr, "Error: Failed to create socket\n");
-        return;
-    }
-    memset((char *)&serverAddr, 0, sizeof(serverAddr));
-    memset(buffer, 0, sizeof(buffer));
-    serverAddr.sin_family = AF_INET;
-    serverAddr.sin_port = htons(port);
-    serverAddr.sin_addr.s_addr = htonl(INADDR_ANY);
-
-    if (bind(socketfd, (struct sockaddr *)&serverAddr, sizeof(serverAddr)) ==
-        -1) {
-        // cerr << "Error: Bind failed" << endl;
-        fprintf(stderr, "Error: Bind failed\n");
-        close(socketfd);
-        return;
-    }
-
-    bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
-                             (struct sockaddr *)&clientAddr, &addrLen);
-    if (bytesReceived == -1) {
-        fprintf(stderr, "Error: Failed to receive data\n");
-        close(socketfd);
-        return;
-    }
-    printf("Received start message from %s: %s\n",
-           inet_ntoa(clientAddr.sin_addr), buffer);
-    strcpy(clientIP, inet_ntoa(clientAddr.sin_addr));
-
-    if (pthread_create(&udpSend, NULL, udpSendThread, NULL) != 0) {
-        fprintf(stderr, "Error creating udpSend thread\n");
-        close(socketfd);
-        return;
-    }
-
-    fds.fd = socketfd;
-    fds.events = POLLIN;
-
-    while (true) {
-        // /* Watch sock to see when it has input. */
-        // FD_ZERO(&readfds);
-        // FD_SET(socketfd, &readfds);
-        // /* Wait up to three seconds. */
-        // tv.tv_sec = 3;
-        // tv.tv_usec = 0;
-
-        // retval = select(socketfd + 1, &readfds, NULL, NULL, &tv);
-        retval = poll(&fds, 1, 3000);
-        if (retval == -1) {
-            fprintf(stderr, "Error: select failed\n");
-            break;
-        } else if (retval == 0) {
-            printf("No data within three seconds.\n");
-            speedControl(0, 0);
-            // break;
-        } else {
-            bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
-                                     (struct sockaddr *)&clientAddr, &addrLen);
-            if (bytesReceived == -1) {
-                fprintf(stderr, "Error: Failed to receive data\n");
-                continue;
-            }
-            printf("Received Message from %s: %2x %2x\n",
-                   inet_ntoa(clientAddr.sin_addr), buffer[0], buffer[1]);
-            speedControl(buffer[0], buffer[1]);
-        }
-    }
-}
-
-int main() {
-    // 关闭1号和2号文件描述符
-    // close(1);
-    // close(2);
-    // // 打开serial.log文件，并将其复制到1号和2号文件描述符
-    // open("serial.log", O_RDWR | O_CREAT, 0666);
-    // dup(1);
-    // dup(2);
-
-    if (!whellInit()) {
-        goto err;
-    }
-
-    if (pthread_create(&udpRecv, NULL, udpRecvThread, NULL) != 0) {
-        fprintf(stderr, "Error creating udpRecv thread\n");
-        goto err;
-    }
-    pthread_join(udpRecv, NULL);
-
-err:
-    close(fd);
-    return 0;
-}
+}
\ No newline at end of file