From 9a657c8ad1c0212db21bb46256f761b3ab4b856d Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sat, 27 Jul 2024 09:27:02 +0800 Subject: [PATCH] update --- a8000_protocol | 2 +- .../mini_servo_motor_ctrl_module.cpp | 3 +- .../step_motor_ctrl_module.cpp | 23 +++++++++- .../xy_robot_ctrl_module/xy_robot_ctrl_module.cpp | 49 +++++++++++----------- .../xy_robot_ctrl_module/xy_robot_ctrl_module.hpp | 5 ++- 5 files changed, 52 insertions(+), 30 deletions(-) diff --git a/a8000_protocol b/a8000_protocol index f27f273..40ada07 160000 --- a/a8000_protocol +++ b/a8000_protocol @@ -1 +1 @@ -Subproject commit f27f273732bc2e98dbee3ff88897c1db054ae10d +Subproject commit 40ada078fcd4420b6779ba894c5756acf0a071de diff --git a/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp b/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp index 5b58fcf..bbb7632 100644 --- a/sdk/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/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; } diff --git a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp index 506ac63..1d3fd68 100644 --- a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/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(m_stepM1); if (tmc5130 && tmc5130->isTMC5130()) { auto state = tmc5130->getGState(); diff --git a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index 7ad33cc..cd13bec 100644 --- a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/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(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; } diff --git a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp index 03b9535..4a4f1e3 100644 --- a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp +++ b/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: