设置当前位置为零点

main/main.c

@@ -203,11 +203,11 @@ void processing_uart_rx_data() {
           //   ESP_LOGI(MAIN_LOG_TAG, get_status);
           //   receipt_json_get_status();
           // }
-          // if (strcmp(ble_uart_receive_data.order, set_motor_current_size) == 0) {
-          //   ESP_LOGI(MAIN_LOG_TAG, set_motor_current_size);
-          //   motor_drive_set_motor_current_size();
-          //   // receipt_json_get_status();
-          // }
+          if (strcmp(ble_uart_receive_data.order, set_motor_current_size) == 0) {
+            ESP_LOGI(MAIN_LOG_TAG, set_motor_current_size);
+            motor_set_zero_point();
+            // receipt_json_get_status();
+          }
           // if (strcmp(ble_uart_receive_data.order, set_motor_to_position) == 0) {
           //   ESP_LOGI(MAIN_LOG_TAG, set_motor_to_position);
           //   motor_drive_set_motor_to_angle(ble_uart_receive_data.direction, ble_uart_receive_data.position,  //

main/motor_drive.c

@@ -31,7 +31,25 @@ void motor_init(motor_t *motor) {
   ESP_ERROR_CHECK(uart_set_pin(uart_num, tx_io_num, rx_io_num, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
   return;
 }
-void motor_set_zero_point() { return; }
+void motor_set_zero_point() {
+  size_t set_current_buffer_size = 5;
+  uint8_t buffer[5] = {0X3E, 0X19, MOTOR_ID, 0X00, 0X58};
+//   char *notify_err = "Set current size err";
+  uart_flush(uart_num);
+  uart_write_bytes(uart_num, buffer, 5);
+
+  set_current_buffer_size = 0;
+  memset(buffer, 0, sizeof(uint8_t) * 5);
+
+  // Wait receive
+  set_current_buffer_size = uart_read_bytes(uart_num, buffer, 5, uart_read_time_ms / portTICK_RATE_MS);
+  if (set_current_buffer_size != 5 || buffer[0] != 0X3E) {
+    ESP_LOGW(MOTOR_DRIVE, "Set current size:%d,buffer[0] = 0X%x", set_current_buffer_size, buffer[0]);
+    // bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
+  }
+
+  return ;
+}
 
 /**
  * @brief 返回当前电机所在位置0->360