From 99f6aa959a04ededcdf668aa33d6dcaf0c434bd9 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 6 Jun 2024 11:37:51 +0800 Subject: [PATCH] update --- .../step_motor_ctrl_module.cpp | 99 ++++++++++++++++++++++ .../step_motor_ctrl_module.hpp | 10 +++ components/tmc/ic/ztmc4361A.cpp | 41 +++++++-- components/tmc/ic/ztmc4361A.hpp | 9 ++ 4 files changed, 154 insertions(+), 5 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 70e55b1..d76b413 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -4,6 +4,7 @@ #include #include "a8000_protocol\protocol.hpp" +#include "sdk/components/tmc/ic/ztmc4361A.hpp" #include "sdk\components\flash\znvs.hpp" #include "sdk\components\tmc\ic\ztmc5130.hpp" using namespace iflytop; @@ -29,6 +30,14 @@ void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO i if (tmcmotor) { tmcmotor->getGState(); // 读取状态，清空下复位标识 } + + TMC4361A* tmc4361motor = dynamic_cast(m_stepM1); + if (tmc4361motor) { + auto tmc4361state = tmc4361motor->getTMC4361AMotorEventState(); + auto tmc4361status = tmc4361motor->getTMC4361AMotorStatus(); + auto tmc2160Status = tmc4361motor->getTMC2160MotorDriverStatus(); + auto tmc2160State = tmc4361motor->getTMC2160MotorGstate(); + } } void StepMotorCtrlModule::create_default_cfg(config_t& cfg) { memset(&cfg, 0, sizeof(cfg)); @@ -163,6 +172,47 @@ int32_t StepMotorCtrlModule::step_motor_read_tmc5130_status(int32_t* status) { return 0; } +int32_t StepMotorCtrlModule::step_motor_read_tmc5130_state(int32_t* status) { + TMC51X0* tmc5130 = dynamic_cast(m_stepM1); + if (tmc5130) { + auto state = tmc5130->getGState(); + memcpy(status, &state, sizeof(int32_t)); + } + return 0; +}; +int32_t StepMotorCtrlModule::step_motor_read_tmc4361a_status(int32_t* status) { + TMC4361A* tmc4361 = dynamic_cast(m_stepM1); + if (tmc4361) { + auto devStatus = tmc4361->getTMC4361AMotorStatus(); + memcpy(status, &devStatus, sizeof(int32_t)); + } + return 0; +}; +int32_t StepMotorCtrlModule::step_motor_read_tmc4361a_state(int32_t* gstate) { + TMC4361A* tmc4361 = dynamic_cast(m_stepM1); + if (tmc4361) { + auto state = tmc4361->getTMC4361AMotorEventState(); + memcpy(gstate, &state, sizeof(int32_t)); + } + return 0; +}; +int32_t StepMotorCtrlModule::step_motor_read_tmc2160_status(int32_t* status) { + TMC4361A* tmc4361 = dynamic_cast(m_stepM1); + if (tmc4361) { + auto devStatus = tmc4361->getTMC2160MotorDriverStatus(); + memcpy(status, &devStatus, sizeof(int32_t)); + } + return 0; +}; +int32_t StepMotorCtrlModule::step_motor_read_tmc2160_state(int32_t* status) { + TMC4361A* tmc4361 = dynamic_cast(m_stepM1); + if (tmc4361) { + auto state = tmc4361->getTMC2160MotorGstate(); + memcpy(status, &state, sizeof(int32_t)); + } + return 0; +}; + /*********************************************************************************************************************** * INTER * ***********************************************************************************************************************/ @@ -208,6 +258,55 @@ void StepMotorCtrlModule::dumpTMC5130Status(TMC51X0::DevStatusReg_t* status) { ZLOGE(TAG, "olb: %d", status->olb); ZLOGE(TAG, "stst: %d", status->stst); } +void StepMotorCtrlModule::dump(TMC2160MotorDriverStatus_t val) { + ZLOGE(TAG, "sg_result: %d", val.sg_result); + ZLOGE(TAG, "s2vsa: %d", val.s2vsa); + ZLOGE(TAG, "s2vsb: %d", val.s2vsb); + ZLOGE(TAG, "stealth: %d", val.stealth); + ZLOGE(TAG, "fsactive: %d", val.fsactive); + ZLOGE(TAG, "cs_actual: %d", val.cs_actual); + ZLOGE(TAG, "reserved2: %d", val.reserved2); + ZLOGE(TAG, "stallguard: %d", val.stallguard); + ZLOGE(TAG, "ot: %d", val.ot); + ZLOGE(TAG, "otpw: %d", val.otpw); + ZLOGE(TAG, "s2ga: %d", val.s2ga); + ZLOGE(TAG, "s2gb: %d", val.s2gb); + ZLOGE(TAG, "ola: %d", val.ola); + ZLOGE(TAG, "olb: %d", val.olb); + ZLOGE(TAG, "stst: %d", val.stst); +} +void StepMotorCtrlModule::dump(TMC4361AMotorStatus_t val) { + + ZLOGE(TAG, "TARGET_REACHED_F: %d", val.TARGET_REACHED_F); + ZLOGE(TAG, "POS_COMP_REACHED_F: %d", val.POS_COMP_REACHED_F); + ZLOGE(TAG, "VEL_REACHED_F: %d", val.VEL_REACHED_F); + ZLOGE(TAG, "VEL_STATE_F: %d", val.VEL_STATE_F); + ZLOGE(TAG, "RAMP_STATE_F: %d", val.RAMP_STATE_F); + ZLOGE(TAG, "STOPL_ACTIVE_F: %d", val.STOPL_ACTIVE_F); + ZLOGE(TAG, "STOPR_ACTIVE_F: %d", val.STOPR_ACTIVE_F); + ZLOGE(TAG, "VSTOPL_ACTIVE_F: %d", val.VSTOPL_ACTIVE_F); + ZLOGE(TAG, "VSTOPR_ACTIVE_F: %d", val.VSTOPR_ACTIVE_F); + ZLOGE(TAG, "ACTIVE_STALL_F: %d", val.ACTIVE_STALL_F); + ZLOGE(TAG, "HOME_ERROR_F: %d", val.HOME_ERROR_F); + ZLOGE(TAG, "FS_ACTIVE_F: %d", val.FS_ACTIVE_F); + ZLOGE(TAG, "ENC_FAIL_F: %d", val.ENC_FAIL_F); + ZLOGE(TAG, "N_ACTIVE_F: %d", val.N_ACTIVE_F); + ZLOGE(TAG, "ENC_LATCH_F: %d", val.ENC_LATCH_F); + ZLOGE(TAG, "MULTI_CYCLE_FAIL_F: %d", val.MULTI_CYCLE_FAIL_F); + ZLOGE(TAG, "___: %d", val.___); + ZLOGE(TAG, "CL_FIT_F: %d", val.CL_FIT_F); + ZLOGE(TAG, "SERIAL_ENC_FLAGS: %d", val.SERIAL_ENC_FLAGS); + ZLOGE(TAG, "SG: %d", val.SG); + ZLOGE(TAG, "OT: %d", val.OT); + ZLOGE(TAG, "OTPW: %d", val.OTPW); + ZLOGE(TAG, "S2GA: %d", val.S2GA); + ZLOGE(TAG, "S2GB: %d", val.S2GB); + ZLOGE(TAG, "OLA: %d", val.OLA); + ZLOGE(TAG, "OLB: %d", val.OLB); + ZLOGE(TAG, "STST: %d", val.STST); +} +void StepMotorCtrlModule::dump(TMC2160MotorGstate_t val) {} +void StepMotorCtrlModule::dump(TMC4361AMotorEventState_t val) {} void StepMotorCtrlModule::setErrorFlag(int32_t ecode, uint32_t motorStatue) { creg.module_errorcode = ecode; 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 34e52fe..fb346bb 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -84,6 +84,11 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { virtual int32_t step_motor_read_io_state(int32_t ioindex, int32_t* state) override; virtual int32_t step_motor_read_tmc5130_status(int32_t* errorflag) override; + virtual int32_t step_motor_read_tmc5130_state(int32_t* status) override; + virtual int32_t step_motor_read_tmc4361a_status(int32_t* status) override; + virtual int32_t step_motor_read_tmc4361a_state(int32_t* gstate) override; + virtual int32_t step_motor_read_tmc2160_status(int32_t* status) override; + virtual int32_t step_motor_read_tmc2160_state(int32_t* status) override; public: IStepperMotor* getMotor() { return m_stepM1; } @@ -118,5 +123,10 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { private: void dumpTMC5130Status(TMC51X0::DevStatusReg_t* status); void setErrorFlag(int32_t ecode, uint32_t motorStatue); + + void dump(TMC2160MotorDriverStatus_t val); + void dump(TMC4361AMotorStatus_t val); + void dump(TMC2160MotorGstate_t val); + void dump(TMC4361AMotorEventState_t val); }; } // namespace iflytop \ No newline at end of file diff --git a/components/tmc/ic/ztmc4361A.cpp b/components/tmc/ic/ztmc4361A.cpp index 6e82d98..e517f4d 100644 --- a/components/tmc/ic/ztmc4361A.cpp +++ b/components/tmc/ic/ztmc4361A.cpp @@ -1,4 +1,4 @@ -#if 1 + #include "ztmc4361A.hpp" #ifdef HAL_SPI_MODULE_ENABLED @@ -18,7 +18,6 @@ using namespace iflytop; pin->setState(val); \ } -#if 1 void TMC4361A::tmc4361_readWriteArray(uint8_t *data, size_t length) { m_csgpio->setState(false); // zchip_clock_early_delayus(10); @@ -203,6 +202,11 @@ uint8_t TMC4361A::reset() { zchip_clock_early_delayus(1000); SET_PIN(m_resetPin, true); + // 软件复位 + tmc4361_writeInt(TMC4361A_RESET_REG, 0x525354); + zchip_clock_early_delayus(1000); + tmc4361_writeInt(TMC4361A_RESET_REG, 0); + /** * @brief 重置芯片镜像寄存?? * @@ -299,7 +303,6 @@ bool TMC4361A::isReachTarget() { } } - /*********************************************************************************************************************** * DRIVERIC_OPERATION * ***********************************************************************************************************************/ @@ -391,7 +394,35 @@ uint32_t TMC4361A::haspassedms(uint32_t now, uint32_t last) { return 0xFFFFFFFF - last + now; } } -#endif -#endif + +TMC2160MotorDriverStatus_t TMC4361A::getTMC2160MotorDriverStatus() { + static_assert(sizeof(TMC2160MotorDriverStatus_t) == 4, "TMC2160MotorDriverStatus_t size not match"); + + TMC2160MotorDriverStatus_t status; + uint32_t ret = driverIC_readInt(TMC2160_DRV_STATUS); + memcpy(&status, &ret, sizeof(status)); + return status; +} +TMC4361AMotorStatus_t TMC4361A::getTMC4361AMotorStatus() { + static_assert(sizeof(TMC4361AMotorStatus_t) == 4, "TMC4361AMotorStatus_t size not match"); + TMC4361AMotorStatus_t status; + uint32_t ret = tmc4361_readInt(TMC4361A_STATUS); + memcpy(&status, &ret, sizeof(status)); + return status; +} +TMC2160MotorGstate_t TMC4361A::getTMC2160MotorGstate() { + static_assert(sizeof(TMC2160MotorGstate_t) == 4, "TMC2160MotorGstate_t size not match"); + TMC2160MotorGstate_t status; + uint32_t ret = driverIC_readInt(TMC2160_GSTAT); + memcpy(&status, &ret, sizeof(status)); + return status; +} +TMC4361AMotorEventState_t TMC4361A::getTMC4361AMotorEventState() { + static_assert(sizeof(TMC4361AMotorEventState_t) == 4, "TMC4361AMotorEventState_t size not match"); + TMC4361AMotorEventState_t status; + uint32_t ret = tmc4361_readInt(TMC4361A_EVENTS); + memcpy(&status, &ret, sizeof(status)); + return status; +} #endif diff --git a/components/tmc/ic/ztmc4361A.hpp b/components/tmc/ic/ztmc4361A.hpp index 3bf1522..52b114c 100644 --- a/components/tmc/ic/ztmc4361A.hpp +++ b/components/tmc/ic/ztmc4361A.hpp @@ -7,6 +7,7 @@ #include #include "../basic/tmc_ic_interface.hpp" +#include "a8000_protocol\api\zi_motor.hpp" #include "sdk/os/zos.hpp" extern "C" { #include "TMC2160\TMC2160.h" @@ -15,6 +16,7 @@ extern "C" { #ifdef HAL_SPI_MODULE_ENABLED namespace iflytop { + #define TMC4361A_LISTENER_MAX 5 class TMC4361A : public IStepperMotor { public: @@ -124,6 +126,13 @@ class TMC4361A : public IStepperMotor { void driverIC_setMotorShaft(bool reverse); uint32_t driverIC_readICVersion(); + public: + + TMC2160MotorDriverStatus_t getTMC2160MotorDriverStatus(); + TMC4361AMotorStatus_t getTMC4361AMotorStatus(); + TMC2160MotorGstate_t getTMC2160MotorGstate(); + TMC4361AMotorEventState_t getTMC4361AMotorEventState(); + private: int32_t to_motor_acc(int32_t acc /* rpm/s^2*/); int32_t to_motor_vel(int32_t vel /*rpm*/);