电机停止，三次比较都相同主动上报，修改之前遗留的bug

main/main.c

@@ -51,7 +51,7 @@ void process_setPosition(cJSON *rxjson) {
   if (cJSON_IsNumber(cJSON_GetObjectItem(rxjson, "direction"))) {
     direction = cJSON_GetNumberValue(cJSON_GetObjectItem(rxjson, "direction"));
   }
-  motor_run_to_postion(1, position, 1);
+  motor_run_to_postion(1, position, 0);
 }
 
 void processrxjson(cJSON *rxjson) {
@@ -90,7 +90,7 @@ void motor_on_event(motor_event_t event) {
     //上报
     //{ "order": "deviceStatusReport", "ble_uart_index": 0, "deviceState": "running", "position":13.9, "deviceException": 0, "deviceExceptionInfo": "", }
     sprintf((char *)buffer, "{ \"order\": \"receipt\", \"code\": 0, \"info\": \"success\", \"index\": %d }", 1);
-    bleuart_notify_send(buffer,sizeof(buffer));
+    bleuart_notify_send(buffer,strlen((char *)buffer));
   }
 }
 

main/motor_drive.c

@@ -1,5 +1,7 @@
 #include "motor_drive.h"
 
+#include <math.h>
+
 #include "driver/uart.h"
 #include "esp_log.h"
 #include "port.h"
@@ -19,6 +21,7 @@ static motor_t *motor_init_structer;
 static motor_on_event_t s_onevent;
 
 static double motor_position;
+static bool motor_cmd_ctr_turn_flag = false;
 
 /**
  * @brief 电机初始化
@@ -78,14 +81,14 @@ void motor_set_zero_point() {
 double motor_get_position_degree() {
   uint16_t turn_angle_data = 0;
   size_t turn_angle_buffer_size = 5;
-  uint8_t buffer[5] = {0X3E, 0X94, MOTOR_ID, 0X00, 0XD3};
+  uint8_t buffer[8] = {0X3E, 0X94, MOTOR_ID, 0X00, 0XD3};
   // char *notify_err = "Turn angle size err";
 
   uart_flush(uart_num);
-  uart_write_bytes(uart_num, buffer, 5);
+  uart_write_bytes(uart_num, buffer, turn_angle_buffer_size);
 
   turn_angle_buffer_size = 0;
-  memset(buffer, 0, sizeof(uint8_t) * 5);
+  memset(buffer, 0, sizeof(buffer));
 
   // Wait receive
   turn_angle_buffer_size = uart_read_bytes(uart_num, buffer, 8, uart_read_time_ms / portTICK_RATE_MS);
@@ -178,6 +181,7 @@ void motor_run_to_postion(int rotation_direction, double position, int speed_lev
   }
 
   motor_position = position;
+  motor_cmd_ctr_turn_flag = true;
 
   // Parse receive
   //   motor_drive_buffer_cmd_parse(buffer);
@@ -201,7 +205,7 @@ void motor_reg_event_cb(motor_on_event_t onevent) {
  */
 static double motor_drive_read_encoder() {
   size_t encoder_buffer_size = 5;
-  uint8_t buffer[5] = {0X3E, 0X90, MOTOR_ID, 0X00, 0XCF};
+  uint8_t buffer[12] = {0X3E, 0X90, MOTOR_ID, 0X00, 0XCF};
   uint16_t encoder_data = 0;
 
   // Send cmd
@@ -209,7 +213,7 @@ static double motor_drive_read_encoder() {
   uart_write_bytes(uart_num, (const char *)buffer, encoder_buffer_size);
 
   encoder_buffer_size = 0;
-  memset(buffer, 0, sizeof(uint8_t) * 5);
+  memset(buffer, 0, sizeof(buffer));
 
   // Wait receive
   encoder_buffer_size = uart_read_bytes(uart_num, buffer, 12, uart_read_time_ms / portTICK_RATE_MS);
@@ -236,7 +240,7 @@ static uint8_t motor_drive_set_packages_ctr(double position, int direction) {
   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};
+  uint8_t buffer[13] = {0x3E, 0XA7, MOTOR_ID, 0X04, 0XEA, 0X00, 0X00, 0X00, 0X00, 0X00};
   //   char *notify_err = "set size error";
 
   position_int = position * 100;
@@ -283,7 +287,7 @@ static uint8_t motor_drive_set_packages_ctr(double position, int direction) {
   uart_write_bytes(uart_num, buffer, 10);
 
   position_buffer_size = 0;
-  memset(buffer, 0, sizeof(uint8_t) * 10);
+  memset(buffer, 0, sizeof(buffer));
 
   // 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);
@@ -298,14 +302,39 @@ static uint8_t motor_drive_set_packages_ctr(double position, int direction) {
   return 0;
 }
 
+bool motor_stop() {
+  static uint32_t time;
+  static double motor_befor_encoder;
+  static double motor_now_encoder;
+  static uint8_t motor_read_encoder_count = 0;
+  if (motor_cmd_ctr_turn_flag && port_haspassedms(time) > 100) {
+    motor_now_encoder = motor_drive_read_encoder();
+    
+    if (motor_now_encoder == motor_befor_encoder) {
+      motor_read_encoder_count = motor_read_encoder_count + 1;
+    } else {
+      motor_befor_encoder = motor_now_encoder;
+    }
+
+    if (motor_read_encoder_count == 3) {
+      motor_read_encoder_count = 0;
+      motor_cmd_ctr_turn_flag = false;
+      return true;
+    }
+
+    time = port_get_ticket();
+  }
+  return false;
+}
+
 /**
  * @brief 电机模块调度(验证电机是否到达指定位置，然后主动上报数据)
  *
  */
 void motor_module_schedule() {
-  // s_onevent(kRunToPosition);  //组包，主动上报
-
-  // time = port_get_ticket();
+  if (motor_stop()) {
+    s_onevent(kRunToPosition);  //组包，主动上报
+  }
 
   return;
 }

main/motor_drive.h

@@ -1,6 +1,7 @@
 #pragma once
 #include <stdint.h>
 #include <string.h>
+#include <stdbool.h>
 
 typedef struct {
   int uartNum;
@@ -18,3 +19,4 @@ void motor_reg_event_cb(motor_on_event_t onevent);
 
 void motor_module_schedule();
 
+bool motor_stop();