update

a8000_protocol

@@ -1 +1 @@
-Subproject commit f27f273732bc2e98dbee3ff88897c1db054ae10d
+Subproject commit 40ada078fcd4420b6779ba894c5756acf0a071de

sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -212,8 +212,9 @@ void MiniServoCtrlModule::befor_motor_move() {
 void MiniServoCtrlModule::after_motor_move() {
   if (creg.module_errorcode != 0) {
     creg.module_status = 2;
+  }else {
+    creg.module_status = 0;
   }
-  creg.module_status = 0;
 }
 bool MiniServoCtrlModule::check_when_run() { return true; }
 

sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp

@@ -113,8 +113,8 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int
     PROCESS_REG(kreg_step_motor_enable_enc, REG_GET(m_cfg.motor_enable_enc), REG_SET(m_cfg.motor_enable_enc));
     PROCESS_REG(kreg_step_motor_dzero_pos, REG_GET(m_cfg.motor_dzero), update_dzero(m_cfg.motor_dzero));
 
-    PROCESS_REG(kret_step_motor_pos_devi_tolerance, REG_GET(m_cfg.pos_devi_tolerance), REG_SET(m_cfg.pos_devi_tolerance));
-    PROCESS_REG(kret_step_motor_io_trigger_append_distance, REG_GET(m_cfg.io_trigger_append_distance), REG_SET(m_cfg.io_trigger_append_distance));
+    PROCESS_REG(kreg_step_motor_pos_devi_tolerance, REG_GET(m_cfg.pos_devi_tolerance), REG_SET(m_cfg.pos_devi_tolerance));
+    PROCESS_REG(kreg_step_motor_io_trigger_append_distance, REG_GET(m_cfg.io_trigger_append_distance), REG_SET(m_cfg.io_trigger_append_distance));
 
     default:
       return err::kmodule_not_find_reg;
@@ -261,6 +261,25 @@ bool StepMotorCtrlModule::check_when_run() {
   //
   if (m_state.debugmode) return true;
 
+  if (m_cfg.motor_enable_enc != 0) {
+    vPortEnterCritical();
+
+    int32_t m1enc = m_stepM1->read_enc_val();
+    int32_t m1pos = m_stepM1->getXACTUAL();
+
+    vPortExitCritical();
+
+    int32_t dm1 = abs(m1enc - m1pos);
+
+    if (m_cfg.pos_devi_tolerance != 0) {
+      if (dm1 > m_cfg.pos_devi_tolerance) {
+        ZLOGE(TAG, "motor pos devi %d", dm1);
+        creg.module_errorcode = err::kstep_motor_lost_step;
+        return false;
+      }
+    }
+  }
+
   TMC51X0* tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1);
   if (tmc5130 && tmc5130->isTMC5130()) {
     auto state     = tmc5130->getGState();

sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp

@@ -193,6 +193,8 @@ int32_t XYRobotCtrlModule::__module_xxx_reg(int32_t param_id, bool read, int32_t
     UPDATE_CFG(max_y);
     UPDATE_CFG(run_to_zero_speed);
     UPDATE_CFG(look_zero_edge_speed);
+    UPDATE_CFG(pos_devi_tolerance);
+    UPDATE_CFG(io_trigger_append_distance);
 
     default:
       return err::kmodule_not_find_reg;
@@ -449,35 +451,32 @@ void XYRobotCtrlModule::after_motor_move() {
 
   if (creg.module_errorcode != 0) {
     creg.module_status = 2;
+  } else {
+    creg.module_status = 0;
   }
-  creg.module_status = 0;
 }
 bool XYRobotCtrlModule::check_when_run() {
-  // if (m_state.debugmode) {
-  //   return true;
-  // }
-
-  // TMC51X0* tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1);
-  // if (tmc5130 && tmc5130->isTMC5130()) {
-  //   auto state     = tmc5130->getGState();
-  //   auto devStatus = tmc5130->getDevStatus();
-  //   if (state.reset) {
-  //     ZLOGE(TAG, "motor reset when run");
-  //     setErrorFlag(err::kstep_motor_subic_reset, (*(uint32_t*)&devStatus));
-  //     return false;
-  //   } else if (state.uv_cp) {
-  //     ZLOGE(TAG, "motor uv_cp when run");
-  //     dumpTMC5130Status(&devStatus);
-  //     setErrorFlag(err::kstep_motor_uv_cp, (*(uint32_t*)&devStatus));
-  //     return false;
-  //   } else if (state.drv_err) {
-  //     ZLOGE(TAG, "motor drv_err when run");
-  //     setErrorFlag(err::kstep_motor_drv_err, (*(uint32_t*)&devStatus));
-  //     dumpTMC5130Status(&devStatus);
-  //     return false;
-  //   }
-  // }
+  if (m_cfg.enable_enc != 0) {
+    vPortEnterCritical();
+
+    int32_t m1enc = m_stepM1->read_enc_val();
+    int32_t m2enc = m_stepM2->read_enc_val();
+    int32_t m1pos = m_stepM1->getXACTUAL();
+    int32_t m2pos = m_stepM2->getXACTUAL();
+
+    vPortExitCritical();
 
+    int32_t dm1 = abs(m1enc - m1pos);
+    int32_t dm2 = abs(m2enc - m2pos);
+
+    if (m_cfg.pos_devi_tolerance != 0) {
+      if (dm1 > m_cfg.pos_devi_tolerance || dm2 > m_cfg.pos_devi_tolerance) {
+        ZLOGE(TAG, "pos_devi_tolerance fail m1enc:%d m1pos:%d d1:%d m2enc:%d m2pos:%d d2:%d", m1enc, m1pos, m1enc - m1pos, m2enc, m2pos, m2enc - m2pos);
+        creg.module_errorcode = err::kstep_motor_lost_step;
+        return false;
+      }
+    }
+  }
   return true;
 }
 

sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp

@@ -44,6 +44,8 @@ class XYRobotCtrlModule : public ZIXYMotor, public ZIModule {
     int32_t run_to_zero_speed;
     int32_t look_zero_edge_speed;
 
+    int32_t pos_devi_tolerance;
+    int32_t io_trigger_append_distance;
   } config_t;
 
   typedef struct {
@@ -55,7 +57,8 @@ class XYRobotCtrlModule : public ZIXYMotor, public ZIModule {
     int32_t before_x;
     int32_t before_y;
 
-    bool enable;
+    bool    enable;
+    int32_t hassync;
   } state_t;
 
  private: