支持反转

main/ble_parse_data.c

@@ -6,7 +6,7 @@
 
 #define cmd_length_set_position 5
 #define cmd_length_get_status 2
-#define cmd_length_set_motor_current_size 2 
+#define cmd_length_set_motor_current_size 2
 static double encoder_befor_num;
 
 static bluetooth_processer_t *parse_bluetooth_processer;
@@ -39,7 +39,7 @@ void bluetooth_gatts_try_process_data() {
 
           encoder_befor_num = motor_drive_read_encoder();
           if (encoder_befor_num >= 0) {
-            if (motor_drive_set_packages_ctr(parse_bluetooth_processer->position) == 0) {
+            if (motor_drive_set_packages_ctr(parse_bluetooth_processer->position, parse_bluetooth_processer->direction) == 0) {
               ets_delay_us(50000);
               if (encoder_befor_num == motor_drive_read_encoder()) {
                 ESP_LOGW(BLE_PARSE_DATA_TAG, "motor no turning");
@@ -119,7 +119,7 @@ bool parse_json_to_struct(cJSON *ch) {
       if (strcmp(ch->valuestring, set_motor_current_size) == 0) {
         cmd_length = cmd_length_set_motor_current_size;
       }
-      
+
       cmd_length--;
     }
     if (strcmp(ch->string, "index") == 0) {

main/motor_drive.c

@@ -39,7 +39,7 @@ void motor_drive_turn(int direction, int speed_level, double position) {
   if ((position > 360) || (position <= 0)) {
     ESP_LOGW(MOTOR_DRIVE, "Position out of range");
   }
-  motor_drive_set_packages_ctr(position);
+  motor_drive_set_packages_ctr(position, direction);
   uart_write_bytes(uart_num, "test", strlen("test"));
 }
 
@@ -86,7 +86,7 @@ void motor_drive_set_packages_data_max64bit(uint8_t cmd, uint8_t buffer_data_siz
   motor_drive_buffer_cmd_generate(buffer, cmd, buffer_data_size, buffer_data);
 }
 
-uint8_t motor_drive_set_packages_ctr(double position) {
+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;  //从第五位开始(低位)
@@ -95,21 +95,44 @@ uint8_t motor_drive_set_packages_ctr(double position) {
   char *notify_err = "set size error";
 
   position_int = position * 100;
+  if (direction == 2) {
+    position_int = 0 - position_int;
+  }
 
   if (position_int != 0) {
-    while ((position_int / 0XFF) > 0) {
-      position_remainder = position_int & 0XFF;
-      buffer[position_buffer_size] = position_remainder;
+    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;
-      position_int = position_int >> 8;
-      checksum += position_remainder;
+      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;
     }
-    buffer[position_buffer_size] = position_int;
-    checksum += position_int;
-    checksum = checksum & 0XFF;
-    buffer[9] = checksum;
   }
-
+  
   // Send cmd
   uart_flush(uart_num);
   uart_write_bytes(uart_num, buffer, 10);

main/motor_drive.h

@@ -40,7 +40,7 @@ void motor_drive_uart_init();
 void motor_drive_turn(int direction, int speed_level, double position);
 double motor_drive_read_encoder();
 void motor_drive_set_packages_data_max64bit(uint8_t cmd, uint8_t buffer_data_size, uint64_t buffer_data);
-uint8_t motor_drive_set_packages_ctr(double position);
+uint8_t motor_drive_set_packages_ctr(double position, int direction);
 void motor_drive_hex_to_str(const char *hex, int hex_len, char *str);
 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);