14 files changed, 264 insertions, 30 deletions

diff --git a/.clang-format b/.clang-format
new file mode 100644
index 0000000..a1054a5
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,121 @@
+---
+Language:        Cpp
+# BasedOnStyle:  LLVM
+AccessModifierOffset: -2
+AlignAfterOpenBracket: Align
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
+AlignEscapedNewlines: Right
+AlignOperands:   true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: false
+AlwaysBreakTemplateDeclarations: MultiLine
+BinPackArguments: true
+BinPackParameters: true
+BraceWrapping:   
+  AfterClass:      false
+  AfterControlStatement: false
+  AfterEnum:       false
+  AfterFunction:   false
+  AfterNamespace:  false
+  AfterObjCDeclaration: false
+  AfterStruct:     false
+  AfterUnion:      false
+  AfterExternBlock: false
+  BeforeCatch:     false
+  BeforeElse:      false
+  IndentBraces:    false
+  SplitEmptyFunction: true
+  SplitEmptyRecord: true
+  SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeBraces: Attach
+BreakBeforeInheritanceComma: false
+BreakInheritanceList: BeforeColon
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit:     80
+CommentPragmas:  '^ IWYU pragma:'
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: false
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: false
+DisableFormat:   false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:   
+  - foreach
+  - Q_FOREACH
+  - BOOST_FOREACH
+IncludeBlocks:   Preserve
+IncludeCategories: 
+  - Regex:           '^"(llvm|llvm-c|clang|clang-c)/'
+    Priority:        2
+  - Regex:           '^(<|"(gtest|gmock|isl|json)/)'
+    Priority:        3
+  - Regex:           '.*'
+    Priority:        1
+IncludeIsMainRegex: '(Test)?$'
+IndentCaseLabels: false
+IndentPPDirectives: None
+IndentWidth:     4
+IndentWrappedFunctionNames: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: true
+MacroBlockBegin: ''
+MacroBlockEnd:   ''
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBinPackProtocolList: Auto
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyBreakTemplateDeclaration: 10
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 60
+PointerAlignment: Right
+ReflowComments:  true
+SortIncludes:    true
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 1
+SpacesInAngles:  false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard:        Cpp11
+StatementMacros: 
+  - Q_UNUSED
+  - QT_REQUIRE_VERSION
+TabWidth:        8
+UseTab:          Never
+...
+
diff --git a/.gitignore b/.gitignore
index 938c377..9497a1d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,4 @@
 build/*
 bin/*
-.vscode/*
\ No newline at end of file
+.vscode/*
+compile_commands.json
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c487036..c01ccfc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,10 +9,10 @@ include_directories(
 )
 # 添加库文件路径
 link_directories(
-    ${CMAKE_SOURCE_DIR}/lib
+    ${CMAKE_SOURCE_DIR}/lib/${OHOS_ARCH}
 )
 # 设置输出路径
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
 
 add_subdirectory(serial)
-add_subdirectory(udp2lcm)
\ No newline at end of file
+add_subdirectory(udp2lcm)
diff --git a/lib/aarch64/liblcm.so b/lib/arm64-v8a/liblcm.so
index 98bb954..98bb954 100644..100755
--- a/lib/aarch64/liblcm.so
+++ b/lib/arm64-v8a/liblcm.so
diff --git a/lib/x86/liblcm.so b/lib/x86_64/liblcm.so
index cb18db8..cb18db8 100755
--- a/lib/x86/liblcm.so
+++ b/lib/x86_64/liblcm.so
diff --git a/runme.sh b/runme.sh
new file mode 100755
index 0000000..947d50a
--- /dev/null
+++ b/runme.sh
@@ -0,0 +1,34 @@
+#!/bin/bash
+
+##########################################################################
+# File Name    : compile.sh
+# Encoding     : utf-8
+# Author       : We-unite
+# Email        : weunite1848@gmail.com
+# Created Time : 2024-02-29 15:19:15
+##########################################################################
+
+set -e
+
+if [ $UID -eq 0 ]; then
+        echo "Please do not run this script as root"
+        exit 1
+fi
+
+if [ $# -ne 1 ]; then
+        echo "ERROR: $0 <arm64-v8a|x86_64>"
+        exit 1
+fi
+
+arch=$1
+link=static # or shared
+native_path=/home/player/app/native
+
+if [ $arch == "arm64-v8a" ]; then
+        export PATH=$native_path/build-tools/cmake/bin:$PATH
+        cmake -B build -D OHOS_STL=c++_$link -D OHOS_ARCH=$arch -D OHOS_PLATFORM=OHOS -D CMAKE_TOOLCHAIN_FILE=$(find $native_path -name ohos.toolchain.cmake)
+else
+        cmake -B build -D OHOS_ARCH=$arch
+fi
+
+cmake --build build
diff --git a/serial/CMakeLists.txt b/serial/CMakeLists.txt
index 5ded98c..c0bc05a 100644
--- a/serial/CMakeLists.txt
+++ b/serial/CMakeLists.txt
@@ -8,4 +8,4 @@ aux_source_directory(${CMAKE_SOURCE_DIR}/lcmtype DIR_SRCS)
 add_executable(serial ${DIR_SRCS})
 
 # 添加链接库
-target_link_libraries(serial lcm)
\ No newline at end of file
+target_link_libraries(serial lcm pthread)
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
+}
diff --git a/serial/serial.h b/serial/serial.h
index 3563557..2fbf870 100644
--- a/serial/serial.h
+++ b/serial/serial.h
@@ -19,7 +19,7 @@
 // TODO: 两个待测数据
 #define RADIUS 0.13    // 两轮之间的距离的一半，米
 #define FULLSPEED 0.60 // 轮子全速，m/s
-#define fabs(x) ((x) > 0 ? (x) : -(x))
+// #define fabs(x) ((x) > 0 ? (x) : -(x))
 typedef unsigned char byte;
 
 int identify_device(const char *port);
@@ -34,6 +34,6 @@ void parsePath(const lcm_recv_buf_t *rbuf, const char *channel,
 void setCurPose(const lcm_recv_buf_t *rbuf, const char *channel,
                 const pose_t *msg, void *userdata);
 void renewCurPose();
-void sendCurPose();
+void *sendCurPose(void *args);
 
-#endif
\ No newline at end of file
+#endif
diff --git a/serial/wheel.c b/serial/wheel.c
index 8155734..c470234 100644
--- a/serial/wheel.c
+++ b/serial/wheel.c
@@ -5,7 +5,8 @@ char *portname; // 串口设备名
 struct termios tty;
 
 bool whellInit() {
-    portname = findUSBDev("wheel");
+    // portname = findUSBDev("wheel");
+    portname = "/dev/ttyUSB1"; // "/dev/ttyUSB1
     if (portname == NULL) {
         fprintf(stderr, "Error: Failed to find wheel serial port\n");
         return false;
@@ -55,6 +56,10 @@ bool wheelSend(byte a, byte a_v, byte b, byte b_v) {
     unsigned char data[7] = {0x53, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00};
     byte checksum = 0;
     data[2] = a;
+    // TODO: 速度转换
+    int temp = ((int)a_v + 2);
+    a_v = (byte)temp;
+    ////////////////////////
     data[3] = a_v;
     data[4] = b;
     data[5] = b_v;
diff --git a/udp2lcm/CMakeLists.txt b/udp2lcm/CMakeLists.txt
index aaf54db..206a8a8 100644
--- a/udp2lcm/CMakeLists.txt
+++ b/udp2lcm/CMakeLists.txt
@@ -7,4 +7,4 @@ aux_source_directory(${CMAKE_SOURCE_DIR}/lcmtype DIR_SRCS)
 
 # 生成可执行文件
 add_executable(udp2lcm ${DIR_SRCS})
-target_link_libraries(udp2lcm lcm)
\ No newline at end of file
+target_link_libraries(udp2lcm lcm pthread)
diff --git a/udp2lcm/udp.c b/udp2lcm/udp.c
index 6c4bd4a..97fb53e 100644
--- a/udp2lcm/udp.c
+++ b/udp2lcm/udp.c
@@ -1,6 +1,6 @@
 #include "udp2lcm.h"
 
-void udpRecvHandler() {
+void *udpRecvHandler(void *args) {
     int socketfd = -1;
     char buffer[MAX_BUFFER_SIZE];
     struct sockaddr_in serverAddr;
@@ -13,7 +13,7 @@ void udpRecvHandler() {
     printf("udpRecvHandler\n");
     if ((socketfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
         fprintf(stderr, "Error: Failed to create socket\n");
-        return;
+        return NULL;
     }
     memset((char *)&serverAddr, 0, sizeof(serverAddr));
     memset(buffer, 0, sizeof(buffer));
@@ -25,7 +25,7 @@ void udpRecvHandler() {
         -1) {
         fprintf(stderr, "Error: Bind failed\n");
         close(socketfd);
-        return;
+        return NULL;
     }
 
     bytesReceived = recvfrom(socketfd, buffer, MAX_BUFFER_SIZE, 0,
@@ -33,7 +33,7 @@ void udpRecvHandler() {
     if (bytesReceived == -1) {
         fprintf(stderr, "Error: Failed to receive data\n");
         close(socketfd);
-        return;
+        return NULL;
     }
     printf("Start message from %s: %s\n", inet_ntoa(clientAddr.sin_addr),
            buffer);
@@ -57,13 +57,13 @@ void udpRecvHandler() {
     robotCtrlData.iparams[0] = 1;
     robot_control_t_publish(lcm, "ROBOT_CONTROL", &robotCtrlData);
     freeRobotCtrl(&robotCtrlData);
-    // 随即下达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);
+    // // 随即下达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);
 
     /*
      * 开始接收来自手机端的命令
@@ -99,13 +99,13 @@ void udpRecvHandler() {
     }
 }
 
-void udpSendHandler() {
+void *udpSendHandler(void *args) {
     // 创建UDP套接字
     int sockfd = -1;
     struct sockaddr_in serverAddr;
     if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
         fprintf(stderr, "Error: Failed to create socket\n");
-        return;
+        return NULL;
     }
 
     memset((char *)&serverAddr, 0, sizeof(serverAddr));
@@ -114,7 +114,7 @@ void udpSendHandler() {
     if (inet_aton(clientIP, &serverAddr.sin_addr) == 0) {
         fprintf(stderr, "Error: Failed to convert IP address\n");
         close(sockfd);
-        return;
+        return NULL;
     }
 
     while (true) {
@@ -124,4 +124,4 @@ void udpSendHandler() {
         pthread_mutex_unlock(&heartBeatMutex);
         sleep(1);
     }
-}
\ No newline at end of file
+}
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++) {
diff --git a/udp2lcm/udp2lcm.h b/udp2lcm/udp2lcm.h
index 1ab7cf3..e78bc73 100644
--- a/udp2lcm/udp2lcm.h
+++ b/udp2lcm/udp2lcm.h
@@ -33,8 +33,8 @@ extern char clientIP[20];
 extern lcm_t *lcm;
 
 bool LCMInit();
-void udpSendHandler();
-void udpRecvHandler();
+void *udpSendHandler(void *args);
+void *udpRecvHandler(void *args);
 void parseCmd(const char *buffer, int bytesReceived);
 void robotCtrlInit(robot_control_t *robotCtrlData, int64_t utime,
                    int8_t commandid, int8_t robotid, int8_t ndparams,
@@ -44,4 +44,4 @@ void poseHandler(const lcm_recv_buf_t *rbuf, const char *channel,
                  const pose_t *msg, void *userdata);
 int16_t swapEndian(int16_t val);
 
-#endif
\ No newline at end of file
+#endif