1 files changed, 47 insertions, 1 deletions

diff --git a/udp2lcm/udp2lcm.c b/udp2lcm/udp2lcm.c
index 36100da..5514b24 100644
--- a/udp2lcm/udp2lcm.c
+++ b/udp2lcm/udp2lcm.c
@@ -71,7 +71,13 @@ void parseCmd(const char *buffer, int bytesReceived) {
     if (buffer[0] == 0) {
         // 手机端用来与udp2lcm服务器建立连接的初始化消息
         // 也是手机端的心跳
-        ;
+        // 下达30号命令，开始建图
+        robotCtrlInit(&robotCtrlData, 0, 30, 0, 1, 1, 0, 0);
+        robotCtrlData.dparams[0] = 0.05;
+        robotCtrlData.niparams = 1;
+        robotCtrlData.iparams[0] = 1;
+        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+        freeRobotCtrl(&robotCtrlData);
     } else if (buffer[0] == 1) {
         // 建图过程中，手机端遥控小车进行移动，命令下发至轮控模块
         path.cmd = buffer[1];
@@ -125,7 +131,47 @@ void parseCmd(const char *buffer, int bytesReceived) {
     } else if (buffer[0] == 4) {
         // 取消导航，删除上一个目标点
         robotCtrlInit(&robotCtrlData, 0, 23, 0, 0, 0, 0, 0);
+        // TODO 第一次停止指令
+        printf("Cancel Navigation!\n");
+        path.cmd = 4;
+        path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
+        /////////////////////////////
+        robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+        // TODO 再次下达停止指令
+        sleep(2);
+        path.cmd = 5;
+        printf("Stop but can recieve cmd\n");
+        path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
+        /////////////////////////
+    } else if (buffer[0] == 5) {
+        /*
+        加载地图，需要注意的是所加载的地图是由电脑控制端下发的
+        该部分由建图部分修改
+        */
+        path.cmd = 0;
+        path.speed = 0;
+        path_ctrl_t_publish(lcm, "wheel_ctrl", &path);
+
+        printf("Load Map!\n");
+        // robotCtrlInit(&robotCtrlData, 0, 32, 0, 0, 0, 0, 0);
+        // robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+        // freeRobotCtrl(&robotCtrlData);
+
+        // sleep(2);
+
+        robotCtrlInit(&robotCtrlData, 0, 10, 0, 7, 0, 0, 0);
+        pthread_mutex_lock(&heartBeatMutex);
+        int16_t x, y, sita;
+        x = swapEndian(*(int16_t *)&heartBeat[3]);
+        y = swapEndian(*(int16_t *)&heartBeat[5]);
+        sita = swapEndian(*(int16_t *)&heartBeat[7]);
+        printf("NOW point: x: %d, y: %d, sita: %d\n", x, y, sita);
+        robotCtrlData.dparams[4] = 0;
+        robotCtrlData.dparams[5] = 0;
+        robotCtrlData.dparams[6] = 0;
+        pthread_mutex_unlock(&heartBeatMutex);
         robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
+        freeRobotCtrl(&robotCtrlData);
     } else {
         fprintf(stderr, "Error: Invalid command: ");
         for (int i = 0; i < 9; i++) {