1 files changed, 29 insertions, 2 deletions

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
+}