From 6060bcb657307ae7c0904d1b2288035d5793079b Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 6 Jun 2024 22:12:08 +0800 Subject: [PATCH] update --- .../step_motor_ctrl_module.cpp | 87 ++++++++-------------- .../step_motor_ctrl_module.hpp | 22 ++---- components/tmc/basic/tmc_ic_interface.hpp | 4 + components/tmc/ic/ztmc5130.cpp | 9 +++ components/tmc/ic/ztmc5130.hpp | 19 +++++ 5 files changed, 72 insertions(+), 69 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 ed9995f..bb7292a 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -10,7 +10,7 @@ using namespace iflytop; #define TAG "SMCM" -void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO iotable[], int nio, config_t* cfg) { +void StepMotorCtrlModule::initialize(int moduleid, TMC51X0* stepM, ZGPIO iotable[], int nio, config_t* cfg) { m_id = moduleid; m_stepM1 = stepM; @@ -51,24 +51,9 @@ void StepMotorCtrlModule::create_default_cfg(config_t& cfg) { cfg.stepmotor_ihold = 1; cfg.stepmotor_irun = 3; cfg.stepmotor_iholddelay = 100; - cfg.motor_default_acc = 300; - cfg.motor_default_dec = 300; cfg.motor_default_velocity = 500; - - cfg.motor_run_to_zero_speed = 100; - cfg.motor_run_to_zero_dec = 600; - - cfg.motor_look_zero_edge_speed = 100; - cfg.motor_look_zero_edge_dec = 600; - - // cfg.motor_vstart = 2000; - // cfg.motor_a1 = ; - // cfg.motor_amax = 300; - // cfg.motor_v1 = ; - // cfg.motor_dmax = 300; - // cfg.motor_d1 = ; - // cfg.motor_vstop = 2000; - // cfg.motor_tzerowait = ; + cfg.motor_run_to_zero_speed = 100; + cfg.motor_look_zero_edge_speed = 100; } /*********************************************************************************************************************** @@ -77,24 +62,6 @@ void StepMotorCtrlModule::create_default_cfg(config_t& cfg) { int32_t StepMotorCtrlModule::module_xxx_reg(int32_t param_id, bool read, int32_t& val) { int32_t suc = pri_module_xxx_reg(param_id, read, val); if (suc != 0) return suc; - switch (param_id) { - case kreg_step_motor_shaft: - case kreg_step_motor_one_circle_pulse: - case kreg_step_motor_one_circle_pulse_denominator: - case kreg_step_motor_default_velocity: - case kreg_step_motor_default_acc: - case kreg_step_motor_default_dec: - case kreg_step_motor_run_to_zero_speed: - case kreg_step_motor_run_to_zero_dec: - case kreg_step_motor_look_zero_edge_speed: - case kreg_step_motor_look_zero_edge_dec: - case kreg_step_motor_ihold: - case kreg_step_motor_irun: - case kreg_step_motor_iholddelay: - case kreg_step_motor_max_d: - case kreg_step_motor_min_d: - return 0; - } return suc; } int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int32_t& val) { @@ -106,12 +73,8 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int PROCESS_REG(kreg_step_motor_one_circle_pulse, REG_GET(m_cfg.motor_one_circle_pulse), REG_SET(m_cfg.motor_one_circle_pulse)); PROCESS_REG(kreg_step_motor_one_circle_pulse_denominator, REG_GET(m_cfg.motor_one_circle_pulse_denominator), REG_SET(m_cfg.motor_one_circle_pulse_denominator)); PROCESS_REG(kreg_step_motor_default_velocity, REG_GET(m_cfg.motor_default_velocity), REG_SET(m_cfg.motor_default_velocity)); - PROCESS_REG(kreg_step_motor_default_acc, REG_GET(m_cfg.motor_default_acc), REG_SET(m_cfg.motor_default_acc)); - PROCESS_REG(kreg_step_motor_default_dec, REG_GET(m_cfg.motor_default_dec), REG_SET(m_cfg.motor_default_dec)); PROCESS_REG(kreg_step_motor_run_to_zero_speed, REG_GET(m_cfg.motor_run_to_zero_speed), REG_SET(m_cfg.motor_run_to_zero_speed)); - PROCESS_REG(kreg_step_motor_run_to_zero_dec, REG_GET(m_cfg.motor_run_to_zero_dec), REG_SET(m_cfg.motor_run_to_zero_dec)); PROCESS_REG(kreg_step_motor_look_zero_edge_speed, REG_GET(m_cfg.motor_look_zero_edge_speed), REG_SET(m_cfg.motor_look_zero_edge_speed)); - PROCESS_REG(kreg_step_motor_look_zero_edge_dec, REG_GET(m_cfg.motor_look_zero_edge_dec), REG_SET(m_cfg.motor_look_zero_edge_dec)); PROCESS_REG(kreg_step_motor_ihold, REG_GET(m_cfg.stepmotor_ihold), REG_SET(m_cfg.stepmotor_ihold)); PROCESS_REG(kreg_step_motor_irun, REG_GET(m_cfg.stepmotor_irun), REG_SET(m_cfg.stepmotor_irun)); PROCESS_REG(kreg_step_motor_iholddelay, REG_GET(m_cfg.stepmotor_iholddelay), REG_SET(m_cfg.stepmotor_iholddelay)); @@ -120,6 +83,15 @@ int32_t StepMotorCtrlModule::pri_module_xxx_reg(int32_t param_id, bool read, int PROCESS_REG(kreg_step_motor_iglobalscaler, REG_GET(m_cfg.stepmotor_iglobalscaler), REG_SET(m_cfg.stepmotor_iglobalscaler)); PROCESS_REG(kreg_step_motor_in_debug_mode, REG_GET(m_state.debugmode), REG_SET(m_state.debugmode)); + PROCESS_REG(kreg_step_motor_vstart, REG_GET(m_cfg.motor_vstart), REG_SET(m_cfg.motor_vstart)); + PROCESS_REG(kreg_step_motor_a1, REG_GET(m_cfg.motor_a1), REG_SET(m_cfg.motor_a1)); + PROCESS_REG(kreg_step_motor_amax, REG_GET(m_cfg.motor_amax), REG_SET(m_cfg.motor_amax)); + PROCESS_REG(kreg_step_motor_v1, REG_GET(m_cfg.motor_v1), REG_SET(m_cfg.motor_v1)); + PROCESS_REG(kreg_step_motor_dmax, REG_GET(m_cfg.motor_dmax), REG_SET(m_cfg.motor_dmax)); + PROCESS_REG(kreg_step_motor_d1, REG_GET(m_cfg.motor_d1), REG_SET(m_cfg.motor_d1)); + PROCESS_REG(kreg_step_motor_vstop, REG_GET(m_cfg.motor_vstop), REG_SET(m_cfg.motor_vstop)); + PROCESS_REG(kreg_step_motor_tzerowait, REG_GET(m_cfg.motor_tzerowait), REG_SET(m_cfg.motor_tzerowait)); + default: return err::kmodule_not_find_config_index; break; @@ -160,6 +132,16 @@ int32_t StepMotorCtrlModule::step_motor_active_cfg() { m_stepM1->setScaleDenominator(m_cfg.motor_one_circle_pulse_denominator); m_stepM1->setMotorShaft(m_cfg.motor_shaft); m_stepM1->setGlobalScale(m_cfg.stepmotor_iglobalscaler); + + m_stepM1->set_vstart(m_cfg.motor_vstart); + m_stepM1->set_a1(m_cfg.motor_a1); + m_stepM1->set_amax(m_cfg.motor_amax); + m_stepM1->set_v1(m_cfg.motor_v1); + m_stepM1->set_dmax(m_cfg.motor_dmax); + m_stepM1->set_d1(m_cfg.motor_d1); + m_stepM1->set_vstop(m_cfg.motor_vstop); + m_stepM1->set_tzerowait(m_cfg.motor_tzerowait); + // stepmotor_iglobalscaler if (m_state.enable) { m_stepM1->enable(true); @@ -405,10 +387,10 @@ bool StepMotorCtrlModule::check_when_run() { bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t direction) { int32_t runToPointSpeed = m_cfg.motor_run_to_zero_speed; - int32_t runToPointDec = m_cfg.motor_run_to_zero_dec; + int32_t runToPointDec = m_cfg.motor_amax; int32_t lookPointVelocity = m_cfg.motor_look_zero_edge_speed; - int32_t lookPointDec = m_cfg.motor_look_zero_edge_dec; + int32_t lookPointDec = m_cfg.motor_amax; direction = direction >= 0 ? 1 : -1; @@ -417,7 +399,7 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio if (!gpio->getState()) { ZLOGI(TAG, "---------STEP1-------- move to point"); - _rotate(runToPointSpeed * direction, runToPointDec, runToPointDec); + _rotate(runToPointSpeed * direction, runToPointDec); bool xreach_io = false; while (!m_thread.getExitFlag()) { @@ -448,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, lookPointDec); + _rotate(-direction * lookPointVelocity, lookPointDec); bool reach_edge = false; while (!m_thread.getExitFlag()) { @@ -477,7 +459,7 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio // 如果设备已经在零点，则反向移动一定距离远离零点 if (!gpio->getState()) { ZLOGI(TAG, "---------STEP3-------- bak to gpio trigger edge"); - _rotate(direction * lookPointVelocity, lookPointDec, lookPointDec); + _rotate(direction * lookPointVelocity, lookPointDec); bool reach_edge = false; while (!m_thread.getExitFlag()) { @@ -512,11 +494,9 @@ bool StepMotorCtrlModule::exec_move_to_io_task(int32_t ioindex, int32_t directio return true; } -void StepMotorCtrlModule::_rotate(int32_t vel, int32_t acc, int32_t dec) { - ZLOGI(TAG, "m%d _rotate %d %d %d", m_id, vel, acc, dec); - m_stepM1->setAcceleration(acc); - m_stepM1->setDeceleration(dec); - m_stepM1->rotate(vel); +void StepMotorCtrlModule::_rotate(int32_t velocity, int32_t acc) { + m_stepM1->set_amax(acc); + m_stepM1->rotate(velocity); } /*********************************************************************************************************************** @@ -619,9 +599,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_rotate(int32_t direction) { [this, direction]() { befor_motor_move(); { - m_stepM1->setAcceleration(m_cfg.motor_default_acc); - m_stepM1->setDeceleration(m_cfg.motor_default_dec); - + m_stepM1->set_amax(m_cfg.motor_amax); int32_t vel = m_cfg.motor_default_velocity; if (direction <= 0) vel = -vel; m_stepM1->rotate(vel); @@ -647,8 +625,7 @@ int32_t StepMotorCtrlModule::do_step_motor_easy_move_to(int32_t tox) { befor_motor_move(); { - m_stepM1->setAcceleration(m_cfg.motor_default_acc); - m_stepM1->setDeceleration(m_cfg.motor_default_dec); + 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/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index 6a1c203..01d059f 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -17,18 +17,11 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { int32_t stepmotor_irun; int32_t stepmotor_iholddelay; int32_t stepmotor_iglobalscaler; - int32_t motor_default_acc; - int32_t motor_default_dec; int32_t motor_default_velocity; int32_t min_d; int32_t max_d; - // int32_t motor_run_to_zero_speed; - int32_t motor_run_to_zero_dec; - // int32_t motor_look_zero_edge_speed; - int32_t motor_look_zero_edge_dec; - // int32_t motor_vstart; int32_t motor_a1; int32_t motor_amax; @@ -37,7 +30,6 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { int32_t motor_d1; int32_t motor_vstop; int32_t motor_tzerowait; - } config_t; typedef struct { @@ -49,10 +41,10 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { } state_t; private: - IStepperMotor* m_stepM1 = nullptr; - int m_id = 0; - config_t m_cfg; - state_t m_state = {0}; + TMC51X0* m_stepM1 = nullptr; + int m_id = 0; + config_t m_cfg; + state_t m_state = {0}; ZGPIO* m_iotable = nullptr; int m_nio = 0; @@ -60,7 +52,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { ZThread m_thread; public: - void initialize(int moduleid, IStepperMotor* stepM, ZGPIO iotable[], int nio, config_t* cfg); + void initialize(int moduleid, TMC51X0* stepM, ZGPIO iotable[], int nio, config_t* cfg); static void create_default_cfg(config_t& cfg); @@ -127,7 +119,9 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { bool check_when_run(); bool exec_move_to_io_task(int32_t ioindex, int32_t direction); - void _rotate(int32_t vel, int32_t acc, int32_t dec); + + // void _rotate(int32_t vel, int32_t acc, int32_t dec); + void _rotate(int32_t velocity, int32_t acc); int32_t pri_module_xxx_reg(int32_t param_id, bool read, int32_t& val); diff --git a/components/tmc/basic/tmc_ic_interface.hpp b/components/tmc/basic/tmc_ic_interface.hpp index 0a01f52..2fb0dea 100644 --- a/components/tmc/basic/tmc_ic_interface.hpp +++ b/components/tmc/basic/tmc_ic_interface.hpp @@ -50,6 +50,10 @@ class IStepperMotor { virtual bool isStoped() = 0; // 是否停止运动 virtual void setNoAccLimit(bool enable) = 0; + + + + }; } // namespace iflytop diff --git a/components/tmc/ic/ztmc5130.cpp b/components/tmc/ic/ztmc5130.cpp index d6247e6..7b22311 100644 --- a/components/tmc/ic/ztmc5130.cpp +++ b/components/tmc/ic/ztmc5130.cpp @@ -87,6 +87,15 @@ void TMC51X0::setNoAccLimit(bool enable) { } } +void TMC51X0::set_vstart(int32_t vel) { writeInt(TMC5130_VSTART, to_motor_vel(vel)); } +void TMC51X0::set_a1(int32_t acc) { writeInt(TMC5130_A1, to_motor_acc(acc)); } +void TMC51X0::set_amax(int32_t acc) { writeInt(TMC5130_AMAX, to_motor_acc(acc)); } +void TMC51X0::set_v1(int32_t vel) { writeInt(TMC5130_V1, to_motor_vel(vel)); } +void TMC51X0::set_dmax(int32_t acc) { writeInt(TMC5130_DMAX, to_motor_acc(acc)); } +void TMC51X0::set_d1(int32_t acc) { writeInt(TMC5130_D1, to_motor_acc(acc)); } +void TMC51X0::set_vstop(int32_t vel) { writeInt(TMC5130_VSTOP, to_motor_vel(vel)); } +void TMC51X0::set_tzerowait(int32_t val) { writeInt(TMC5130_TZEROWAIT, val); } + void TMC51X0::enable(bool enable) { // m_port->TMC5130Port_setENNPinState(m_channel, !enable); SET_PIN(m_ennpin, !enable); diff --git a/components/tmc/ic/ztmc5130.hpp b/components/tmc/ic/ztmc5130.hpp index 43b7495..f6eaa40 100644 --- a/components/tmc/ic/ztmc5130.hpp +++ b/components/tmc/ic/ztmc5130.hpp @@ -14,6 +14,16 @@ extern "C" { namespace iflytop { #define TMC5130_LISTENER_MAX 5 + +/** + * @brief + * 注意事项 + * + * 1. 6点加速控制只在位置模式下有效，在速度模式下无效 + * 2. 速度模式下只有AMAX有效 + * + */ + class Tmc5130RampStat { public: uint32_t m_state; @@ -125,6 +135,15 @@ class TMC51X0 : public IStepperMotor { virtual bool isStoped() { return isReachTarget(); } virtual void setNoAccLimit(bool enable) override; + virtual void set_vstart(int32_t val); + virtual void set_a1(int32_t val); + virtual void set_amax(int32_t val); + virtual void set_v1(int32_t val); + virtual void set_dmax(int32_t val); + virtual void set_d1(int32_t val); + virtual void set_vstop(int32_t val); + virtual void set_tzerowait(int32_t val); + public: DevStatusReg_t getDevStatus() { // R 读后不清 static_assert(sizeof(DevStatusReg_t) == 4);