From 66623c932119b859d492c5278f3646728148118d Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 6 Jun 2024 23:28:57 +0800 Subject: [PATCH] update --- components/step_motor_ctrl_module/step_motor_ctrl_module.cpp | 10 ++++++---- components/tmc/ic/ztmc5130.cpp | 12 ++++++++++++ components/tmc/ic/ztmc5130.hpp | 9 +++++---- 3 files changed, 23 insertions(+), 8 deletions(-) diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp index bb7292a..1f4f033 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -430,7 +430,7 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio // 如果设备已经在零点,则反向移动一定距离远离零点 if (gpio->getState()) { ZLOGI(TAG, "---------STEP2-------- find edge"); - _rotate(-direction * lookPointVelocity, lookPointDec); + _rotate(-direction * lookPointVelocity, lookPointDec); bool reach_edge = false; while (!m_thread.getExitFlag()) { @@ -495,8 +495,11 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio } void StepMotorCtrlModule::_rotate(int32_t velocity, int32_t acc) { - m_stepM1->set_amax(acc); - m_stepM1->rotate(velocity); + if (velocity > 0) { + m_stepM1->moveToEnd(1, abs(velocity)); + } else { + m_stepM1->moveToEnd(-1, abs(velocity)); + } } /*********************************************************************************************************************** @@ -625,7 +628,6 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to(int32_t tox) { befor_motor_move(); { - m_stepM1->setAcceleration(m_cfg.motor_amax); int32_t motor_pos = 0; inter_forward_kinematics(tox, motor_pos); m_stepM1->moveTo(motor_pos, m_cfg.motor_default_velocity); diff --git a/components/tmc/ic/ztmc5130.cpp b/components/tmc/ic/ztmc5130.cpp index 7b22311..ad72830 100644 --- a/components/tmc/ic/ztmc5130.cpp +++ b/components/tmc/ic/ztmc5130.cpp @@ -198,6 +198,18 @@ void TMC51X0::moveTo(int32_t position, uint32_t velocityMax) { writeInt(TMC5130_VMAX, velocityMax); writeInt(TMC5130_XTARGET, position); } +void TMC51X0::moveToEnd(int32_t direction, uint32_t velocityMax) { + if (direction > 0) { + writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION); + writeInt(TMC5130_VMAX, velocityMax); + writeInt(TMC5130_XTARGET, INT32_MAX); + } else { + writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION); + writeInt(TMC5130_VMAX, velocityMax); + writeInt(TMC5130_XTARGET, INT32_MIN); + } +} + void TMC51X0::moveBy(int32_t relativePosition, uint32_t velocityMax) { // determine actual position and add numbers of ticks to move relativePosition += readInt(TMC5130_XACTUAL); moveTo(relativePosition, velocityMax); diff --git a/components/tmc/ic/ztmc5130.hpp b/components/tmc/ic/ztmc5130.hpp index f6eaa40..bc66b10 100644 --- a/components/tmc/ic/ztmc5130.hpp +++ b/components/tmc/ic/ztmc5130.hpp @@ -14,14 +14,13 @@ extern "C" { namespace iflytop { #define TMC5130_LISTENER_MAX 5 - /** - * @brief + * @brief * 注意事项 - * + * * 1. 6点加速控制只在位置模式下有效,在速度模式下无效 * 2. 速度模式下只有AMAX有效 - * + * */ class Tmc5130RampStat { @@ -116,6 +115,8 @@ class TMC51X0 : public IStepperMotor { virtual void rotate(int32_t velocity); virtual void moveTo(int32_t position, uint32_t velocityMax); + virtual void moveToEnd(int32_t direction, uint32_t velocityMax); + virtual void moveBy(int32_t relativePosition, uint32_t velocityMax); virtual void stop();