Update sth && add "cancel target"

- set sita as rad
- fix the change between little to big end
- sth else

3 files changed, 20 insertions, 13 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);
     }
 }
diff --git a/serial/serial.h b/serial/serial.h
index 7f09bf8..3563557 100644
--- a/serial/serial.h
+++ b/serial/serial.h
@@ -17,8 +17,8 @@
 #define MAX_BUFFER_SIZE 1024
 #define DEFAULT_SPEED 0x15 // 默认速度，百分比
 // TODO: 两个待测数据
-#define RADIUS 0.26   // 两轮之间的距离的一半，米
-#define FULLSPEED 1.5 // 轮子全速，m/s
+#define RADIUS 0.13    // 两轮之间的距离的一半，米
+#define FULLSPEED 0.60 // 轮子全速，m/s
 #define fabs(x) ((x) > 0 ? (x) : -(x))
 typedef unsigned char byte;
 
diff --git a/udp2lcm/udp2lcm.c b/udp2lcm/udp2lcm.c
index 9ac380f..36100da 100644
--- a/udp2lcm/udp2lcm.c
+++ b/udp2lcm/udp2lcm.c
@@ -116,14 +116,22 @@ void parseCmd(const char *buffer, int bytesReceived) {
         x = swapEndian(*(int16_t *)&buffer[3]);
         y = swapEndian(*(int16_t *)&buffer[5]);
         sita = 0;
-        // printf("End point: x: %d, y: %d, sita: %d\n", x, y, sita);
-        // printf("Original buffer: ", buffer);
         robotCtrlInit(&robotCtrlData, 0, 20, 0, 3, 0, 0, 0);
         robotCtrlData.dparams[0] = (double)x / 20;
         robotCtrlData.dparams[1] = (double)y / 20;
         robotCtrlData.dparams[2] = sita;
         robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
         freeRobotCtrl(&robotCtrlData);
+    } else if (buffer[0] == 4) {
+        // 取消导航，删除上一个目标点
+        robotCtrlInit(&robotCtrlData, 0, 23, 0, 0, 0, 0, 0);
+        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+    } else {
+        fprintf(stderr, "Error: Invalid command: ");
+        for (int i = 0; i < 9; i++) {
+            fprintf(stderr, "%02hhx ", buffer[i]);
+        }
+        fprintf(stderr, "\n");
     }
 }
 
@@ -147,7 +155,6 @@ void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel,
     heartBeat[7] = sita >> 8;
     heartBeat[8] = sita;
     pthread_mutex_unlock(&heartBeatMutex);
-    printf("curpose: x: %d, y: %d, sita: %d\n", x, y, sita);
 }
 
 int16_t swapEndian(int16_t val) { return (val << 8) | ((val >> 8) & 0xFF); }
\ No newline at end of file