读取编码器接口

.vscode/settings.json

@@ -37,6 +37,33 @@
         "algorithm": "c",
         "array": "c",
         "string": "c",
-        "string_view": "c"
+        "string_view": "c",
+        "*.ipp": "c",
+        "*.inc": "c",
+        "atomic": "c",
+        "strstream": "c",
+        "*.tcc": "c",
+        "codecvt": "c",
+        "cstddef": "c",
+        "cstdio": "c",
+        "cstring": "c",
+        "cwchar": "c",
+        "vector": "c",
+        "functional": "c",
+        "memory": "c",
+        "memory_resource": "c",
+        "optional": "c",
+        "tuple": "c",
+        "type_traits": "c",
+        "random": "c",
+        "fstream": "c",
+        "initializer_list": "c",
+        "new": "c",
+        "stdexcept": "c",
+        "system_error": "c",
+        "thread": "c",
+        "ratio": "c",
+        "typeinfo": "c",
+        "utility": "c"
     },
 }

main/main.c

@@ -55,6 +55,7 @@ void app_main(void) {
   motor_drive_uart_init();
   // ble_uart_init(UART_NUM_1, 1, 2);
   // char* test_str = "This is a test string.\n";
+  double encoder = 0.0;
 
   while (true) {
     bluetooth_gatts_try_process_data();
@@ -69,7 +70,8 @@ void app_main(void) {
     // }
 
     if (s_bluetooth_processer.auto_report_flag) {
-      motor_drive_read_encoder();
+      encoder = motor_drive_read_encoder();
+      ESP_LOGI("test", "encoder :%.2lf",encoder);
       s_bluetooth_processer.auto_report_flag = false;
     }
 

main/motor_drive.c

@@ -41,11 +41,10 @@ void motor_drive_turn(int direction, int speed_level, double position) {
 }
 
 double motor_drive_read_encoder() {
-  uint8_t i = 0;
-  uint8_t encoder_buffer_size = 0;
+  size_t encoder_buffer_size = 0;
   uint8_t buffer[20] = {0};
+  uint16_t encoder_data = 0;
 
-  uart_flush(uart_num);
   // Generate cmd
   encoder_buffer_size = motor_drive_buffer_cmd_generate(buffer, 0x90, 4, 0X5A97FF00);
   if (encoder_buffer_size == 0) {
@@ -53,28 +52,27 @@ double motor_drive_read_encoder() {
     return -1;
   }
 
+  uart_flush(uart_num);
   // Send cmd
   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;
+  memset(buffer, 0, sizeof(uint8_t) * 20);
 
   // Wait receive
-  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);
+  encoder_buffer_size = uart_read_bytes(uart_num, buffer, 12, 2000 / portTICK_RATE_MS);
+  if (encoder_buffer_size != 12 || buffer[0] != 0X3E) {
+    ESP_LOGW(MOTOR_DRIVE, "encoder size:%d,buffer[0] = 0X%x", encoder_buffer_size, buffer[0]);
+    return -1;
   }
 
   // Parse receive
-  // R_encoder = motor_drive_buffer_cmd_parse();
+  motor_drive_buffer_cmd_parse(buffer);
+
+  encoder_data = buffer[5] + (buffer[6] << 8);
 
   // parse motor usart
-  // return R_encoder;
-  return 0.0;
+  return ((double)encoder_data/100);
 }
 
 void motor_drive_set_packages_data_max64bit(uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data) {
@@ -125,7 +123,7 @@ void motor_drive_hex_to_str(char *hex, int hex_len, char *str) {
   }
 }
 
-uint8_t motor_drive_buffer_cmd_generate(uint8_t *buffer, uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data) {
+size_t motor_drive_buffer_cmd_generate(uint8_t *buffer, uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data) {
   uint8_t i = 0;
   uint8_t checksum = 0;
   uint8_t buffer_data_uint8 = 0;

main/motor_drive.h

@@ -42,5 +42,5 @@ double motor_drive_read_encoder();
 void motor_drive_set_packages_data_max64bit(uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data);
 void motor_drive_set_packages_ctr(double position);
 void motor_drive_hex_to_str(char *hex, int hex_len, char *str);
-uint8_t motor_drive_buffer_cmd_generate(uint8_t *buffer, uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data);
+size_t motor_drive_buffer_cmd_generate(uint8_t *buffer, uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data);
 void motor_drive_buffer_cmd_parse(uint8_t *buffer);