From 9aee8f2a2c26251c921e81bca81abc3a3a2f6927 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 5 Jun 2024 13:24:02 +0800 Subject: [PATCH] =?UTF-8?q?=E6=95=B4=E7=90=86TMC4361?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../step_motor_ctrl_module.cpp | 1 + components/tmc/basic/tmc_ic_interface.hpp | 99 +++++--------- components/tmc/ic/README.md | 18 +++ components/tmc/ic/tmc_driver_ic.hpp | 56 -------- components/tmc/ic/ztmc4361A.cpp | 150 +++++++++------------ components/tmc/ic/ztmc4361A.hpp | 133 ++++++------------ components/tmc/ic/ztmc5130.hpp | 19 --- 7 files changed, 153 insertions(+), 323 deletions(-) create mode 100644 components/tmc/ic/README.md delete mode 100644 components/tmc/ic/tmc_driver_ic.hpp 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 76703a4..3734a00 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -247,6 +247,7 @@ int32_t StepMotorCtrlModule::step_motor_active_cfg() { m_stepM1->setScale(m_cfg.motor_one_circle_pulse); m_stepM1->setScaleDenominator(m_cfg.motor_one_circle_pulse_denominator); m_stepM1->setMotorShaft(m_cfg.motor_shaft); + // stepmotor_iglobalscaler if (m_state.enable) { m_stepM1->enable(true); } diff --git a/components/tmc/basic/tmc_ic_interface.hpp b/components/tmc/basic/tmc_ic_interface.hpp index c6a1d78..a7ffa7a 100644 --- a/components/tmc/basic/tmc_ic_interface.hpp +++ b/components/tmc/basic/tmc_ic_interface.hpp @@ -3,71 +3,6 @@ namespace iflytop { using namespace std; class IStepperMotor; -class IStepperMotor { - public: - IStepperMotor() {} - virtual ~IStepperMotor() {} - - // virtual void registerListener(MotorEventListener* listener) = 0; - - /**************************************************** - * 速度模式 * - ****************************************************/ - /** - * @brief 旋转电机，正转为正值，负转为负值 - * - * @param velocity 电机旋转速度，单位为pps - */ - virtual void rotate(int32_t velocity) = 0; - /**************************************************** - * 位置模式 * - ****************************************************/ - /** - * @brief 电机运动到指定位置 - * - * @param position 目标位置 - * @param velocityMax 最大速度 电机旋转速度，单位为pps - */ - virtual void moveTo(int32_t position, uint32_t velocityMax) = 0; - /** - * @brief 电机相对于当前位置移动指定的距离 - * - * @param relativePosition 相对于当前位置的距离 - * @param velocityMax 最大速度 电机旋转速度，单位为pps - */ - virtual void moveBy(int32_t relativePosition, uint32_t velocityMax) = 0; - /** - * @brief 停止电机 - */ - virtual void stop() = 0; - - virtual int32_t getXACTUAL() = 0; - virtual void setXACTUAL(int32_t value) = 0; - virtual int32_t getVACTUAL() = 0; - virtual int32_t getXTARGET() { return 0; }; - - virtual int32_t getENC_POS() = 0; - virtual void setENC_POS(int32_t value) = 0; - - virtual void setAcceleration(float accelerationpps2) = 0; // 设置最大加速度 - virtual void setDeceleration(float accelerationpps2) = 0; // 设置最大减速度 - - virtual void setMotorShaft(bool reverse) = 0; - - virtual bool isReachTarget() = 0; - virtual bool isStoped() = 0; - - virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) = 0; - - virtual void setGlobalScale(uint8_t globalscale) = 0; - - virtual void setScale(int32_t scale) = 0; - virtual void setScaleDenominator(int32_t scale) = 0; - - virtual void enable(bool enable) = 0; - - virtual void setNoAccLimit(bool enable){}; -}; typedef enum { kmres_256 = 0x00, @@ -81,4 +16,38 @@ typedef enum { kmres_1 = 0x08, } mres_type_t; +typedef enum { + kTMC4361A = 0x2, + kTMC2160 = 0x30, +} tmcic_id_t; + +class IStepperMotor { + public: + IStepperMotor() {} + virtual ~IStepperMotor() {} + virtual void enable(bool enable) = 0; + + virtual void rotate(int32_t velocity) = 0; + virtual void moveTo(int32_t position, uint32_t velocityMax) = 0; + virtual void moveBy(int32_t relativePosition, uint32_t velocityMax) = 0; + virtual void stop() = 0; + + virtual void setXACTUAL(int32_t value) = 0; // 设置当前位置 + virtual int32_t getXACTUAL() = 0; // 当前位置 + virtual int32_t getVACTUAL() = 0; // 当前速度 + + virtual void setAcceleration(float accelerationpps2) = 0; // 设置最大加速度 + virtual void setDeceleration(float accelerationpps2) = 0; // 设置最大减速度 + virtual void setMotorShaft(bool reverse) = 0; + virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) = 0; + virtual void setGlobalScale(uint8_t globalscale) = 0; + virtual void setScale(int32_t scale) = 0; + virtual void setScaleDenominator(int32_t scale) = 0; + + virtual bool isReachTarget() = 0; // 是否到达目标位置 + virtual bool isStoped() = 0; // 是否停止运动 + + virtual void setNoAccLimit(bool enable) = 0; +}; + } // namespace iflytop diff --git a/components/tmc/ic/README.md b/components/tmc/ic/README.md new file mode 100644 index 0000000..0167587 --- /dev/null +++ b/components/tmc/ic/README.md @@ -0,0 +1,18 @@ +5130 +``` +/** + * @brief + * + * 注意事项： + * 1. 速度单位 + * velocity的单位是:p/t + * t = 2^24/fin (fin为外接时钟，当时钟频率为16MHz时，t = 1.048576s) + * 此时 ppt约等于pps + * + * 参考TMC5130A_datasheet_rev1.20%20(1)%20(3).pdf 38页 蓝框中的字 + * + * 2. 芯片复位方法 + * reset(); 复位芯片，同时会将芯片的寄存器设置为默认值。一般情况下不需要用户主动调用该方法，因为在初始化时会自动调用该方法。 + * + */ +``` \ No newline at end of file diff --git a/components/tmc/ic/tmc_driver_ic.hpp b/components/tmc/ic/tmc_driver_ic.hpp deleted file mode 100644 index f9b5583..0000000 --- a/components/tmc/ic/tmc_driver_ic.hpp +++ /dev/null @@ -1,56 +0,0 @@ -#pragma once -#include - -class TMCDriverICPort { - public: - public: - /** - * @brief 复位引脚，低有效。 - * - * @param state - */ - virtual void TMCDriverICPort_setResetPinState(uint16_t channel, bool state) = 0; - /** - * @brief 使能引脚，低有效。 - * - * @param state - */ - virtual void TMCDriverICPort_setENNPinState(uint16_t channel, bool state) = 0; - /** - * @brief us级别延时函数。 - * - * @param us - */ - virtual void TMCDriverICPort_sleepus(int32_t us) = 0; - /** - * @brief SPI读写接口 - * - * @param data - * @param length - */ - virtual void TMCDriverICPort_readWriteArray(uint16_t channel, uint8_t *data, size_t length) = 0; -}; - -class TMCDriverICInterface { - public: - virtual void initialize(uint8_t channel, TMCDriverICPort *port) = 0; - virtual uint8_t reset() = 0; - virtual uint8_t restore() = 0; - virtual void enableIC(bool enable) = 0; - - virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) = 0; - // register access - virtual void writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) = 0; - virtual void writeInt(uint8_t address, int32_t value) = 0; - virtual int32_t readInt(uint8_t address) = 0; - /** - * @brief 读取芯片版本 - * - * @return int32_t 默认为0x30 - */ - virtual int32_t readICVersion() = 0; - - virtual void setMotorShaft(bool reverse) = 0; - - virtual ~TMCDriverICInterface(){}; -}; diff --git a/components/tmc/ic/ztmc4361A.cpp b/components/tmc/ic/ztmc4361A.cpp index 3660994..924957f 100644 --- a/components/tmc/ic/ztmc4361A.cpp +++ b/components/tmc/ic/ztmc4361A.cpp @@ -10,8 +10,8 @@ #include "sdk/os/zos.hpp" using namespace iflytop; -#define PRV_FIELD_WRITE(address, mask, shift, value) (writeInt(address, FIELD_SET(readInt(address), mask, shift, value))) -#define PRV_FIELD_READ(address, mask, shift) FIELD_GET(readInt(address), mask, shift) +#define PRV_FIELD_WRITE(address, mask, shift, value) (tmc4361_writeInt(address, FIELD_SET(tmc4361_readInt(address), mask, shift, value))) +#define PRV_FIELD_READ(address, mask, shift) FIELD_GET(tmc4361_readInt(address), mask, shift) #define SET_PIN(pin, val) \ if (pin) { \ @@ -19,15 +19,15 @@ using namespace iflytop; } #if 1 -void TMC4361A::readWriteArray(uint8_t *data, size_t length) { +void TMC4361A::tmc4361_readWriteArray(uint8_t *data, size_t length) { m_csgpio->setState(false); // zchip_clock_early_delayus(10); zchip_clock_early_delayus(10); HAL_SPI_TransmitReceive(m_spi, data, data, length, 1000); m_csgpio->setState(true); } -void TMC4361A::writeInt(uint8_t address, int32_t value) { writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0)); } -int32_t TMC4361A::readInt(uint8_t address) { +void TMC4361A::tmc4361_writeInt(uint8_t address, int32_t value) { tmc4361_writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0)); } +int32_t TMC4361A::tmc4361_readInt(uint8_t address) { CriticalContext cc; int value; @@ -38,23 +38,23 @@ int32_t TMC4361A::readInt(uint8_t address) { if (!TMC_IS_READABLE(m_registerAccessTable[address])) return m_defaultRegisterResetState[address]; data[0] = address; - readWriteArray(&data[0], 5); + tmc4361_readWriteArray(&data[0], 5); data[0] = address; - readWriteArray(&data[0], 5); + tmc4361_readWriteArray(&data[0], 5); m_status = data[0]; value = ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4]; return value; } -void TMC4361A::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) { +void TMC4361A::tmc4361_writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) { CriticalContext cc; int value; uint8_t data[5] = {static_cast(address | static_cast(TMC4361A_WRITE_BIT)), x1, x2, x3, x4}; - readWriteArray(&data[0], 5); + tmc4361_readWriteArray(&data[0], 5); m_status = data[0]; @@ -65,7 +65,7 @@ void TMC4361A::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3 shadowRegister[address] = value; } -void TMC4361A::readWriteCover(uint8_t *data, size_t length) { +void TMC4361A::tmc4361_readWriteCover(uint8_t *data, size_t length) { CriticalContext cc; // Buffering old values to not interrupt manual covering int32_t old_high = shadowRegister[TMC4361A_COVER_HIGH_WR]; @@ -82,20 +82,20 @@ void TMC4361A::readWriteCover(uint8_t *data, size_t length) { for (i = 0; i < length; i++) bytes[i] = data[length - i - 1]; // Send the datagram - if (length > 4) writeDatagram(TMC4361A_COVER_HIGH_WR, bytes[7], bytes[6], bytes[5], bytes[4]); - writeDatagram(TMC4361A_COVER_LOW_WR, bytes[3], bytes[2], bytes[1], bytes[0]); + if (length > 4) tmc4361_writeDatagram(TMC4361A_COVER_HIGH_WR, bytes[7], bytes[6], bytes[5], bytes[4]); + tmc4361_writeDatagram(TMC4361A_COVER_LOW_WR, bytes[3], bytes[2], bytes[1], bytes[0]); zos_delay(10); // Read the reply if (length > 4) { - tmp = readInt(TMC4361A_COVER_DRV_HIGH_RD); + tmp = tmc4361_readInt(TMC4361A_COVER_DRV_HIGH_RD); bytes[4] = BYTE(tmp, 0); bytes[5] = BYTE(tmp, 1); bytes[6] = BYTE(tmp, 2); bytes[7] = BYTE(tmp, 3); } - tmp = readInt(TMC4361A_COVER_DRV_LOW_RD); + tmp = tmc4361_readInt(TMC4361A_COVER_DRV_LOW_RD); bytes[0] = BYTE(tmp, 0); bytes[1] = BYTE(tmp, 1); bytes[2] = BYTE(tmp, 2); @@ -107,7 +107,7 @@ void TMC4361A::readWriteCover(uint8_t *data, size_t length) { } // Rewriting old values to prevent interrupting manual covering. Imitating unchanged values and state. - writeInt(TMC4361A_COVER_HIGH_WR, old_high); + tmc4361_writeInt(TMC4361A_COVER_HIGH_WR, old_high); shadowRegister[TMC4361A_COVER_LOW_WR] = old_low; } @@ -116,16 +116,8 @@ TMC4361A::TMC4361A(/* args */) { m_lastCallPeriodicJobTick = 0; // m_reachtarget = false; } -/** - * @brief - * 当调用TMC-API中的tmc4361A_reset/restore时候，TMC-API中的方法会初始化整个芯片的寄存器 - * 当寄存器初始化完成之后，会调用这个方法?? - * 我们可以在这个方法中，对芯片的部分寄存器进行初始化?? - * @param state - */ -void TMC4361A::tmc4361AConfigCallback(ConfigState state) {} - -void TMC4361A::writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value) { // + +void TMC4361A::tmc4361_writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value) { // PRV_FIELD_WRITE(address, mask, shift, value); } void TMC4361A::setAcceleration(float accelerationpps2) { @@ -143,7 +135,7 @@ void TMC4361A::setAcceleration(float accelerationpps2) { accelerationpps2 = to_motor_acc(accelerationpps2); int32_t acc = (int32_t)accelerationpps2; - writeInt(TMC4361A_AMAX, acc << 2); + tmc4361_writeInt(TMC4361A_AMAX, acc << 2); } void TMC4361A::setDeceleration(float accelerationpps2) { /** @@ -158,7 +150,7 @@ void TMC4361A::setDeceleration(float accelerationpps2) { */ accelerationpps2 = to_motor_acc(accelerationpps2); int32_t acc = (int32_t)accelerationpps2; - writeInt(TMC4361A_DMAX, acc << 2); + tmc4361_writeInt(TMC4361A_DMAX, acc << 2); } #define INIT_GPIO(m_pin, pin, ...) \ if (pin != PinNull) { \ @@ -194,7 +186,7 @@ void TMC4361A::initialize(cfg_t *cfg) { setAcceleration(500000); setDeceleration(500000); - enableIC(false); + enable(false); zchip_clock_early_delayus(300 * 1000); zchip_clock_early_delayus(300 * 1000); @@ -219,7 +211,7 @@ uint8_t TMC4361A::reset() { if (!TMC_IS_RESETTABLE(m_registerAccessTable[add])) { continue; } - writeInt(add, m_defaultRegisterResetState[add]); + tmc4361_writeInt(add, m_defaultRegisterResetState[add]); } uint8_t driver, dataLength; @@ -241,21 +233,17 @@ uint8_t TMC4361A::reset() { break; } value = 0x44400040 | (dataLength << 13) | (driver << 0); - writeInt(TMC4361A_SPIOUT_CONF, value); - writeInt(TMC4361A_CURRENT_CONF, 0x00000003); - writeInt(TMC4361A_SCALE_VALUES, 0x00000000); + tmc4361_writeInt(TMC4361A_SPIOUT_CONF, value); + tmc4361_writeInt(TMC4361A_CURRENT_CONF, 0x00000003); + tmc4361_writeInt(TMC4361A_SCALE_VALUES, 0x00000000); return 1; } uint8_t TMC4361A::restore() { return 1; } -int32_t TMC4361A::getXACTUAL() { return to_user_pos(readInt(TMC4361A_XACTUAL)); } -void TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, to_motor_pos(value)); } -int32_t TMC4361A::getVACTUAL() { return to_user_pos(readInt(TMC4361A_VACTUAL)); } -int32_t TMC4361A::getXTARGET() { return to_user_pos(readInt(TMC4361A_X_TARGET)); } -int32_t TMC4361A::getENC_POS() { return to_user_pos(readInt(TMC4361A_ENC_POS)); } -void TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, to_motor_pos(value)); } -int32_t TMC4361A::getENC_POS_DEV() { return to_user_pos(readInt(TMC4361A_ENC_POS_DEV_RD)); } -void TMC4361A::enableIC(bool enable) { +int32_t TMC4361A::getXACTUAL() { return to_user_pos(tmc4361_readInt(TMC4361A_XACTUAL)); } +void TMC4361A::setXACTUAL(int32_t value) { tmc4361_writeInt(TMC4361A_XACTUAL, to_motor_pos(value)); } +int32_t TMC4361A::getVACTUAL() { return to_user_pos(tmc4361_readInt(TMC4361A_VACTUAL)); } +void TMC4361A::enable(bool enable) { SET_PIN(m_ennPin, !enable); SET_PIN(m_driverIC_ennPin, !enable); } @@ -271,7 +259,7 @@ void TMC4361A::rotate(int32_t velocity) { velocity = to_motor_vel(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)); + tmc4361_writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocity)); } void TMC4361A::stop() { rotate(0); } void TMC4361A::moveTo(int32_t position, uint32_t velocityMax) { @@ -283,46 +271,37 @@ void TMC4361A::moveTo(int32_t position, uint32_t velocityMax) { 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); + tmc4361_writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocityMax)); + tmc4361_writeInt(TMC4361A_X_TARGET, position); } void TMC4361A::moveBy(int32_t relativePosition, uint32_t velocityMax) { - relativePosition += readInt(TMC4361A_XACTUAL); + relativePosition += tmc4361_readInt(TMC4361A_XACTUAL); moveTo(relativePosition, velocityMax); } -int32_t TMC4361A::readICVersion() { - int32_t value = readInt(TMC4361A_VERSION_NO_RD); - return (value & TMC4361A_VERSION_NO_MASK) >> TMC4361A_VERSION_NO_SHIFT; +tmcic_id_t TMC4361A::readICVersion() { + int32_t value = tmc4361_readInt(TMC4361A_VERSION_NO_RD); + return (tmcic_id_t)((value & TMC4361A_VERSION_NO_MASK) >> TMC4361A_VERSION_NO_SHIFT); } -int32_t TMC4361A::readSubICVersion() { return driverIC_readICVersion(); } -/******************************************************************************* - * 2160 function end * - *******************************************************************************/ -uint32_t TMC4361A::readEVENTS() { - uint32_t value = readInt(TMC4361A_EVENTS); - return value; -} -uint32_t TMC4361A::haspassedms(uint32_t now, uint32_t last) { - if (now >= last) { - return now - last; - } else { - return 0xFFFFFFFF - last + now; - } -} bool TMC4361A::isReachTarget() { - uint32_t value = readInt(TMC4361A_STATUS); - // printf("TMC4361A_STATUS:%08x\n", value); + uint32_t value = tmc4361_readInt(TMC4361A_STATUS); if (m_motor_mode == kposmode) { return (value & TMC4361A_TARGET_REACHED_F_MASK) > 0; } else { - return (value & TMC4361A_VEL_REACHED_F_MASK) && (readInt(TMC4361A_VMAX) == 0); + return (value & TMC4361A_VEL_REACHED_F_MASK) && (tmc4361_readInt(TMC4361A_VMAX) == 0); } } -/******************************************************************************* - * DRIVER_IC * - *******************************************************************************/ + +void TMC4361A::setGLOBAL_SCALER(int32_t value) { + if (value > 255) value = 255; + if (value != 0 && value < 32) value = 32; + driverIC_writeInt(TMC2160_GLOBAL_SCALER, value); +} + +/*********************************************************************************************************************** + * DRIVERIC_OPERATION * + ***********************************************************************************************************************/ #define DRIVER_ID_FIELD_READ(address, mask, shift) FIELD_GET(driverIC_readInt(address), mask, shift) #define DRIVER_ID_FIELD_WRITE(address, mask, shift, value) (driverIC_writeInt(address, FIELD_SET(driverIC_readInt(address), mask, shift, value))) void TMC4361A::driverIC_reset() { @@ -350,7 +329,7 @@ void TMC4361A::driverIC_reset() { void TMC4361A::driverIC_enableIC(bool enable) { SET_PIN(m_driverIC_ennPin, !enable); } void TMC4361A::driverIC_writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) { uint8_t data[5] = {static_cast(address | static_cast(TMC2160_WRITE_BIT)), x1, x2, x3, x4}; - readWriteCover(&data[0], 5); + tmc4361_readWriteCover(&data[0], 5); } void TMC4361A::driverIC_writeInt(uint8_t address, int32_t value) { driverIC_writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0)); } int32_t TMC4361A::driverIC_readInt(uint8_t address) { @@ -361,10 +340,10 @@ int32_t TMC4361A::driverIC_readInt(uint8_t address) { uint8_t data[5]; data[0] = address; - readWriteCover(&data[0], 5); + tmc4361_readWriteCover(&data[0], 5); data[0] = address; - readWriteCover(&data[0], 5); + tmc4361_readWriteCover(&data[0], 5); return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4]; } @@ -380,24 +359,9 @@ uint32_t TMC4361A::driverIC_readICVersion() { } void TMC4361A::driverIC_setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_writeInt(TMC2160_IHOLD_IRUN, (iholddelay << TMC2160_IHOLDDELAY_SHIFT) | (irun << TMC2160_IRUN_SHIFT) | (ihold << TMC2160_IHOLD_SHIFT)); } -void TMC4361A::setGLOBAL_SCALER(int32_t value) { - if (value > 255) value = 255; - if (value != 0 && value < 32) value = 32; - driverIC_writeInt(TMC2160_GLOBAL_SCALER, value); -} - -// Left Virtual Limit Switch XACTUAL ≤ VIRT_STOP_LEFT 时触发 -void TMC4361A::setLeftVirtualLimitSwitch(bool enable, int32_t position) { - PRV_FIELD_WRITE(TMC4361A_REFERENCE_CONF, TMC4361A_VIRTUAL_LEFT_LIMIT_EN_MASK, TMC4361A_VIRTUAL_LEFT_LIMIT_EN_SHIFT, enable ? 1 : 0); - writeInt(TMC4361A_VIRT_STOP_LEFT, position); -} - -// Right Virtual Limit Switch XACTUAL ≥ VIRT_STOP_RIGHT 时触发 -void TMC4361A::setRightVirtualLimitSwitch(bool enable, int32_t position) { - PRV_FIELD_WRITE(TMC4361A_REFERENCE_CONF, TMC4361A_VIRTUAL_RIGHT_LIMIT_EN_MASK, TMC4361A_VIRTUAL_RIGHT_LIMIT_EN_SHIFT, enable ? 1 : 0); - writeInt(TMC4361A_VIRT_STOP_RIGHT, position); -} - +/*********************************************************************************************************************** + * POS_CONVERT * + ***********************************************************************************************************************/ int32_t TMC4361A::to_motor_acc(int32_t acc) { // int32_t val = acc / 60.0 * 51200; return val; @@ -419,7 +383,13 @@ int32_t TMC4361A::to_user_vel(int32_t vel) { // int32_t val = vel * 60.0 / 51200.0; return val; } - +uint32_t TMC4361A::haspassedms(uint32_t now, uint32_t last) { + if (now >= last) { + return now - last; + } else { + return 0xFFFFFFFF - last + now; + } +} #endif #endif diff --git a/components/tmc/ic/ztmc4361A.hpp b/components/tmc/ic/ztmc4361A.hpp index d8a020a..f22b92a 100644 --- a/components/tmc/ic/ztmc4361A.hpp +++ b/components/tmc/ic/ztmc4361A.hpp @@ -8,7 +8,6 @@ #include "../basic/tmc_ic_interface.hpp" #include "sdk/os/zos.hpp" -#include "tmc_driver_ic.hpp" extern "C" { #include "TMC2160\TMC2160.h" #include "TMC4361A\TMC4361A.h" @@ -25,18 +24,6 @@ class TMC4361A : public IStepperMotor { IC_TMC2660, } driver_ic_type_t; -#if 0 - typedef struct { - SPI_HandleTypeDef *spi; // - ZGPIO *csgpio; // - ZGPIO *resetPin; // - ZGPIO *fREEZEPin; // - ZGPIO *ennPin; // - ZGPIO *driverIC_ennPin; // - ZGPIO *driverIC_resetPin; // - } cfg_t; -#endif - class cfg_t { public: SPI_HandleTypeDef *spi = nullptr; // @@ -80,96 +67,56 @@ class TMC4361A : public IStepperMotor { public: TMC4361A(/* args */); - /******************************************************************************* - * 初始化相关方 * - *******************************************************************************/ - - /** - * @brief 静态方法，创建默认的TMC4361A配置参数,使用时只需修改自己需要的参数即可 - * - * 注意: - * 1. 该方法内部使用的是一个静态变量，所以每次调用该方法时，返回的都是同一个对象的地址 - * 2. 该方法返回的数值不需要被释放 - * @param config - */ + void initialize(cfg_t *cfg); - void initialize(cfg_t *cfg); - void enableIC(bool enable); - virtual void enable(bool enable) { enableIC(enable); } + /*********************************************************************************************************************** + * IStepperMotor * + ***********************************************************************************************************************/ - /******************************************************************************* - * IStepperMotor impl * - *******************************************************************************/ - // virtual void registerListener(MotorEventListener *listener); - virtual void rotate(int32_t velocity); - virtual void moveTo(int32_t position, uint32_t velocityMax); - virtual void moveBy(int32_t relativePosition, uint32_t velocityMax); - virtual void stop(); - virtual int32_t getXACTUAL(); - virtual int32_t getXTARGET(); - virtual void setXACTUAL(int32_t value); - virtual int32_t getVACTUAL(); - virtual int32_t getENC_POS(); - virtual void setENC_POS(int32_t value); - virtual void setAcceleration(float accelerationpps2); // 设置最大加速度,最大值4000000 - virtual void setDeceleration(float accelerationpps2); // 设置最大减速度,最大值4000000 + virtual void enable(bool enable); - virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_setIHOLD_IRUN(ihold, irun, iholddelay); } - virtual void setMotorShaft(bool reverse) { driverIC_setMotorShaft(reverse); } + virtual void rotate(int32_t velocity); + virtual void moveTo(int32_t position, uint32_t velocityMax); + virtual void moveBy(int32_t relativePosition, uint32_t velocityMax); + virtual void stop(); - //???????????? - void setGLOBAL_SCALER(int32_t value); + virtual void setXACTUAL(int32_t value); // 设置当前位置 + virtual int32_t getXACTUAL(); // 当前位置 + virtual int32_t getVACTUAL(); // 当前速度 + virtual void setAcceleration(float accelerationpps2); // 设置最大加速度 + 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 setScale(int32_t scale); virtual void setScaleDenominator(int32_t scale); - // Left Virtual Limit Switch XACTUAL ≤ VIRT_STOP_LEFT 时触发 - void setLeftVirtualLimitSwitch(bool enable, int32_t position); + virtual bool isReachTarget(); + virtual bool isStoped() { return isReachTarget(); } + virtual void setNoAccLimit(bool enable) { return; }; // TODO:impl it - // Right Virtual Limit Switch XACTUAL ≥ VIRT_STOP_RIGHT 时触发 - void setRightVirtualLimitSwitch(bool enable, int32_t position); + void setGLOBAL_SCALER(int32_t value); - /******************************************************************************* - * 驱动器初始化方法 * - *******************************************************************************/ - /** - * @brief 初始化方 - * - * @param channel SPI通道 - * @param driver_ic_type 驱动芯片的类 - * - * TMC4361A:0x2 DriverIC:0x30 - */ - int32_t readICVersion(); - uint8_t reset(); - uint8_t restore(); + /*********************************************************************************************************************** + * 4361Opera * + ***********************************************************************************************************************/ + tmcic_id_t readICVersion(); + uint8_t reset(); + uint8_t restore(); /******************************************************************************* * 驱动器寄存器读写方法 * *******************************************************************************/ - void writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4); - void writeInt(uint8_t address, int32_t value); - int32_t readInt(uint8_t address); - void readWriteCover(uint8_t *data, size_t length); - void writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value); - // ???????????? - - int32_t getENC_POS_DEV(); // ENC_POS和XACTUAL的偏差 - uint32_t readEVENTS(); // 读取事件寄存 - /******************************************************* - * driverIc function * - *******************************************************/ - int32_t readSubICVersion(); - virtual bool isReachTarget(); - virtual bool isStoped() { return isReachTarget(); } + void tmc4361_writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4); + void tmc4361_writeInt(uint8_t address, int32_t value); + int32_t tmc4361_readInt(uint8_t address); + void tmc4361_readWriteCover(uint8_t *data, size_t length); + void tmc4361_writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value); public: - // only used in tmc4361A.cpp - void tmc4361AConfigCallback(ConfigState state); - void readWriteArray(uint8_t *data, size_t length); - - private: - uint32_t haspassedms(uint32_t now, uint32_t last); + void tmc4361_readWriteArray(uint8_t *data, size_t length); + public: void driverIC_reset(); void driverIC_enableIC(bool enable); void driverIC_setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay); @@ -180,12 +127,12 @@ class TMC4361A : public IStepperMotor { uint32_t driverIC_readICVersion(); private: - int32_t to_motor_acc(int32_t acc); // rpm/s^2 - int32_t to_motor_vel(int32_t vel); // rpm - int32_t to_motor_pos(int32_t pos); // - - int32_t to_user_pos(int32_t pos); // - int32_t to_user_vel(int32_t vel); + int32_t to_motor_acc(int32_t acc /* rpm/s^2*/); + int32_t to_motor_vel(int32_t vel /*rpm*/); + int32_t to_motor_pos(int32_t pos); + int32_t to_user_pos(int32_t pos); + int32_t to_user_vel(int32_t vel); + uint32_t haspassedms(uint32_t now, uint32_t last); }; } // namespace iflytop diff --git a/components/tmc/ic/ztmc5130.hpp b/components/tmc/ic/ztmc5130.hpp index 635a199..a3194f1 100644 --- a/components/tmc/ic/ztmc5130.hpp +++ b/components/tmc/ic/ztmc5130.hpp @@ -13,26 +13,10 @@ extern "C" { #ifdef HAL_SPI_MODULE_ENABLED namespace iflytop { #define TMC5130_LISTENER_MAX 5 -/** - * @brief - * - * 注意事项： - * 1. 速度单位 - * velocity的单位是:p/t - * t = 2^24/fin (fin为外接时钟，当时钟频率为16MHz时，t = 1.048576s) - * 此时 ppt约等于pps - * - * 参考TMC5130A_datasheet_rev1.20%20(1)%20(3).pdf 38页 蓝框中的字 - * - * 2. 芯片复位方法 - * reset(); 复位芯片，同时会将芯片的寄存器设置为默认值。一般情况下不需要用户主动调用该方法，因为在初始化时会自动调用该方法。 - * - */ class Tmc5130RampStat { public: uint32_t m_state; - // 参考TMC5130A_datasheet_rev1.20%20(1).pdf 40页 typedef enum { /** * @brief @@ -116,9 +100,6 @@ class TMC5130 : public IStepperMotor { virtual int32_t getVACTUAL(); // 读取电机当前位置,与编码器的位置值不同，该值是电机在不丢步的情况下的位置 uint32_t readXTARGET(); // 读取驱动器目标位置 - virtual int32_t getENC_POS() { return 0; } // TODO impl it - virtual void setENC_POS(int32_t value) {} // TODO impl it - virtual void setScale(int32_t scale); virtual void setScaleDenominator(int32_t scale);