From ec3b4a335d62fc4e920e200c3d7cb2463d024d64 Mon Sep 17 00:00:00 2001
From: zwsd <zwsdtjl@zwsd.com>
Date: Sun, 24 Jul 2022 17:23:42 +0800
Subject: [PATCH] update

---
 main/main.c        |  4 +++-
 main/motor_drive.c | 37 ++++++++++++++++++++++++++++++++-----
 main/motor_drive.h |  2 +-
 3 files changed, 36 insertions(+), 7 deletions(-)

diff --git a/main/main.c b/main/main.c
index 6bc950c..4ff992a 100644
--- a/main/main.c
+++ b/main/main.c
@@ -68,8 +68,10 @@ void app_main(void) {
     //   s_bluetooth_processer.motor_drive_turn_flag = false;
     // }
 
+    motor_drive_read_encoder();
+
     // motor_drive_set_packages_ctr(35.68);
-    motor_drive_set_packages_data_max64bit(0x34,4,0X76482534);
+    // motor_drive_set_packages_data_max64bit(0x34,4,0X76482534);
 
     // uart_write_bytes(UART_NUM_2, (const char*)test_str, strlen(test_str));
     // ets_delay_us(1000000);
diff --git a/main/motor_drive.c b/main/motor_drive.c
index 71215d9..49f595a 100644
--- a/main/motor_drive.c
+++ b/main/motor_drive.c
@@ -40,12 +40,35 @@ void motor_drive_turn(int direction, int speed_level, double position) {
   uart_write_bytes(uart_num, "test", strlen("test"));
 }
 
-double motor_drive_read_encoder() { return 0.0; }
+double motor_drive_read_encoder() {
+  uint8_t buffer[20] = {0};
+
+  // Generate cmd
+  motor_drive_buffer_cmd_generate(buffer, 0x90, 0, 0);
+
+  // Send cmd
+  // uart_write_bytes(UART_NUM_2, (const char*)test_str, strlen(test_str));
+
+  // Wait receive
+  // while (1) {
+  //   len = uart_read_bytes(ECHO_UART_PORT_NUM, data, BUF_SIZE, 20 / portTICK_RATE_MS);
+  // if(len != 0 || timout){
+  // break;
+  // }
+  // }
+
+  // Parse receive
+  // R_encoder = motor_drive_buffer_cmd_parse();
+
+  // parse motor usart
+  // return R_encoder;
+  return 0.0;
+}
 
 void motor_drive_set_packages_data_max64bit(uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data) {
   uint8_t buffer[20] = {0};
 
-  motor_drive_buffer_init(buffer, cmd, buffer_data_size, buffer_data);
+  motor_drive_buffer_cmd_generate(buffer, cmd, buffer_data_size, buffer_data);
 }
 
 void motor_drive_set_packages_ctr(double position) {
@@ -90,11 +113,12 @@ void motor_drive_hex_to_str(char *hex, int hex_len, char *str) {
   }
 }
 
-void motor_drive_buffer_init(uint8_t *buffer, uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data) {
+void 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;
   uint8_t strbuffer[20] = {'\0'};
+  uint8_t hex_to_str_size = buffer_data_size;
   if (buffer == NULL) {
     ESP_LOGW(MOTOR_DRIVE, "buffer nil ,init error");
     return;
@@ -106,6 +130,7 @@ void motor_drive_buffer_init(uint8_t *buffer, uint8_t cmd, uint8_t buffer_data_s
   buffer[4] = (0X3E + cmd + 0X1 + buffer_data_size) % 255;
 
   if (buffer_data_size > 0) {
+    hex_to_str_size = buffer_data_size + 1;
     for (i = 0; i < buffer_data_size; i++) {
       buffer_data_uint8 = buffer_data;
       buffer_data = buffer_data >> 8;
@@ -115,6 +140,8 @@ void motor_drive_buffer_init(uint8_t *buffer, uint8_t cmd, uint8_t buffer_data_s
     buffer[5 + buffer_data_size] = (checksum % 255);
   }
 
-  motor_drive_hex_to_str((char *)buffer, (5 + buffer_data_size + 1), (char *)strbuffer);
+  motor_drive_hex_to_str((char *)buffer, (5 + hex_to_str_size), (char *)strbuffer);
   ESP_LOGI(MOTOR_DRIVE, "%s", strbuffer);
-}
\ No newline at end of file
+}
+
+void motor_drive_buffer_cmd_parse(uint8_t *buffer) {}
\ No newline at end of file
diff --git a/main/motor_drive.h b/main/motor_drive.h
index 9d25683..5cfeab0 100644
--- a/main/motor_drive.h
+++ b/main/motor_drive.h
@@ -42,4 +42,4 @@ 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);
-void motor_drive_buffer_init(uint8_t *buffer, uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data);
\ No newline at end of file
+void motor_drive_buffer_cmd_generate(uint8_t *buffer, uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data);
\ No newline at end of file