From 1bdaaf771370d1e1e7615a5c7524ee8df5bbd71b Mon Sep 17 00:00:00 2001
From: zwsd <zwsdtjl@zwsd.com>
Date: Mon, 25 Jul 2022 16:22:17 +0800
Subject: [PATCH] =?UTF-8?q?=E8=AF=BB=E5=8F=96=E7=BC=96=E7=A0=81=E5=99=A8?=
 =?UTF-8?q?=E6=8E=A5=E5=8F=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .vscode/settings.json | 29 ++++++++++++++++++++++++++++-
 main/main.c           |  4 +++-
 main/motor_drive.c    | 28 +++++++++++++---------------
 main/motor_drive.h    |  2 +-
 4 files changed, 45 insertions(+), 18 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 2f4144d..e67a294 100644
--- a/.vscode/settings.json
+++ b/.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"
     },
 }
\ No newline at end of file
diff --git a/main/main.c b/main/main.c
index 742e5b5..4ac0ac0 100644
--- a/main/main.c
+++ b/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;
     }
 
diff --git a/main/motor_drive.c b/main/motor_drive.c
index 0789860..fe1bc67 100644
--- a/main/motor_drive.c
+++ b/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;
diff --git a/main/motor_drive.h b/main/motor_drive.h
index df3d03c..c1f55f4 100644
--- a/main/motor_drive.h
+++ b/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);
\ No newline at end of file