diff --git a/main/ble_parse_data.c b/main/ble_parse_data.c
index 83f455a..4bc2f64 100644
--- a/main/ble_parse_data.c
+++ b/main/ble_parse_data.c
@@ -37,17 +37,19 @@ void bluetooth_gatts_try_process_data() {
           parse_bluetooth_processer->auto_report_flag = true;
           ESP_LOGI(BLE_PARSE_DATA_TAG, set_position);
 
-          encoder_befor_num = motor_drive_read_encoder();
-          if (encoder_befor_num >= 0) {
-            if (motor_drive_set_packages_ctr(parse_bluetooth_processer->position, parse_bluetooth_processer->direction) == 0) {
-              ets_delay_us(50000);
-              if (encoder_befor_num == motor_drive_read_encoder()) {
-                ESP_LOGW(BLE_PARSE_DATA_TAG, "motor no turning");
-              } else {
-                ESP_LOGI(BLE_PARSE_DATA_TAG, "motor turning");
-              }
-            }
-          }
+          motor_drive_set_motor_to_angle(1,180,0);
+
+          // encoder_befor_num = motor_drive_read_encoder();
+          // if (encoder_befor_num >= 0) {
+          //   if (motor_drive_set_packages_ctr(parse_bluetooth_processer->position, parse_bluetooth_processer->direction) == 0) {
+          //     ets_delay_us(50000);
+          //     if (encoder_befor_num == motor_drive_read_encoder()) {
+          //       ESP_LOGW(BLE_PARSE_DATA_TAG, "motor no turning");
+          //     } else {
+          //       ESP_LOGI(BLE_PARSE_DATA_TAG, "motor turning");
+          //     }
+          //   }
+          // }
 
           // receipt_json_set_position();
         }
diff --git a/main/motor_drive.c b/main/motor_drive.c
index 96b3a28..a3fcb0e 100644
--- a/main/motor_drive.c
+++ b/main/motor_drive.c
@@ -132,7 +132,7 @@ uint8_t motor_drive_set_packages_ctr(double position, int direction) {
       buffer[9] = checksum;
     }
   }
-  
+
   // Send cmd
   uart_flush(uart_num);
   uart_write_bytes(uart_num, buffer, 10);
@@ -269,70 +269,75 @@ double motor_drive_read_single_turn_angle() {
   return ((double)turn_angle_data / 100.0);
 }
 
-// void motor_drive_set_motor_to_angle(uint8_t rotation_direction, double position, uint8_t speed_level) {
-//   int position_int = 0;
-//   uint8_t position_remainder = 0;
-//   uint8_t position_buffer_size = 6;  //从第五位开始(低位)
-//   uint8_t checksum = 0;
-//   uint32_t limit_speed = 0;
-//   uint8_t buffer[14] = {0x3E, 0XA6, MOTOR_ID, 0X08, 0XED, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00};
-//   char *notify_err = "set size error";
-
-//   if ((rotation_direction == 0X00) || (rotation_direction == 0X01)) {
-//     buffer[5] = rotation_direction;
-//     checksum += rotation_direction;
-//   }
-
-//   position_int = position * 100;
-//   position_int = position_int % 36000;
-
-//   if (position_int != 0) {
-//     while ((position_int / 0XFF) > 0) {
-//       position_remainder = position_int & 0XFF;
-//       buffer[position_buffer_size] = position_remainder;
-//       position_buffer_size += 1;
-//       position_int = position_int >> 8;
-//       checksum += position_remainder;
-//     }
-//     buffer[position_buffer_size] = position_int;
-//     checksum += position_int;
-//   }
-
-//   if (speed_level <= 9) {
-//     limit_speed = speed_level * 360;
-//     position_buffer_size = 9;
-//   }
-
-//   while (limit_speed != 0) {
-//     while ((limit_speed / 0XFF) > 0) {
-//       position_remainder = limit_speed & 0XFF;
-//       buffer[position_buffer_size] = position_remainder;
-//       position_buffer_size += 1;
-//       limit_speed = limit_speed >> 8;
-//       checksum += position_remainder;
-//     }
-//     buffer[position_buffer_size] = limit_speed;
-//     checksum += limit_speed;
-//   }
-
-//   checksum = checksum & 0XFF;
-//   buffer[13] = checksum;
-
-// Send cmd
-// uart_flush(uart_num);
-// uart_write_bytes(uart_num, buffer, 14);
-
-// position_buffer_size = 0;
-// memset(buffer, 0, sizeof(uint8_t) * 14);
-
-// Wait uart receive,if time out return error and output log
-// position_buffer_size = uart_read_bytes(uart_num, buffer, 13, uart_read_time_ms / portTICK_RATE_MS);
-// if (position_buffer_size != 13 || buffer[0] != 0X3E) {
-//   ESP_LOGW(MOTOR_DRIVE, "set motor size error ,buffer_size:%d,buffer[0] = 0X%x", position_buffer_size, buffer[0]);
-//   bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
-// }
-
-// Parse receive
-// motor_drive_buffer_cmd_parse(buffer);
-//   return;
-// }
\ No newline at end of file
+void motor_drive_set_motor_to_angle(int rotation_direction, double position, int speed_level) {
+  int position_int = 0;
+  uint8_t position_remainder = 0;
+  uint8_t position_buffer_size = 6;  //从第五位开始(低位)
+  uint8_t checksum = 0;
+  uint32_t limit_speed = 0;
+  uint8_t buffer[14] = {0x3E, 0XA6, MOTOR_ID, 0X08, 0XED, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00};
+  char *notify_err = "set size error";
+
+  if ((rotation_direction >= 0) && (rotation_direction < 2)) {
+    buffer[5] = rotation_direction;
+    checksum += rotation_direction;
+  }
+
+  position_int = position * 100;
+  position_int = position_int % 36000;
+
+  if (position_int != 0) {
+    while ((position_int / 0X100) > 0) {
+      position_remainder = position_int & 0XFF;
+      buffer[position_buffer_size] = position_remainder;
+      position_buffer_size += 1;
+      position_int = position_int >> 8;
+      checksum += position_remainder;
+    }
+    buffer[position_buffer_size] = position_int;
+    checksum += position_int;
+  }
+
+  if (speed_level <= 9) {
+    limit_speed = speed_level * 360;
+    position_buffer_size = 9;
+  }
+
+  for (uint8_t i = 0; i < 14; i++) {
+    /* code */
+    ESP_LOGI(MOTOR_DRIVE, "buffer[%d] = %d", i, buffer[i]);
+  }
+
+  if (limit_speed != 0) {
+    while ((limit_speed / 0X100) > 0) {
+      position_remainder = limit_speed & 0XFF;
+      buffer[position_buffer_size] = position_remainder;
+      position_buffer_size += 1;
+      limit_speed = limit_speed >> 8;
+      checksum += position_remainder;
+    }
+    buffer[position_buffer_size] = limit_speed;
+    checksum += limit_speed;
+  }
+
+  checksum = checksum & 0XFF;
+  buffer[13] = checksum;
+
+  // Send cmd
+  uart_flush(uart_num);
+  uart_write_bytes(uart_num, buffer, 14);
+
+  position_buffer_size = 0;
+  memset(buffer, 0, sizeof(uint8_t) * 14);
+
+  // Wait uart receive, if time out return error and output log
+  position_buffer_size = uart_read_bytes(uart_num, buffer, 13, uart_read_time_ms / portTICK_RATE_MS);
+  if (position_buffer_size != 13 || buffer[0] != 0X3E) {
+    ESP_LOGW(MOTOR_DRIVE, "set motor size error ,buffer_size:%d,buffer[0] = 0X%x", position_buffer_size, buffer[0]);
+    bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
+  }
+
+  // Parse receive
+  motor_drive_buffer_cmd_parse(buffer);
+  return;
+}
\ No newline at end of file
diff --git a/main/motor_drive.h b/main/motor_drive.h
index e2ecdac..12cabbf 100644
--- a/main/motor_drive.h
+++ b/main/motor_drive.h
@@ -46,4 +46,4 @@ size_t motor_drive_buffer_cmd_generate(uint8_t *buffer, uint8_t cmd, uint8_t buf
 void motor_drive_buffer_cmd_parse(uint8_t *buffer);
 u_int8_t motor_drive_set_motor_current_size();
 double motor_drive_read_single_turn_angle();
-// void motor_drive_set_motor_to_angle(uint8_t rotation_direction, double position, uint8_t speed_level);
\ No newline at end of file
+void motor_drive_set_motor_to_angle(int rotation_direction, double position, int speed_level);
\ No newline at end of file