v1.0

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;
 }