Merge branch 'master' of https://git.brisky.space/btl143/BC2D-POS-firmware

app/by_motion.c

@@ -16,6 +16,7 @@ PID_TypeDef pid_m1;
 PID_TypeDef pid_m2;
 uint8_t motion_enable_flag;
 uint8_t motion_busy_flag;
+uint8_t motion_reset_flag;
 int16_t time_via;
 float position_abs;
 
@@ -71,6 +72,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 +81,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 +91,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
   // 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 +111,17 @@ void by_motion_set_distance2(float distance, int16_t speed)
     distance = -distance;
   }
 
-  distance = position_abs - distance;
+  distance = distance - position_abs;
+  position_abs += distance;
+
+  if (5.0 >= position_abs) {
+    position_abs = 0;
+    by_motion_set_speed_m1(-10);
+    motion_busy_flag  = 1;
+    motion_reset_flag = 1;
+    LOGD("RESET position_abs = %f\r\n", position_abs);
+    return;
+  }
 
   if (distance < 0.0f) {
     distance = -distance;
@@ -116,8 +131,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
+  LOGD("position_abs = %f\r\n", position_abs);
   // lwprintf("time_via = %d\r\n", time_via);
 }
 
@@ -156,8 +171,7 @@ void by_motion_init(void)
   DRV_ENABLE();
 
   // 上电回零
-  DWT_Delay(3000000);
+  by_motion_set_speed_m1(-10);
-  by_motion_set_speed_m1(-4);
   while (SET == gpio_input_data_bit_read(GPIOB, GPIO_PINS_3)) {
     // 等待复位
   }
@@ -220,6 +234,15 @@ void by_motion_tmr_handle(void)
     return;
   }
 
+  if (motion_reset_flag) {
+    if (!gpio_input_data_bit_read(GPIOB, GPIO_PINS_3)) {
+      motion_reset_flag = 0;
+      motion_busy_flag  = 0;
+      by_motion_set_speed_m1(0);
+    }
+    return;
+  }
+
   if (time_via > 0) {
     time_via--;
   } else {