支持电机旋转多少度

main/main.c

@@ -186,15 +186,15 @@ void processing_uart_rx_data() {
 
             encoder_befor_num = motor_drive_read_encoder();
             if (encoder_befor_num >= 0) {
-              ESP_LOGI(MAIN_LOG_TAG, "%.2lf", encoder_befor_num);
-              // if (motor_drive_set_packages_ctr(ble_uart_receive_data.position, ble_uart_receive_data.direction) == 0) {
-              //   ets_delay_us(50000);
-              //   if (encoder_befor_num == motor_drive_read_encoder()) {
-              //     ESP_LOGW(MAIN_LOG_TAG, "motor no turning");
-              //   } else {
-              //     ESP_LOGI(MAIN_LOG_TAG, "motor turning");
-              //   }
-              // }
+              // ESP_LOGI(MAIN_LOG_TAG, "%.2lf", encoder_befor_num);
+              if (motor_drive_set_packages_ctr(ble_uart_receive_data.position, ble_uart_receive_data.direction) == 0) {
+                ets_delay_us(50000);
+                if (encoder_befor_num == motor_drive_read_encoder()) {
+                  ESP_LOGW(MAIN_LOG_TAG, "motor no turning");
+                } else {
+                  ESP_LOGI(MAIN_LOG_TAG, "motor turning");
+                }
+              }
             }
 
             // receipt_json_set_position();

main/motor_drive.c

@@ -32,12 +32,14 @@ void motor_init(motor_t *motor) {
   return;
 }
 void motor_set_zero_point() { return; }
+
 /**
  * @brief 返回当前电机所在位置0->360
  *
  * @return uint32_t
  */
 uint32_t motor_get_position_degree() { return 0; }
+
 /**
  * @brief 电机转到多少度
  */
@@ -68,3 +70,70 @@ double motor_drive_read_encoder() {
 
   return ((double)encoder_data / 100.0);
 }
+
+uint8_t motor_drive_set_packages_ctr(double position, int direction) {
+  int position_int = 0;
+  uint8_t position_remainder = 0;
+  uint8_t position_buffer_size = 5;  //从第五位开始(低位)
+  uint8_t checksum = 0;
+  uint8_t buffer[10] = {0x3E, 0XA7, MOTOR_ID, 0X04, 0XEA, 0X00, 0X00, 0X00, 0X00, 0X00};
+//   char *notify_err = "set size error";
+
+  position_int = position * 100;
+  if (direction == 2) {
+    position_int = 0 - position_int;
+  }
+
+  if (position_int != 0) {
+    if (position_int > 0) {  // Positive number
+      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;
+      checksum = checksum & 0XFF;
+      buffer[9] = checksum;
+    } else {  // Negative
+      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;
+      position_buffer_size += 1;
+      while (position_buffer_size != 9) {
+        buffer[position_buffer_size] = 0XFF;
+        position_buffer_size += 1;
+        checksum += 0XFF;
+      }
+      checksum += position_int;
+      checksum = checksum & 0XFF;
+      buffer[9] = checksum;
+    }
+  }
+
+  // Send cmd
+  uart_flush(uart_num);
+  uart_write_bytes(uart_num, buffer, 10);
+
+  position_buffer_size = 0;
+  memset(buffer, 0, sizeof(uint8_t) * 10);
+
+  // 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));
+    return 1;
+  }
+
+  // Parse receive
+  // motor_drive_buffer_cmd_parse(buffer);
+  return 0;
+}

main/motor_drive.h

@@ -12,15 +12,8 @@ typedef void (*motor_on_event_t)(motor_event_t event);
 
 void motor_init(motor_t* motor);
 void motor_set_zero_point();
-/**
- * @brief 返回当前电机所在位置0->360
- *
- * @return uint32_t
- */
 uint32_t motor_get_position_degree();
-/**
- * @brief 电机转到多少度
- */
 void motor_run_to_postion(int potion);
 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);