From 5aa192ca54d7e03866bfd9132f293d85f192e4a7 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Mon, 31 Jul 2023 15:20:51 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E5=BD=92=E9=9B=B6=E5=A4=B1?= =?UTF-8?q?=E8=B4=A5BUG?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../device_base_control_service.cpp | 5 +++-- .../iflytop_can_slave_v1/iflytop_can_slave.cpp | 3 ++- .../single_axis_motor_control.cpp | 25 ++++++++++++++++++++-- .../single_axis_motor_control.hpp | 10 +++++++-- components/tmc/basic/tmc_ic_interface.hpp | 1 + components/tmc/ic/ztmc4361A.cpp | 12 +++++------ components/tmc/ic/ztmc4361A.hpp | 8 ++++++- components/tmc/ic/ztmc5130.cpp | 17 ++++++++++----- components/tmc/ic/ztmc5130.hpp | 1 + 9 files changed, 63 insertions(+), 19 deletions(-) diff --git a/components/iflytop_can_slave_modules/device_base_control_service.cpp b/components/iflytop_can_slave_modules/device_base_control_service.cpp index 63c5ebe..ecbad4b 100644 --- a/components/iflytop_can_slave_modules/device_base_control_service.cpp +++ b/components/iflytop_can_slave_modules/device_base_control_service.cpp @@ -6,8 +6,9 @@ void DeviceBaseControlService::initialize(IflytopCanProtocolStackProcesser* prot m_protocol_processer = protocol_processer; m_deviceId = deviceId; ZASSERT(m_protocol_processer != NULL); - activeRegs(); + m_slave = m_protocol_processer->createICPSSlaveModule("DeviceBaseControlService", this, 0); + activeRegs(); } void DeviceBaseControlService::activeRegs() { m_protocol_processer->activeReg(m_slave, REG_DEVICE_ID, icps::kr, m_deviceId); @@ -35,4 +36,4 @@ icps::error_t DeviceBaseControlService::onHostRegisterWriteEvent(IflytopCanProto } bool DeviceBaseControlService::isInEngineerMode() { return m_protocol_processer->readRegValue(REG_DEVICE_ENGINEER_MODE_MODE) != 0; } -#endif \ No newline at end of file +#endif diff --git a/components/iflytop_can_slave_v1/iflytop_can_slave.cpp b/components/iflytop_can_slave_v1/iflytop_can_slave.cpp index dbdaaa9..4992de3 100644 --- a/components/iflytop_can_slave_v1/iflytop_can_slave.cpp +++ b/components/iflytop_can_slave_v1/iflytop_can_slave.cpp @@ -114,7 +114,8 @@ void IflytopCanProtocolStackProcesser::initialize(cfg_t *config) { } ICPSSlaveModule *IflytopCanProtocolStackProcesser::createICPSSlaveModule(const char *modulename, ICPSListener *listener, int regaddoff) { // ZASSERT(m_listenerSize < ZARRAY_SIZE(m_listeners)); - ICPSSlaveModule *container = new ICPSSlaveModule(); + ICPSSlaveModule *container = new ICPSSlaveModule(); + ZASSERT(container != NULL); container->modulename = modulename; container->listener = listener; container->moduleRegAddoff = regaddoff; diff --git a/components/single_axis_motor_control/single_axis_motor_control.cpp b/components/single_axis_motor_control/single_axis_motor_control.cpp index bee2c2e..3c61b87 100644 --- a/components/single_axis_motor_control/single_axis_motor_control.cpp +++ b/components/single_axis_motor_control/single_axis_motor_control.cpp @@ -19,6 +19,9 @@ void SingleAxisMotorControler::initialize(const char* name, IflytopCanProtocolSt ZASSERT(motor); if (m_homegpio) { ZASSERT(m_homegpio->isItRisingAndItFallingEXITGPIO()); + m_homegpio->regListener([this](ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) { + if (m_gpioirqprocesser) m_gpioirqprocesser(GPIO_Pin, irqevent); + }); } m_slave = m_processer->createICPSSlaveModule(name, this, m_regaddoff); @@ -47,6 +50,9 @@ void SingleAxisMotorControler::initialize(const char* name, IflytopCanProtocolSt cfg_min_pos = m_processer->activeReg(m_slave, SAMC_REG_CFG_MIN_POS, icps::kwr, 0); // 最小位置 cfg_max_pos = m_processer->activeReg(m_slave, SAMC_REG_CFG_MAX_POS, icps::kwr, 0); // 最大位置 cfg_runtohome_keep_move_distance = m_processer->activeReg(m_slave, SAMC_REG_CFG_RUNTOHOME_KEEP_MOVE_DISTANCE, icps::kwr, 0); // 归零阶段1移动距离 + + m_step_scheduler.initialize(10, 1); + ZHALCORE::getInstance()->regPeriodJob([this](ZHALCORE::Context& context) { this->periodJob(&context); }, 1); } icps::error_t SingleAxisMotorControler::onHostRegisterWriteEvent(icps::WriteEvent* event) { @@ -171,6 +177,7 @@ icps::error_t SingleAxisMotorControler::runToHome() { * */ m_step_scheduler.pushActionAction([this](StepActionContext* context) { // + ZLOGI(m_name, "runToHome step 2 move away from zero point"); regGpioIrqProcesser(nullptr); motor_moveBy(cfg_runtohome_leave_zero_point_distance->getValue(), // cfg_runhome_velocity->getValue(), // @@ -183,6 +190,7 @@ icps::error_t SingleAxisMotorControler::runToHome() { * @brief STEP 3 返回原点 */ m_step_scheduler.pushActionAction([this](StepActionContext* context) { // + ZLOGI(m_name, "runToHome step 3 move to zero point"); regGpioIrqProcesser([this](ZGPIO* GPIO_Pin, ZGPIO::IrqTypeEvent_t irqevent) { if (GPIO_Pin != m_homegpio) return; m_runToHomeContext.stopByIrq = true; @@ -217,6 +225,7 @@ icps::error_t SingleAxisMotorControler::runToHome() { * @brief STEP 4 移动到零点 */ m_step_scheduler.pushActionAction([this](StepActionContext* context) { // + ZLOGI(m_name, "runToHome step 4 move to zero point"); regGpioIrqProcesser(nullptr); motor_moveTo(0, // cfg_runhome_velocity->getValue(), // @@ -228,8 +237,12 @@ icps::error_t SingleAxisMotorControler::runToHome() { /** * @brief 异常处理 */ - m_step_scheduler.regDoneCallback([this]() { changeToState(kstate_idle); }); + m_step_scheduler.regDoneCallback([this]() { + regGpioIrqProcesser(nullptr); + changeToState(kstate_idle); + }); m_step_scheduler.regBreakCallback([this]() { + regGpioIrqProcesser(nullptr); m_exceptionReg->setValue(kexcep_loseZeroPoint); changeToState(kstate_exception); }); @@ -251,11 +264,19 @@ icps::error_t SingleAxisMotorControler::clearException() { } void SingleAxisMotorControler::changeToState(state_t state) { + ZLOGI(m_name, "changeState %d->%d", m_state, state); m_state = state; m_statusReg->setValue(m_state); } -void SingleAxisMotorControler::periodJob(ZHALCORE::Context* context) {} +void SingleAxisMotorControler::periodJob(ZHALCORE::Context* context) { + if (context->getScheduleTimes() % 33 == 0 /*33 ms*/) { + m_currentVelocityReg->setValue(m_motor->getVACTUAL()); + m_currentPosReg->setValue(m_motor->getXACTUAL()); + // ZLOGI(m_name, "%d", m_statusReg->getValue()); + } + // m_statusReg->setValue(context->getScheduleTimes() % 4); +} void SingleAxisMotorControler::motor_moveTo(int32_t pos, int32_t velocity, int32_t acc, int32_t dec) { ZLOGI(m_name, "motor_moveTo %d %d %d %d", pos, velocity, acc, dec); int32_t position = m_motor->getXACTUAL(); diff --git a/components/single_axis_motor_control/single_axis_motor_control.hpp b/components/single_axis_motor_control/single_axis_motor_control.hpp index 9e8eff2..52a7d1a 100644 --- a/components/single_axis_motor_control/single_axis_motor_control.hpp +++ b/components/single_axis_motor_control/single_axis_motor_control.hpp @@ -133,8 +133,14 @@ class SingleAxisMotorControler : public ICPSListener { private: bool motorIsStop(StepActionContext* context); - void regGpioIrqProcesser(ZGPIO::onirq_t irqprocesser) { m_gpioirqprocesser = irqprocesser; } - void clearGpioIrqProcesser() { m_gpioirqprocesser = NULL; } + void regGpioIrqProcesser(ZGPIO::onirq_t irqprocesser) { + CriticalContext cc; + m_gpioirqprocesser = irqprocesser; + } + void clearGpioIrqProcesser() { + CriticalContext cc; + m_gpioirqprocesser = NULL; + } }; } // namespace iflytop diff --git a/components/tmc/basic/tmc_ic_interface.hpp b/components/tmc/basic/tmc_ic_interface.hpp index 5fb1760..f3edc88 100644 --- a/components/tmc/basic/tmc_ic_interface.hpp +++ b/components/tmc/basic/tmc_ic_interface.hpp @@ -58,6 +58,7 @@ class IStepperMotor { virtual void setDeceleration(float accelerationpps2) = 0; // 设置最大减速度 virtual bool isReachTarget() = 0; + virtual bool isStoped() = 0; }; } // namespace iflytop \ No newline at end of file diff --git a/components/tmc/ic/ztmc4361A.cpp b/components/tmc/ic/ztmc4361A.cpp index 2f70301..b7d7d51 100644 --- a/components/tmc/ic/ztmc4361A.cpp +++ b/components/tmc/ic/ztmc4361A.cpp @@ -110,7 +110,7 @@ void TMC4361A::readWriteCover(uint8_t *data, size_t length) { TMC4361A::TMC4361A(/* args */) { m_driver_ic_type = IC_TMC2130; m_lastCallPeriodicJobTick = 0; - m_reachtarget = false; + // m_reachtarget = false; } /** * @brief @@ -251,18 +251,18 @@ int32_t tmc4361A_discardVelocityDecimals(int32_t value) { return value << 8; } void TMC4361A::rotate(int32_t velocity) { + m_motor_mode = kvelmode; PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 0); writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocity)); } void TMC4361A::stop() { rotate(0); } void TMC4361A::moveTo(int32_t position, uint32_t velocityMax) { - m_reachtarget = false; + m_motor_mode = kposmode; PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 1); writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocityMax)); writeInt(TMC4361A_X_TARGET, position); } void TMC4361A::moveBy(int32_t relativePosition, uint32_t velocityMax) { - m_reachtarget = false; relativePosition += readInt(TMC4361A_XACTUAL); moveTo(relativePosition, velocityMax); } @@ -289,10 +289,10 @@ uint32_t TMC4361A::haspassedms(uint32_t now, uint32_t last) { } bool TMC4361A::isReachTarget() { uint32_t value = readInt(TMC4361A_STATUS); - if ((value & TMC4361A_TARGET_REACHED_F_MASK) > 0) { - return true; + if (m_motor_mode == kposmode) { + return (value & TMC4361A_TARGET_REACHED_F_MASK) > 0; } else { - return false; + return (value & TMC4361A_VEL_REACHED_F_MASK) && (readInt(TMC4361A_VMAX) == 0); } } /******************************************************************************* diff --git a/components/tmc/ic/ztmc4361A.hpp b/components/tmc/ic/ztmc4361A.hpp index 4e309cf..4c07eca 100644 --- a/components/tmc/ic/ztmc4361A.hpp +++ b/components/tmc/ic/ztmc4361A.hpp @@ -35,6 +35,11 @@ class TMC4361A : public IStepperMotor { ZGPIO *driverIC_resetPin; } cfg_t; + typedef enum { + kvelmode, + kposmode, + } motor_mode_t; + private: int32_t shadowRegister[TMC_REGISTER_COUNT]; const uint8_t *m_registerAccessTable; @@ -54,7 +59,7 @@ class TMC4361A : public IStepperMotor { ZGPIO *m_driverIC_ennPin; ZGPIO *m_driverIC_resetPin; - bool m_reachtarget; + motor_mode_t m_motor_mode = kvelmode; public: TMC4361A(/* args */); @@ -122,6 +127,7 @@ class TMC4361A : public IStepperMotor { *******************************************************/ int32_t readSubICVersion(); virtual bool isReachTarget(); + virtual bool isStoped() { return isReachTarget(); } public: // only used in tmc4361A.cpp diff --git a/components/tmc/ic/ztmc5130.cpp b/components/tmc/ic/ztmc5130.cpp index 1f80f5d..411247c 100644 --- a/components/tmc/ic/ztmc5130.cpp +++ b/components/tmc/ic/ztmc5130.cpp @@ -131,12 +131,19 @@ uint32_t TMC5130::haspassedms(uint32_t now, uint32_t last) { } } bool TMC5130::isReachTarget() { - uint32_t state = getTMC5130_RAMPSTAT(); - Tmc5130RampStat event = Tmc5130RampStat(state); - if (event.isSetted(Tmc5130RampStat::ktmc5130_rs_posreached)) { - return true; + /** + * @brief + */ + int mode = readInt(TMC5130_RAMPMODE); + if (mode == TMC5130_MODE_POSITION) { + uint32_t state = getTMC5130_RAMPSTAT(); + Tmc5130RampStat event = Tmc5130RampStat(state); + return event.isSetted(Tmc5130RampStat::ktmc5130_rs_posreached); + } else { + uint32_t state = getTMC5130_RAMPSTAT(); + Tmc5130RampStat event = Tmc5130RampStat(state); + return event.isSetted(Tmc5130RampStat::ktmc5130_rs_vzero) && event.isSetted(Tmc5130RampStat::ktmc5130_rs_velreached); } - return false; } /******************************************************************************* * basic * diff --git a/components/tmc/ic/ztmc5130.hpp b/components/tmc/ic/ztmc5130.hpp index f02790c..39fb5d0 100644 --- a/components/tmc/ic/ztmc5130.hpp +++ b/components/tmc/ic/ztmc5130.hpp @@ -124,6 +124,7 @@ class TMC5130 : public IStepperMotor { virtual void stop(); virtual bool isReachTarget(); // 是否到达目标位置 + virtual bool isStoped() { return isReachTarget(); } /******************************************************************************* * 驱动器寄存器读写方法 *