From 07068dc9a888386bb3b325727b1a056d120b4ee1 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 5 Jun 2024 21:06:33 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B95130=20=E7=B1=BB=E5=90=8D?= =?UTF-8?q?=E4=B8=BA51x0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../step_motor_ctrl_module.cpp | 10 +-- .../step_motor_ctrl_module.hpp | 2 +- components/tmc/basic/tmc_ic_interface.hpp | 1 + components/tmc/ic/ztmc4361A.cpp | 4 ++ components/tmc/ic/ztmc4361A.hpp | 2 +- components/tmc/ic/ztmc5130.cpp | 79 ++++++++++++---------- components/tmc/ic/ztmc5130.hpp | 5 +- 7 files changed, 58 insertions(+), 45 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 12cf149..ba806d3 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -25,7 +25,7 @@ void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO i step_motor_active_cfg(); step_motor_enable(true); - TMC5130* tmcmotor = dynamic_cast(m_stepM1); + TMC51X0* tmcmotor = dynamic_cast(m_stepM1); if (tmcmotor) { tmcmotor->getGState(); // 读取状态，清空下复位标识 } @@ -152,9 +152,9 @@ int32_t StepMotorCtrlModule::step_motor_read_io_state(int32_t ioindex, int32_t* int32_t StepMotorCtrlModule::step_motor_read_tmc5130_status(int32_t* status) { // *errorflag = m_state.motorStatus; - TMC5130* tmc5130 = dynamic_cast(m_stepM1); + TMC51X0* tmc5130 = dynamic_cast(m_stepM1); if (tmc5130) { - static_assert(sizeof(TMC5130::DevStatusReg_t) == 4); + static_assert(sizeof(TMC51X0::DevStatusReg_t) == 4); auto devStatus = tmc5130->getDevStatus(); memcpy(status, &devStatus, sizeof(int32_t)); } @@ -193,7 +193,7 @@ void StepMotorCtrlModule::after_motor_move() { creg.m_module_status = 0; } -void StepMotorCtrlModule::dumpTMC5130Status(TMC5130::DevStatusReg_t* status) { +void StepMotorCtrlModule::dumpTMC5130Status(TMC51X0::DevStatusReg_t* status) { ZLOGE(TAG, "sg_result: %d", status->sg_result); ZLOGE(TAG, "fsactive: %d", status->fsactive); ZLOGE(TAG, "cs_actual: %d", status->cs_actual); @@ -214,7 +214,7 @@ void StepMotorCtrlModule::setErrorFlag(int32_t ecode, uint32_t motorStatue) { bool StepMotorCtrlModule::check_when_run() { // - TMC5130* tmc5130 = dynamic_cast(m_stepM1); + TMC51X0* tmc5130 = dynamic_cast(m_stepM1); if (tmc5130) { auto state = tmc5130->getGState(); auto devStatus = tmc5130->getDevStatus(); 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 5462660..34e52fe 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -116,7 +116,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor { int32_t pri_module_xxx_reg(int32_t param_id, bool read, int32_t& val); private: - void dumpTMC5130Status(TMC5130::DevStatusReg_t* status); + void dumpTMC5130Status(TMC51X0::DevStatusReg_t* status); void setErrorFlag(int32_t ecode, uint32_t motorStatue); }; } // namespace iflytop \ No newline at end of file diff --git a/components/tmc/basic/tmc_ic_interface.hpp b/components/tmc/basic/tmc_ic_interface.hpp index a7ffa7a..b019714 100644 --- a/components/tmc/basic/tmc_ic_interface.hpp +++ b/components/tmc/basic/tmc_ic_interface.hpp @@ -19,6 +19,7 @@ typedef enum { typedef enum { kTMC4361A = 0x2, kTMC2160 = 0x30, + kTMC5130 = 0x11, } tmcic_id_t; class IStepperMotor { diff --git a/components/tmc/ic/ztmc4361A.cpp b/components/tmc/ic/ztmc4361A.cpp index 924957f..f94ee07 100644 --- a/components/tmc/ic/ztmc4361A.cpp +++ b/components/tmc/ic/ztmc4361A.cpp @@ -248,6 +248,10 @@ void TMC4361A::enable(bool enable) { SET_PIN(m_driverIC_ennPin, !enable); } +void TMC4361A::setGlobalScale(uint8_t globalscale) { + +}; + int32_t tmc4361A_discardVelocityDecimals(int32_t value) { if (abs(value) > 8000000) { value = (value < 0) ? -8000000 : 8000000; diff --git a/components/tmc/ic/ztmc4361A.hpp b/components/tmc/ic/ztmc4361A.hpp index f22b92a..7234616 100644 --- a/components/tmc/ic/ztmc4361A.hpp +++ b/components/tmc/ic/ztmc4361A.hpp @@ -88,7 +88,7 @@ class TMC4361A : public IStepperMotor { virtual void setDeceleration(float accelerationpps2); // 设置最大减速度 virtual void setMotorShaft(bool reverse) { driverIC_setMotorShaft(reverse); }; virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_setIHOLD_IRUN(ihold, irun, iholddelay); } - virtual void setGlobalScale(uint8_t globalscale) { ZLOGW("TMC4361", "setGlobalScale fail, not support"); }; // TODO:impl it + virtual void setGlobalScale(uint8_t globalscale); virtual void setScale(int32_t scale); virtual void setScaleDenominator(int32_t scale); diff --git a/components/tmc/ic/ztmc5130.cpp b/components/tmc/ic/ztmc5130.cpp index 0867e3a..cd2eea7 100644 --- a/components/tmc/ic/ztmc5130.cpp +++ b/components/tmc/ic/ztmc5130.cpp @@ -21,8 +21,8 @@ using namespace iflytop; if (pin) { \ pin->setState(val); \ } -TMC5130::TMC5130(/* args */) {} -void TMC5130::initialize(cfg_t *cfg) { +TMC51X0::TMC51X0(/* args */) {} +void TMC51X0::initialize(cfg_t *cfg) { m_cfg = *cfg; // m_channel = channel; @@ -50,7 +50,7 @@ void TMC5130::initialize(cfg_t *cfg) { reset(); } -void TMC5130::setMRES(mres_type_t value) { +void TMC51X0::setMRES(mres_type_t value) { PRV_FIELD_WRITE(TMC5130_CHOPCONF, TMC5130_MRES_MASK, TMC5130_MRES_SHIFT, value); m_MRES = value; @@ -77,7 +77,7 @@ void TMC5130::setMRES(mres_type_t value) { } } -void TMC5130::setNoAccLimit(bool enable) { +void TMC51X0::setNoAccLimit(bool enable) { if (!enable) { writeInt(TMC5130_VSTART, 100); writeInt(TMC5130_VSTOP, 100); @@ -87,11 +87,11 @@ void TMC5130::setNoAccLimit(bool enable) { } } -void TMC5130::enable(bool enable) { +void TMC51X0::enable(bool enable) { // m_port->TMC5130Port_setENNPinState(m_channel, !enable); SET_PIN(m_ennpin, !enable); } -uint8_t TMC5130::reset() { +uint8_t TMC51X0::reset() { stop(); // m_port->TMC5130Port_setResetNPinState(m_channel, false); SET_PIN(m_csnpin, false); @@ -125,19 +125,26 @@ uint8_t TMC5130::reset() { enable(false); return 0; } -int32_t TMC5130::getXACTUAL() { return to_user_pos(readInt(TMC5130_XACTUAL)); } -void TMC5130::setXACTUAL(int32_t value) { writeInt(TMC5130_XACTUAL, to_motor_pos(value)); } -int32_t TMC5130::getVACTUAL() { return to_user_pos(readInt(TMC5130_VACTUAL)); } -void TMC5130::setAcceleration(float accelerationpps2) { writeInt(TMC5130_AMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // 设置最大加速度 -void TMC5130::setDeceleration(float accelerationpps2) { writeInt(TMC5130_DMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // 设置最大减速度 -void TMC5130::setMotorShaft(bool reverse) { PRV_FIELD_WRITE(TMC5130_GCONF, TMC5130_SHAFT_MASK, TMC5130_SHAFT_SHIFT, reverse); } -void TMC5130::setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { writeInt(TMC5130_IHOLD_IRUN, (iholddelay << TMC5130_IHOLDDELAY_SHIFT) | (irun << TMC5130_IRUN_SHIFT) | (ihold << TMC5130_IHOLD_SHIFT)); } +int32_t TMC51X0::getXACTUAL() { return to_user_pos(readInt(TMC5130_XACTUAL)); } +void TMC51X0::setXACTUAL(int32_t value) { writeInt(TMC5130_XACTUAL, to_motor_pos(value)); } +int32_t TMC51X0::getVACTUAL() { return to_user_pos(readInt(TMC5130_VACTUAL)); } +void TMC51X0::setAcceleration(float accelerationpps2) { writeInt(TMC5130_AMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // 设置最大加速度 +void TMC51X0::setDeceleration(float accelerationpps2) { writeInt(TMC5130_DMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // 设置最大减速度 +void TMC51X0::setMotorShaft(bool reverse) { PRV_FIELD_WRITE(TMC5130_GCONF, TMC5130_SHAFT_MASK, TMC5130_SHAFT_SHIFT, reverse); } +void TMC51X0::setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { writeInt(TMC5130_IHOLD_IRUN, (iholddelay << TMC5130_IHOLDDELAY_SHIFT) | (irun << TMC5130_IRUN_SHIFT) | (ihold << TMC5130_IHOLD_SHIFT)); } #define TMC5160_GLOBAL_SCALER 0x0B #define TMC5160_GLOBAL_SCALER_MASK 0xFF #define TMC5160_GLOBAL_SCALER_SHIFT 0 -void TMC5130::setGlobalScale(uint8_t globalscale) { // +void TMC51X0::setGlobalScale(uint8_t globalscale) { // + + uint32_t chipId = readICVersion(); + if (chipId == kTMC5130) { + // 5130不支持全局缩放 + return; + } + if (globalscale == 0) { globalscale = 0; } else if (globalscale <= 31 || globalscale >= 1) { @@ -147,27 +154,27 @@ void TMC5130::setGlobalScale(uint8_t globalscale) { // } writeInt(TMC5160_GLOBAL_SCALER, (readInt(TMC5160_GLOBAL_SCALER) & ~TMC5160_GLOBAL_SCALER_MASK) | (globalscale << TMC5160_GLOBAL_SCALER_SHIFT)); } -uint8_t TMC5130::getGlobalScale() { // +uint8_t TMC51X0::getGlobalScale() { // return PRV_FIELD_READ(TMC5160_GLOBAL_SCALER, TMC5160_GLOBAL_SCALER_MASK, TMC5160_GLOBAL_SCALER_SHIFT); } -uint32_t TMC5130::readICVersion() { +uint32_t TMC51X0::readICVersion() { uint32_t chipID = PRV_FIELD_READ(TMC5130_IOIN, TMC5130_VERSION_MASK, TMC5130_VERSION_SHIFT); return chipID; } -uint32_t TMC5130::getTMC5130_RAMPSTAT() { return readInt(TMC5130_RAMPSTAT); } -Tmc5130RampStat TMC5130::getTMC5130_RAMPSTAT2() { +uint32_t TMC51X0::getTMC5130_RAMPSTAT() { return readInt(TMC5130_RAMPSTAT); } +Tmc5130RampStat TMC51X0::getTMC5130_RAMPSTAT2() { uint32_t value = getTMC5130_RAMPSTAT(); return Tmc5130RampStat(value); } -void TMC5130::stop() { rotate(0); } -void TMC5130::rotate(int32_t velocity) { +void TMC51X0::stop() { rotate(0); } +void TMC51X0::rotate(int32_t velocity) { // velocity *= m_scale; velocity = to_motor_vel(velocity); writeInt(TMC5130_VMAX, abs(velocity)); writeInt(TMC5130_RAMPMODE, (velocity >= 0) ? TMC5130_MODE_VELPOS : TMC5130_MODE_VELNEG); } -void TMC5130::moveTo(int32_t position, uint32_t velocityMax) { +void TMC51X0::moveTo(int32_t position, uint32_t velocityMax) { // position *= m_scale; // velocityMax *= m_scale; position = to_motor_pos(position); @@ -177,19 +184,19 @@ void TMC5130::moveTo(int32_t position, uint32_t velocityMax) { writeInt(TMC5130_VMAX, velocityMax); writeInt(TMC5130_XTARGET, position); } -void TMC5130::moveBy(int32_t relativePosition, uint32_t velocityMax) { // determine actual position and add numbers of ticks to move +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); } -uint32_t TMC5130::readXTARGET() { return readInt(TMC5130_XTARGET); } -uint32_t TMC5130::haspassedms(uint32_t now, uint32_t last) { +uint32_t TMC51X0::readXTARGET() { return readInt(TMC5130_XTARGET); } +uint32_t TMC51X0::haspassedms(uint32_t now, uint32_t last) { if (now >= last) { return now - last; } else { return 0xFFFFFFFF - last + now; } } -bool TMC5130::isReachTarget() { +bool TMC51X0::isReachTarget() { /** * @brief */ @@ -205,8 +212,8 @@ bool TMC5130::isReachTarget() { } } -void TMC5130::setScale(int32_t scale) { m_scale = scale; } -void TMC5130::setScaleDenominator(int32_t scale) { m_scale_deceleration = scale; } +void TMC51X0::setScale(int32_t scale) { m_scale = scale; } +void TMC51X0::setScaleDenominator(int32_t scale) { m_scale_deceleration = scale; } /******************************************************************************* * basic * @@ -215,7 +222,7 @@ void TMC5130::setScaleDenominator(int32_t scale) { m_scale_deceleration = scale; // CriticalContext cc; // writeInt(address, readInt(address) & ~mask | value << shift); // } -void TMC5130::readWriteArray(uint8_t *data, size_t length) { +void TMC51X0::readWriteArray(uint8_t *data, size_t length) { CriticalContext cc; // m_port->TMC5130Port_readWriteArray(m_channel, data, length); // m_csnpin @@ -223,7 +230,7 @@ void TMC5130::readWriteArray(uint8_t *data, size_t length) { HAL_SPI_TransmitReceive(m_hspi, data, data, length, 100); SET_PIN(m_csnpin, true); } -void TMC5130::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) { +void TMC51X0::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) { CriticalContext cc; uint8_t data[5] = {static_cast(address | static_cast(TMC5130_WRITE_BIT)), x1, x2, x3, x4}; readWriteArray(&data[0], 5); @@ -234,11 +241,11 @@ void TMC5130::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, address = TMC_ADDRESS(address); m_shadowRegister[address] = value; } -void TMC5130::writeInt(uint8_t address, int32_t value) { +void TMC51X0::writeInt(uint8_t address, int32_t value) { CriticalContext cc; writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0)); } -int32_t TMC5130::readInt(uint8_t address) { +int32_t TMC51X0::readInt(uint8_t address) { CriticalContext cc; address = TMC_ADDRESS(address); @@ -255,28 +262,28 @@ int32_t TMC5130::readInt(uint8_t address) { return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4]; } -int32_t TMC5130::to_motor_acc(int32_t acc) { // +int32_t TMC51X0::to_motor_acc(int32_t acc) { // int32_t val = acc / 60.0 * m_onecirclepulse; // 65535 if (val > 65535) val = 65535; return val; } -int32_t TMC5130::to_motor_vel(int32_t vel) { // +int32_t TMC51X0::to_motor_vel(int32_t vel) { // int32_t val = vel / 60.0 * m_onecirclepulse; if (val > 8388095 /*2^23-512*/) { val = 8388095; } return val; } // rpm -int32_t TMC5130::to_motor_pos(int32_t pos) { // +int32_t TMC51X0::to_motor_pos(int32_t pos) { // int32_t val = pos * 1.0 / m_scale * m_scale_deceleration * m_onecirclepulse + 0.5; return val; } // -int32_t TMC5130::to_user_pos(int32_t pos) { // +int32_t TMC51X0::to_user_pos(int32_t pos) { // int32_t val = pos / m_onecirclepulse * m_scale / m_scale_deceleration + 0.5; return val; } // -int32_t TMC5130::to_user_vel(int32_t vel) { // +int32_t TMC51X0::to_user_vel(int32_t vel) { // int32_t val = vel * 60.0 / m_onecirclepulse; return val; } diff --git a/components/tmc/ic/ztmc5130.hpp b/components/tmc/ic/ztmc5130.hpp index 90f62ef..6a7b980 100644 --- a/components/tmc/ic/ztmc5130.hpp +++ b/components/tmc/ic/ztmc5130.hpp @@ -41,9 +41,10 @@ class Tmc5130RampStat { Tmc5130RampStat(uint32_t state) : m_state(state) {} bool isSetted(ramp_stat_bit_t bit) { return (m_state & bit) != 0; } + }; -class TMC5130 : public IStepperMotor { +class TMC51X0 : public IStepperMotor { public: typedef struct { uint32_t sg_result : 10; @@ -98,7 +99,7 @@ class TMC5130 : public IStepperMotor { double m_onecirclepulse = 51200; public: - TMC5130(/* args */); + TMC51X0(/* args */); void initialize(cfg_t *cfg);