1 files changed, 8 insertions, 8 deletions

diff --git a/serial/serial.c b/serial/serial.c
index 61be0aa..a2f253a 100644
--- a/serial/serial.c
+++ b/serial/serial.c
@@ -95,12 +95,16 @@ void parsePath(const lcm_recv_buf_t *rbuf, const char *channel,
     }
     v = msg->xyr[0][0];
     w = msg->xyr[0][1];
+    printf("v: %f m/s, w: %f rad/s\n", v, w);
     if (fabs(v) <= 0.01 && fabs(w) <= 0.01) {
         wheelSend(0x00, 0x00, 0x00, 0x00);
         renewCurPose();
         curSpeed = 0;
         curOmega = 0;
     } else {
+        if (w <= 0.2) {
+            w *= 2;
+        }
         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;
@@ -110,6 +114,7 @@ void parsePath(const lcm_recv_buf_t *rbuf, const char *channel,
         curSpeed = (int8_t)((double)v / FULLSPEED * 100);
         curOmega = (int8_t)w;
     }
+    printf("curSpeed: %d, curOmega: %f\n", curSpeed, curOmega);
 }
 
 void setCurPose(const lcm_recv_buf_t *rbuf, const char *channel,
@@ -127,16 +132,11 @@ 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] = 0.0;
-        pose.pos[1] = 0.0;
-        pose.pos[2] = 0.0;
+        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);
-        // printf("sent (%lf, %lf, %lf)\n", pose.pos[0], pose.pos[1],
-        // pose.pos[2]);
         usleep(15 * 1000);
     }
 }