4 files changed, 39 insertions, 7 deletions

diff --git a/serial/CMakeLists.txt b/serial/CMakeLists.txt
index 5ded98c..c0bc05a 100644
--- a/serial/CMakeLists.txt
+++ b/serial/CMakeLists.txt
@@ -8,4 +8,4 @@ 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
+target_link_libraries(serial lcm pthread)
diff --git a/serial/serial.c b/serial/serial.c
index a2f253a..6163703 100644
--- a/serial/serial.c
+++ b/serial/serial.c
@@ -1,7 +1,12 @@
 #include "serial.h"
+#include <math.h>
 
 lcm_t *lcm;
 
+// TODO 屏蔽停止指令
+bool stopFlag = false;
+////////////////////////
+
 int curStatus = -1, curSpeed = 0; // 这里的速度指百分比速度
 double curOmega = 0.0;
 // 临界区数据
@@ -72,6 +77,23 @@ void parseCmd(const lcm_recv_buf_t *rbuf, const char *channel,
         curOmega = (double)-DEFAULT_SPEED * FULLSPEED / 100 / RADIUS;
         curSpeed = 0;
         break;
+    // TODO 屏蔽停止指令
+    case 4:
+        wheelSend(0x00, 0x00, 0x00, 0x00);
+        curOmega = 0;
+        curSpeed = 0;
+        stopFlag = true;
+        printf("Stop for 2 seconds\n");
+        break;
+    // 取消屏蔽
+    case 5:
+        wheelSend(0x00, 0x00, 0x00, 0x00);
+        curOmega = 0;
+        curSpeed = 0;
+        stopFlag = false;
+        printf("Stop but can recieve cmd\n");
+        break;
+    ////////////////////////////////////
     case 0:
     default:
         wheelSend(0x00, 0x00, 0x00, 0x00);
@@ -85,6 +107,11 @@ 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) {
+    // TODO
+    if (stopFlag) {
+        return;
+    }
+    /////////////////
     int16_t length = msg->length;
     double v, w;
     int8_t vWheels[2];
@@ -127,7 +154,7 @@ void setCurPose(const lcm_recv_buf_t *rbuf, const char *channel,
     pthread_mutex_unlock(&curPoseMutex);
 }
 
-void sendCurPose() {
+void *sendCurPose(void *args) {
     pose_t pose;
     while (true) {
         renewCurPose();
@@ -152,4 +179,4 @@ void renewCurPose() {
     curPose[2] += curOmega * dt;
     lastTime = clock();
     pthread_mutex_unlock(&curPoseMutex);
-}
\ No newline at end of file
+}
diff --git a/serial/serial.h b/serial/serial.h
index 3563557..2fbf870 100644
--- a/serial/serial.h
+++ b/serial/serial.h
@@ -19,7 +19,7 @@
 // TODO: 两个待测数据
 #define RADIUS 0.13    // 两轮之间的距离的一半，米
 #define FULLSPEED 0.60 // 轮子全速，m/s
-#define fabs(x) ((x) > 0 ? (x) : -(x))
+// #define fabs(x) ((x) > 0 ? (x) : -(x))
 typedef unsigned char byte;
 
 int identify_device(const char *port);
@@ -34,6 +34,6 @@ void parsePath(const lcm_recv_buf_t *rbuf, const char *channel,
 void setCurPose(const lcm_recv_buf_t *rbuf, const char *channel,
                 const pose_t *msg, void *userdata);
 void renewCurPose();
-void sendCurPose();
+void *sendCurPose(void *args);
 
-#endif
\ No newline at end of file
+#endif
diff --git a/serial/wheel.c b/serial/wheel.c
index 8155734..c470234 100644
--- a/serial/wheel.c
+++ b/serial/wheel.c
@@ -5,7 +5,8 @@ char *portname; // 串口设备名
 struct termios tty;
 
 bool whellInit() {
-    portname = findUSBDev("wheel");
+    // portname = findUSBDev("wheel");
+    portname = "/dev/ttyUSB1"; // "/dev/ttyUSB1
     if (portname == NULL) {
         fprintf(stderr, "Error: Failed to find wheel serial port\n");
         return false;
@@ -55,6 +56,10 @@ 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;
+    // TODO: 速度转换
+    int temp = ((int)a_v + 2);
+    a_v = (byte)temp;
+    ////////////////////////
     data[3] = a_v;
     data[4] = b;
     data[5] = b_v;