diff --git a/.clangd b/.clangd
new file mode 100644
index 0000000..ec0b890
--- /dev/null
+++ b/.clangd
@@ -0,0 +1,2 @@
+CompileFlags:
+  CompilationDatabase: build
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000..a1c7a21
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,8 @@
+{
+    "configurations": [
+        {
+            "name": "STM32",
+            "compileCommands": "${workspaceFolder}/build/compile_commands.json"
+        }
+    ]
+}
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..8627b90
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,15 @@
+{
+    "cmake.cmakePath": "cube-cmake",
+    "cmake.configureArgs": [
+        "-DCMAKE_COMMAND=cube-cmake"
+    ],
+    "cmake.preferredGenerators": [
+        "Ninja"
+    ],
+    "stm32cube-ide-clangd.path": "cube",
+    "stm32cube-ide-clangd.arguments": [
+        "starm-clangd",
+        "--query-driver=${env:CUBE_BUNDLE_PATH}/gnu-tools-for-stm32/14.3.1+st.2/bin/arm-none-eabi-gcc*",
+        "--query-driver=${env:CUBE_BUNDLE_PATH}/gnu-tools-for-stm32/14.3.1+st.2/bin/arm-none-eabi-g++*"
+    ]
+}
\ No newline at end of file
diff --git a/Core/Src/main.c b/Core/Src/main.c
index d0146c2..dc984e3 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -63,7 +63,8 @@ static void MPU_Config(void);
 
 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */
-
+int BHBF_robot_init(void);
+void BHBF_robot_task(void);
 /* USER CODE END 0 */
 
 /**
@@ -124,13 +125,14 @@ int main(void)
   MX_ADC2_Init();
   MX_LWIP_Init();
   /* USER CODE BEGIN 2 */
-
+  BHBF_robot_init();
   /* USER CODE END 2 */
 
   /* Infinite loop */
   /* USER CODE BEGIN WHILE */
   while (1)
   {
+    BHBF_robot_task();
     /* USER CODE END WHILE */
 
     /* USER CODE BEGIN 3 */
diff --git a/robot b/robot
index 807c9e4..35274be 160000
--- a/robot
+++ b/robot
@@ -1 +1 @@
-Subproject commit 807c9e444b571a585bb1bcf21abf0e625e59ca10
+Subproject commit 35274be076ac624c61342f01ac755b5ddd324578