From e38a0252012050f13658c9302518fc7083a86fde Mon Sep 17 00:00:00 2001
From: bmy <2583236812@qq.com>
Date: Mon, 20 May 2024 12:10:23 +0800
Subject: [PATCH] =?UTF-8?q?fix:=20=E4=BF=AE=E5=A4=8D=E4=BD=8D=E7=BD=AE?=
 =?UTF-8?q?=E8=AE=BE=E7=BD=AE=E6=8C=87=E4=BB=A4=E9=94=99=E8=AF=AF?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/by_motion.c | 17 +++++++++++------
 1 file changed, 11 insertions(+), 6 deletions(-)

diff --git a/app/by_motion.c b/app/by_motion.c
index 45e5826..d808986 100644
--- a/app/by_motion.c
+++ b/app/by_motion.c
@@ -71,6 +71,7 @@ void by_motion_set_speed_m1(int16_t speed)
 
 void by_motion_set_distance(float distance, int16_t speed)
 {
+  // TODO 验证距离是否超限
   if (motion_busy_flag) {
     return;
   }
@@ -79,6 +80,8 @@ void by_motion_set_distance(float distance, int16_t speed)
     speed = -speed;
   }
 
+  position_abs += distance;
+
   if (distance < 0.0f) {
     by_motion_set_speed_m1(-1 * speed);
     distance = -distance;
@@ -87,13 +90,14 @@ void by_motion_set_distance(float distance, int16_t speed)
   }
 
   motion_busy_flag = 1;
-  position_abs += distance;
-  time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s
+  time_via         = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s
   // lwprintf("time_via = %d\r\n", time_via);
+  LOGD("position_abs = %f\r\n", position_abs);
 }
 
 void by_motion_set_distance2(float distance, int16_t speed)
 {
+  // TODO 验证距离是否超限
   if (motion_busy_flag) {
     return;
   }
@@ -106,7 +110,8 @@ void by_motion_set_distance2(float distance, int16_t speed)
     distance = -distance;
   }
 
-  distance = position_abs - distance;
+  distance = distance - position_abs;
+  position_abs += distance;
 
   if (distance < 0.0f) {
     distance = -distance;
@@ -116,8 +121,8 @@ void by_motion_set_distance2(float distance, int16_t speed)
   }
 
   motion_busy_flag = 1;
-  position_abs += distance;
-  time_via = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s
+  time_via         = (int16_t)(distance * 50.0f / ((float)speed * MU)); // 控制频率为 50Hz，控制值单位为 s
+  LOGD("position_abs = %f\r\n", position_abs);
   // lwprintf("time_via = %d\r\n", time_via);
 }
 
@@ -156,7 +161,7 @@ void by_motion_init(void)
   DRV_ENABLE();
 
   // 上电回零
-  DWT_Delay(3000000);
+  DWT_Delay(1000000);
   by_motion_set_speed_m1(-4);
   while (SET == gpio_input_data_bit_read(GPIOB, GPIO_PINS_3)) {
     // 等待复位