diff --git a/.vscode/settings.json b/.vscode/settings.json
index e8b8c52..2f4144d 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -34,6 +34,9 @@
         "uart.h": "c",
         "bitset": "c",
         "chrono": "c",
-        "algorithm": "c"
+        "algorithm": "c",
+        "array": "c",
+        "string": "c",
+        "string_view": "c"
     },
 }
\ No newline at end of file
diff --git a/main/motor_drive.c b/main/motor_drive.c
index 47db783..0789860 100644
--- a/main/motor_drive.c
+++ b/main/motor_drive.c
@@ -42,26 +42,32 @@ void motor_drive_turn(int direction, int speed_level, double position) {
 
 double motor_drive_read_encoder() {
   uint8_t i = 0;
-  uint8_t generate_buffer_size = 0;
+  uint8_t encoder_buffer_size = 0;
   uint8_t buffer[20] = {0};
 
+  uart_flush(uart_num);
   // Generate cmd
-  generate_buffer_size = motor_drive_buffer_cmd_generate(buffer, 0x90, 4, 0X5A97FF00);
-  if (generate_buffer_size == 0){
-      ESP_LOGW(MOTOR_DRIVE, "generate_buffer_size null");
-      return -1;
+  encoder_buffer_size = motor_drive_buffer_cmd_generate(buffer, 0x90, 4, 0X5A97FF00);
+  if (encoder_buffer_size == 0) {
+    ESP_LOGW(MOTOR_DRIVE, "generate_buffer_size null");
+    return -1;
   }
 
   // Send cmd
-  uart_write_bytes(uart_num, (const char *)buffer, generate_buffer_size);
+  uart_write_bytes(uart_num, (const char *)buffer, encoder_buffer_size);
+
+  memset(buffer, 0, sizeof(uint8_t) * 20);
+  ESP_LOGI(MOTOR_DRIVE, "%s", buffer);
+  encoder_buffer_size = 0;
 
   // Wait receive
-  // while (1) {
-  //   len = uart_read_bytes(ECHO_UART_PORT_NUM, data, BUF_SIZE, 20 / portTICK_RATE_MS);
-  // if(len != 0 || timout){
-  // break;
-  // }
-  // }
+  while (1) {
+    encoder_buffer_size = uart_read_bytes(uart_num, buffer, 4, 2000 / portTICK_RATE_MS);
+    if (encoder_buffer_size == 0) {
+      break;
+    }
+    ESP_LOGI(MOTOR_DRIVE, "%s", buffer);
+  }
 
   // Parse receive
   // R_encoder = motor_drive_buffer_cmd_parse();