电机转到指定角度

main/main.c

@@ -208,12 +208,12 @@ void processing_uart_rx_data() {
             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,  //
-          //                                  ble_uart_receive_data.speedLevel);
-          //   // 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_run_to_postion(ble_uart_receive_data.direction, ble_uart_receive_data.position,  //
+                                 ble_uart_receive_data.speedLevel);
+            // receipt_json_get_status();
+          }
         }
       }
     }

main/motor_drive.c

@@ -14,6 +14,11 @@
 
 static motor_t *motor_init_structer;
 
+/**
+ * @brief 电机初始化
+ * 
+ * @param motor 初始化的结构体(暂时写死，所以结构体为空)
+ */
 void motor_init(motor_t *motor) {
   motor_init_structer = motor;
   uart_config_t uart_config = {
@@ -31,10 +36,15 @@ 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;
 }
+
+/**
+ * @brief 设置电机零点位置，需电机转动前设置。
+ *
+ */
 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";
+  //   char *notify_err = "Set current size err";
   uart_flush(uart_num);
   uart_write_bytes(uart_num, buffer, 5);
 
@@ -48,7 +58,7 @@ void motor_set_zero_point() {
     // bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
   }
 
-  return ;
+  return;
 }
 
 /**
@@ -61,9 +71,93 @@ uint32_t motor_get_position_degree() { return 0; }
 /**
  * @brief 电机转到多少度
  */
-void motor_run_to_postion(int potion) { return; }
+void motor_run_to_postion(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";
+
+  // forward
+  if ((rotation_direction == 1)) {
+    buffer[5] = 0;
+    checksum += 0;
+  }
+
+  // reverse
+  if ((rotation_direction == 2)) {
+    buffer[5] = 1;
+    checksum += 1;
+  }
+
+  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;
+  }
+
+  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;
+}
+
+/**
+ * @brief 注册回调
+ * 
+ * @param onevent 注册的回调函数
+ */
 void motor_reg_event_cb(motor_on_event_t onevent) { return; }
 
+/**
+ * @brief 读取编码器
+ * 
+ * @return double 编码器当前位置值返回
+ */
 double motor_drive_read_encoder() {
   size_t encoder_buffer_size = 5;
   uint8_t buffer[5] = {0X3E, 0X90, MOTOR_ID, 0X00, 0XCF};
@@ -89,6 +183,13 @@ double motor_drive_read_encoder() {
   return ((double)encoder_data / 100.0);
 }
 
+/**
+ * @brief 控制电机转多少度
+ * 
+ * @param position 角度
+ * @param direction 方向
+ * @return uint8_t 
+ */
 uint8_t motor_drive_set_packages_ctr(double position, int direction) {
   int position_int = 0;
   uint8_t position_remainder = 0;

main/motor_drive.h

@@ -13,7 +13,7 @@ typedef void (*motor_on_event_t)(motor_event_t event);
 void motor_init(motor_t* motor);
 void motor_set_zero_point();
 uint32_t motor_get_position_degree();
-void motor_run_to_postion(int potion);
+void motor_run_to_postion(int rotation_direction, double position, int speed_level);
 void motor_reg_event_cb(motor_on_event_t onevent);
 double motor_drive_read_encoder();
 uint8_t motor_drive_set_packages_ctr(double position, int direction);