1 files changed, 14 insertions, 6 deletions

diff --git a/serial/serial.c b/serial/serial.c
index 57b432c..61be0aa 100644
--- a/serial/serial.c
+++ b/serial/serial.c
@@ -3,7 +3,7 @@
 lcm_t *lcm;
 
 int curStatus = -1, curSpeed = 0; // 这里的速度指百分比速度
-double curOmega;
+double curOmega = 0.0;
 // 临界区数据
 pthread_mutex_t curPoseMutex;
 clock_t lastTime;
@@ -20,6 +20,7 @@ int main() {
     }
 
     // 订阅来自手机端的轮控消息
+    lastTime = clock();
     path_ctrl_t_subscribe(lcm, "wheel_ctrl", parseCmd, NULL);
     // 订阅来自算法模块的路径规划消息
     path_t_subscribe(lcm, "PATH", parsePath, NULL);
@@ -45,6 +46,8 @@ err:
 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;
     }
@@ -124,12 +127,17 @@ void sendCurPose() {
     while (true) {
         renewCurPose();
         pthread_mutex_lock(&curPoseMutex);
-        pose.pos[0] = curPose[0];
-        pose.pos[1] = curPose[1];
-        pose.pos[2] = curPose[2];
+        // pose.pos[0] = curPose[0];
+        // pose.pos[1] = curPose[1];
+        // pose.pos[2] = curPose[2];
+        pose.pos[0] = 0.0;
+        pose.pos[1] = 0.0;
+        pose.pos[2] = 0.0;
         pthread_mutex_unlock(&curPoseMutex);
         pose_t_publish(lcm, "POSE", &pose);
-        usleep(16);
+        // printf("sent (%lf, %lf, %lf)\n", pose.pos[0], pose.pos[1],
+        // pose.pos[2]);
+        usleep(15 * 1000);
     }
 }
 
@@ -142,6 +150,6 @@ void renewCurPose() {
     curPose[0] += speed * cos(curPose[2]) * dt;
     curPose[1] += speed * sin(curPose[2]) * dt;
     curPose[2] += curOmega * dt;
-    lastTime = curTime;
+    lastTime = clock();
     pthread_mutex_unlock(&curPoseMutex);
 }
\ No newline at end of file