1 files changed, 110 insertions, 93 deletions

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