From 338a4ec12d0fa6ba12d3bd4e693ba6a2dc8edcb7 Mon Sep 17 00:00:00 2001
From: zwsd <zwsdtjl@zwsd.com>
Date: Tue, 12 Jul 2022 11:21:32 +0800
Subject: [PATCH] setPosition

---
 main/gatts_demo.c  | 26 +++++++++++++++++---------
 main/motor_drive.c | 45 +++++++++++++++++++++++++++++++++++++++++++++
 main/motor_drive.h |  7 +++++++
 3 files changed, 69 insertions(+), 9 deletions(-)

diff --git a/main/gatts_demo.c b/main/gatts_demo.c
index 99579e5..3a5d389 100644
--- a/main/gatts_demo.c
+++ b/main/gatts_demo.c
@@ -241,6 +241,9 @@ uint32_t total = 0;
 #define cmd_length_get_status 2
 // #define cmd_length_device_status_report 6
 
+#define set_position "setPosition"
+#define get_status "getStatus"
+
 /***********************************************************************************************************************
  * **********************************************user_function_statement********************************************** *
  ***********************************************************************************************************************/
@@ -943,14 +946,17 @@ void bluetooth_gatts_try_process_data()
             // JSON解析到结构体，如果order更改表示有指令传输进来，并且更改指令标志位(cmd_flag)为true
             if (parse_json_to_struct(json_tmp->child))
             {
-                ESP_LOGI(GATTS_TAG, "order:%s ,index:%d speedLevel:%d position:%f direction:%d", s_bluetooth_processer.order, s_bluetooth_processer.index, s_bluetooth_processer.speed_level, s_bluetooth_processer.position, s_bluetooth_processer.direction);
-                if (strcmp(s_bluetooth_processer.order, "setPosition") == 0)
+                ESP_LOGI(GATTS_TAG, "order:%s ,index:%d speedLevel:%d position:%f direction:%d",
+                         s_bluetooth_processer.order, s_bluetooth_processer.index, s_bluetooth_processer.speed_level,
+                         s_bluetooth_processer.position, s_bluetooth_processer.direction);
+                if (strcmp(s_bluetooth_processer.order, set_position) == 0)
                 {
-                    ESP_LOGI(GATTS_TAG, "setPosition");
+                    ESP_LOGI(GATTS_TAG, set_position);
+                    // motor_cmd_set_position(s_bluetooth_processer.speed_level, s_bluetooth_processer.position, s_bluetooth_processer.direction);
                 }
-                if (strcmp(s_bluetooth_processer.order, "getStatus") == 0)
+                if (strcmp(s_bluetooth_processer.order, get_status) == 0)
                 {
-                    ESP_LOGI(GATTS_TAG, "getStatus");
+                    ESP_LOGI(GATTS_TAG, get_status);
                 }
                 // if (strcmp(s_bluetooth_processer.order, "deviceStatusReport") == 0)
                 // {
@@ -1025,11 +1031,11 @@ bool parse_json_to_struct(cJSON *ch)
         {
             s_bluetooth_processer.order = ch->valuestring;
 
-            if (strcmp(ch->valuestring, "setPosition") == 0)
+            if (strcmp(ch->valuestring, set_position) == 0)
             {
                 cmd_length = cmd_length_set_position;
             }
-            if (strcmp(ch->valuestring, "getStatus") == 0)
+            if (strcmp(ch->valuestring, get_status) == 0)
             {
                 cmd_length = cmd_length_get_status;
             }
@@ -1065,8 +1071,10 @@ bool parse_json_to_struct(cJSON *ch)
     if (cmd_length == 0)
     {
         s_bluetooth_processer.cmd_flag = true;
-    }else{
-        ESP_LOGE(GATTS_TAG,"JSON directive missing or exceeded");
+    }
+    else
+    {
+        ESP_LOGE(GATTS_TAG, "JSON directive missing or exceeded");
     }
 
     return s_bluetooth_processer.cmd_flag;
diff --git a/main/motor_drive.c b/main/motor_drive.c
index f64b79e..9d6f039 100644
--- a/main/motor_drive.c
+++ b/main/motor_drive.c
@@ -1,2 +1,47 @@
 #include "motor_drive.h"
 
+bool motor_cmd_set_position(int speed_level, double position, int direction)
+{
+    int last_direction = direction;
+    if (!motor_validation_set_position_parameters(speed_level, position, direction))
+    {
+        return false;
+    }
+
+    if (direction == 0)
+    {
+        last_direction = motor_find_short_path_direction(position);
+    }
+
+    motor_set_position(speed_level, position, last_direction);
+    return true;
+}
+
+void motor_set_position(int speed_level, double position, int direction)
+{
+}
+
+bool motor_validation_set_position_parameters(int speed_level, double position, int direction)
+{
+    return true;
+}
+
+int motor_find_short_path_direction(double position)
+{
+    double encoder_position = 0.0;
+    encoder_position = motor_read_encoder();
+    if (encoder_position > position)
+    {
+        return 2;
+    }
+    else
+    {
+        return 1;
+    }
+    return;
+}
+
+double motor_read_encoder()
+{
+    return 0.0;
+}
diff --git a/main/motor_drive.h b/main/motor_drive.h
index e69de29..38d83f4 100644
--- a/main/motor_drive.h
+++ b/main/motor_drive.h
@@ -0,0 +1,7 @@
+#include "stdbool.h"
+
+void motor_set_position(int speed_level, double position, int direction);
+bool motor_cmd_set_position(int speed_level, double position, int direction);
+bool motor_validation_set_position_parameters(int speed_level, double position, int direction);
+int motor_find_short_path_direction(double position);
+double motor_read_encoder();
\ No newline at end of file