From 3821317ce8e37ff0d8260cadc4f5af7628dea017 Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Thu, 2 Nov 2023 00:57:31 +0800
Subject: [PATCH] v1.0

---
 usrc/intelligent_winding_robot_ctrl.cpp | 25 ++++++++++++++-----------
 1 file changed, 14 insertions(+), 11 deletions(-)

diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp
index 6cdebe7..ddc6a93 100644
--- a/usrc/intelligent_winding_robot_ctrl.cpp
+++ b/usrc/intelligent_winding_robot_ctrl.cpp
@@ -39,7 +39,7 @@ class WidthDetector {
   APPDM*                                 m_dm;
 
   int32_t m_bullet_distance        = 33;  // 理论值33,这里取保守值20
-  int32_t m_bullet_full_distance   = 154;
+  int32_t m_bullet_full_distance   = 144;
   int32_t lastDistanceChangeTicket = 0;
 
  public:
@@ -56,7 +56,7 @@ class WidthDetector {
       if (g_detectGpio.getState() != nowstate) {
         return m_lastState;
       }
-      osDelay(3);
+      osDelay(1);
     }
     m_lastState = nowstate;
     return m_lastState;
@@ -98,17 +98,17 @@ class WidthDetector {
     }
   }
 
-  void m14_raoxiantance_move_to_reset() { m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 500, 0); }
+  void m14_raoxiantance_move_to_reset() { m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 330, 0); }
   void start_run_back() {
-    m_dm->motor_move_to(14, cfg->m14_raoxiantance_reset_pos, 30, 0);
+    m_dm->motor_move_to(14, cfg->m14_raoxiantance_reset_pos, 330, 0);
     g_isrunning = true;
   }
   void start_run_forward_slow() {
-    m_dm->motor_move_to(14, cfg->m14_raoxiantance_tance_zero_pos + 100, 30, 0);
+    m_dm->motor_move_to(14, cfg->m14_raoxiantance_tance_zero_pos , 330, 0);
     g_isrunning = true;
   }
   void start_run_forward() {
-    m_dm->motor_move_to(14, cfg->m14_raoxiantance_tance_zero_pos + 100, 500, 0);
+    m_dm->motor_move_to(14, cfg->m14_raoxiantance_tance_zero_pos , 330, 0);
     g_isrunning = true;
   }
   void stop_run() {
@@ -123,6 +123,7 @@ class WidthDetector {
 
     int32_t enterticket = zos_get_tick();
 
+
     start_run_forward();
     while (true) {
       if (g_isrunning && getDetectGPIOState() != 0) {
@@ -153,7 +154,7 @@ class WidthDetector {
       }
       zos_delay(3);
     }
-
+    lastDistanceChangeTicket = zos_get_tick();
     m_detect_thread.start([this, forward]() {
       if (!forward) {
         while (!m_detect_thread.getExitFlag()) {
@@ -166,6 +167,7 @@ class WidthDetector {
           } else if (!g_isrunning && getDetectGPIOState() != 0) {
             start_run_back();
           }
+          // ZLOGI(TAG, "%d",getDetectGPIOState());
           zos_delay(3);
         }
       } else {
@@ -188,7 +190,7 @@ class WidthDetector {
   void stopDetect() {
     m_detect_thread.stop();
 
-    m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 500, 0);
+    m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 330, 0);
     wait_module_idle(14, 0);
   }
 
@@ -197,7 +199,7 @@ class WidthDetector {
   bool isRemoveLineEnd() { return g_nowdpos < m_bullet_distance + 10; }
 
   bool distanceIsStopChange() {
-    if (zos_haspassedms(lastDistanceChangeTicket) > 10 * 1000) {
+    if (zos_haspassedms(lastDistanceChangeTicket) > 20 * 1000) {
       return true;
     }
     return false;
@@ -244,7 +246,7 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() {
   cfg.m13_yaxian_forward_reset_pos  = 1015;
   cfg.m13_yaxian_backward_reset_pos = 2885;
   cfg.m13_jiaxian_clamp_direction   = -1;
-  cfg.m13_jiaxian_clamp_torque      = 600;
+  cfg.m13_jiaxian_clamp_torque      = 330;
 
   cfg.m14_raoxiantance_reset_pos      = 2047;
   cfg.m14_raoxiantance_tance_zero_pos = 2829;
@@ -350,6 +352,8 @@ int32_t IntelligentWindingRobotCtrl::stop_all_module() {
 
 int32_t IntelligentWindingRobotCtrl::disable_all_module() {
   ZLOGI(TAG, "m_dm->motor_enable(2, 0)");
+  stop_all_module();
+
   m_dm->motor_enable(2, 0);
   m_dm->xymotor_enable(3, 0);
   m_dm->motor_enable(4, 0);
@@ -364,7 +368,6 @@ int32_t IntelligentWindingRobotCtrl::disable_all_module() {
   m_dm->motor_enable(22, 0);
   m_dm->motor_enable(23, 0);
 
-  stop_all_module();
 
   return 0;
 }