diff --git a/a8000_protocol b/a8000_protocol index 9ece9bf..918981e 160000 --- a/a8000_protocol +++ b/a8000_protocol @@ -1 +1 @@ -Subproject commit 9ece9bfe4e730bd668cdd6c4da0a3071de7ede14 +Subproject commit 918981e4b998fa87b498ea06a1d4ac6c37d372c1 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 90a4eb9..203922b 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 @@ -19,8 +19,9 @@ void StepMotorCtrlModule::initialize(int moduleid, TMC51X0* stepM, ZGPIO iotable m_thread.init(TAG, 1024, osPriorityNormal); m_cfg = *cfg; - m_state.enable = false; - m_state.dpos = 0; + m_state.enable = false; + m_state.dpos = 0; + m_state.has_move_to_zero = 0; step_motor_active_cfg(); step_motor_enable(true); @@ -86,6 +87,7 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int MODULE_COMMON_PROCESS_REG_CB(); PROCESS_REG(kreg_step_motor_pos, getnowpos(val), ACTION_NONE); PROCESS_REG(kreg_step_motor_is_enable, REG_GET(m_state.enable), ACTION_NONE); + PROCESS_REG(kreg_step_motor_has_move_zero, REG_GET(m_state.has_move_to_zero), ACTION_NONE); PROCESS_REG(kreg_step_motor_dpos, REG_GET(m_state.dpos), ACTION_NONE); PROCESS_REG(kreg_step_motor_shaft, REG_GET(m_cfg.motor_shaft), REG_SET(m_cfg.motor_shaft)); @@ -129,6 +131,10 @@ int32_t StepMotorCtrlModule::step_motor_enable(int32_t enable) { m_thread.stop(); m_stepM1->enable(enable); m_state.enable = enable; + if (enable == 0 && m_cfg.motor_enable_enc == 0) { + m_state.has_move_to_zero = 0; + } + return 0; } int32_t StepMotorCtrlModule::step_motor_read_pos(int32_t* pos) { return getnowpos(*pos); } @@ -474,6 +480,10 @@ int32_t StepMotorCtrlModule::step_motor_easy_move_to(int32_t tox) { int32_t ecode = check_befor_run(); if (ecode != 0) return ecode; + if (m_state.has_move_to_zero == 0) { + return err::kstep_motor_not_move_to_zero; + } + return do_step_motor_easy_move_to(tox); } int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero() { @@ -641,6 +651,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to_zero() { } while (false); after_motor_move(); if (moveToZeroSuc) { + m_state.has_move_to_zero = 1; setnowpos(0 + m_cfg.motor_dzero - m_cfg.io_trigger_append_distance); } }, @@ -684,6 +695,9 @@ void StepMotorCtrlModule::checkdpos(int32_t expect_dpos) { int32_t StepMotorCtrlModule::step_motor_easy_move_to_zero_point_quick() { // ZLOGI(TAG, "m%d motor_move_to_zero_point_quick", m_id); + if (m_state.has_move_to_zero == 0) { + return err::kstep_motor_not_move_to_zero; + } m_thread.stop(); creg.module_status = 1; m_thread.start( diff --git a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index 0103727..ee68b92 100644 --- a/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/sdk/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -43,6 +43,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { int32_t before_move_pos; int32_t enable; int32_t debugmode; + int32_t has_move_to_zero; } state_t; private: 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 659e2d4..2f39cd1 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 @@ -21,6 +21,7 @@ void XYRobotCtrlModule::initialize(int32_t id, TMC51X0* stepM1, TMC51X0* stepM2, m_thread.init("XYRobotCtrlModule", 1024, osPriorityNormal); module_active_cfg(); xymotor_enable(1); + m_state.has_move_to_zero = 0; } void XYRobotCtrlModule::create_default_cfg(config_t& cfg) { @@ -52,6 +53,10 @@ int32_t XYRobotCtrlModule::xymotor_enable(int32_t enable) { m_stepM1->enable(enable); m_stepM2->enable(enable); m_state.is_enable = enable; + if (enable == 0 && m_cfg.enable_enc == 0) { + m_state.has_move_to_zero = 0; + } + return 0; } int32_t XYRobotCtrlModule::do_public_check() { @@ -126,6 +131,10 @@ int32_t XYRobotCtrlModule::xymotor_move_to(int32_t x, int32_t y) { return err::kxymotor_target_pos_outof_range; } + if (m_state.has_move_to_zero == 0) { + return err::kxymotor_not_move_to_zero; + } + return do_xymotor_move_to(x, y); } int32_t XYRobotCtrlModule::xymotor_move_by(int32_t dx, int32_t dy) { @@ -151,7 +160,7 @@ int32_t XYRobotCtrlModule::xymotor_read_pos(int32_t* x, int32_t* y) { } #define UPDATE_CFG(key) PROCESS_REG(kreg_xyrobot_##key, REG_GET(m_cfg.key), REG_SET(m_cfg.key)) -#define READ_STATE(key) PROCESS_REG(kreg_xyrobot_##key, REG_GET(m_state.key),ACTION_NONE) +#define READ_STATE(key) PROCESS_REG(kreg_xyrobot_##key, REG_GET(m_state.key), ACTION_NONE) int32_t XYRobotCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { int32_t ret = __module_xxx_reg(param_id, read, val); if (ret != 0) { @@ -197,6 +206,8 @@ int32_t XYRobotCtrlModule::__module_xxx_reg(int32_t param_id, bool read, int32_t UPDATE_CFG(pos_devi_tolerance); UPDATE_CFG(io_trigger_append_distance); + + READ_STATE(has_move_to_zero); READ_STATE(is_enable); default: return err::kmodule_not_find_reg; @@ -247,6 +258,7 @@ int32_t XYRobotCtrlModule::do_xymotor_move_to_zero() { after_motor_move(); if (ecode == 0) { setnowpos(0, 0); + m_state.has_move_to_zero = 1; } return; }); 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 89195f8..84e4ee7 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 @@ -59,6 +59,7 @@ class XYRobotCtrlModule : public ZIXYMotor, public ZIModule { int32_t is_enable; int32_t hassync; + int32_t has_move_to_zero; } state_t; private: diff --git a/usrc/project_configs.h b/usrc/project_configs.h index 725cfd5..c6a5e35 100644 --- a/usrc/project_configs.h +++ b/usrc/project_configs.h @@ -1,5 +1,5 @@ #pragma once -#define PC_VERSION 521 +#define PC_VERSION 600 #define PC_MANUFACTURER "http://www.iflytop.com/" #define PC_PROJECT_NAME "a8000_subboard" #define PC_IFLYTOP_ENABLE_OS 1