diff --git a/components/tmc/ic/old/ztmc2160.cpp b/components/tmc/ic/old/ztmc2160.cpp deleted file mode 100644 index e17d002..0000000 --- a/components/tmc/ic/old/ztmc2160.cpp +++ /dev/null @@ -1,76 +0,0 @@ -#if 0 -#include "ztmc2160.hpp" - -#include "TMC2160/TMC2160.h" - -using namespace iflytop; - -#define ZFIELD_READ(address, mask, shift) FIELD_GET(readInt(address), mask, shift) -#define ZFIELD_WRITE(address, mask, shift, value) (writeInt(address, FIELD_SET(readInt(address), mask, shift, value))) - -void TMC2160::initialize(uint8_t channel, TMCDriverICPort *port) { - m_channel = channel; - m_port = port; -} -void TMC2160::setMotorShaft(bool reverse) { // - int32_t val = reverse ? 1 : 0; - ZFIELD_WRITE(TMC2160_GCONF, TMC2160_SHAFT_MASK, TMC2160_SHAFT_SHIFT, val); -} - -uint8_t TMC2160::reset() { - m_port->TMCDriverICPort_setResetPinState(m_channel, false); - m_port->TMCDriverICPort_sleepus(1000); - m_port->TMCDriverICPort_setResetPinState(m_channel, true); - // Reset the dirty bits - int index = 0; - while (true) { - while ((index < TMC2160_REGISTER_COUNT) && !TMC_IS_RESTORABLE(tmc2160_defaultRegisterAccess[index])) { - index++; - } - if (index >= TMC2160_REGISTER_COUNT) { - break; - } - // printf("Resetting register %d\n", index); - writeInt(index, tmc2160_defaultRegisterResetState[index]); - index++; - } - - writeInt(TMC2160_PWMCONF, 0xC40C001E); - writeInt(TMC2160_DRV_CONF, 0x00080400); - // tmc2160_writeInt(tmc2160, TMC2160_PWMCONF, 0x000504C8); - return true; -} -uint8_t TMC2160::restore() { return 1; } -void TMC2160::enableIC(bool enable) { m_port->TMCDriverICPort_setENNPinState(m_channel, !enable); } -void TMC2160::setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { - writeInt(TMC2160_IHOLD_IRUN, (iholddelay << TMC2160_IHOLDDELAY_SHIFT) | (irun << TMC2160_IRUN_SHIFT) | (ihold << TMC2160_IHOLD_SHIFT)); -} -void TMC2160::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}; - m_port->TMCDriverICPort_readWriteArray(m_channel, &data[0], 5); - // int value = ((uint32_t)x1 << 24) | ((uint32_t)x2 << 16) | (x3 << 8) | x4; -} -void TMC2160::writeInt(uint8_t address, int32_t value) { // - writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0)); -} -int32_t TMC2160::readInt(uint8_t address) { - address = TMC_ADDRESS(address); - - // register not readable -> shadow register copy - if (!TMC_IS_READABLE(tmc2160_defaultRegisterAccess[address])) return 0; - - uint8_t data[5]; - - data[0] = address; - m_port->TMCDriverICPort_readWriteArray(m_channel, &data[0], 5); - - data[0] = address; - m_port->TMCDriverICPort_readWriteArray(m_channel, &data[0], 5); - - return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4]; -} -int32_t TMC2160::readICVersion() { - int32_t value = readInt(TMC2160_IOIN___OUTPUT); - return (value & TMC2160_VERSION_MASK) >> TMC2160_VERSION_SHIFT; -} -#endif \ No newline at end of file diff --git a/components/tmc/ic/old/ztmc2160.hpp b/components/tmc/ic/old/ztmc2160.hpp deleted file mode 100644 index 48225f2..0000000 --- a/components/tmc/ic/old/ztmc2160.hpp +++ /dev/null @@ -1,46 +0,0 @@ -#if 0 -#pragma once -#include -#include -#include -#include -extern "C" { -#include "TMC2160/TMC2160.h" -#include "tmc_driver_ic.hpp" -} -#include "../basic/tmc_ic_interface.hpp" -#include "sdk/hal/zhal.hpp" -namespace iflytop { -class TMC2160 : public TMCDriverICInterface { - protected: - TMC2160TypeDef m_tmc_handler; - uint8_t m_channel; - TMCDriverICPort* m_port; - - public: - TMC2160(/* args */){}; - virtual ~TMC2160(){}; - - virtual void initialize(uint8_t channel, TMCDriverICPort* port); - virtual uint8_t reset(); - virtual uint8_t restore(); - virtual void enableIC(bool enable); - - virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay); - - // register access - virtual void writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4); - virtual void writeInt(uint8_t address, int32_t value); - virtual int32_t readInt(uint8_t address); - /** - * @brief 读取芯片版本 - * - * @return int32_t 默认为0x30 - */ - virtual int32_t readICVersion(); - virtual void setMotorShaft(bool reverse); - - public: -}; -} // namespace iflytop -#endif diff --git a/components/tmc/ic/old/ztmc4361A.cpp b/components/tmc/ic/old/ztmc4361A.cpp deleted file mode 100644 index 91da210..0000000 --- a/components/tmc/ic/old/ztmc4361A.cpp +++ /dev/null @@ -1,526 +0,0 @@ -#if 0 -#include - -#include "../basic/basic.hpp" -#include "./TMC4361A/TMC4361A.h" -#include "ztmc4361A.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) - -#if 1 -void TMC4361A::readWriteArray(uint8_t *data, size_t length) { m_config.m_port->TMC4361APort_readWriteArray(m_channel, data, length); } -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) { - CriticalContext cc; - - int value; - uint8_t data[5]; - - address = TMC_ADDRESS(address); - - if (!TMC_IS_READABLE(m_registerAccessTable[address])) return m_defaultRegisterResetState[address]; - - data[0] = address; - readWriteArray(&data[0], 5); - - data[0] = address; - 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) { - CriticalContext cc; - - int value; - uint8_t data[5] = {static_cast(address | static_cast(TMC4361A_WRITE_BIT)), x1, x2, x3, x4}; - - readWriteArray(&data[0], 5); - - m_status = data[0]; - - value = ((uint32_t)x1 << 24) | ((uint32_t)x2 << 16) | (x3 << 8) | x4; - - // Write to the shadow register and mark the register dirty - address = TMC_ADDRESS(address); - shadowRegister[address] = value; -} - -void TMC4361A::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]; - int32_t old_low = shadowRegister[TMC4361A_COVER_LOW_WR]; - - // Check if datagram length is valid - if (length == 0 || length > 8) return; - - uint8_t bytes[8] = {0}; - uint32_t tmp; - size_t i; - - // Copy data into buffer of maximum cover datagram length (8 bytes) - 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]); - - m_config.m_port->TMC4361APort_sleepus(10 * 1000); - - // Read the reply - if (length > 4) { - tmp = 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); - bytes[0] = BYTE(tmp, 0); - bytes[1] = BYTE(tmp, 1); - bytes[2] = BYTE(tmp, 2); - bytes[3] = BYTE(tmp, 3); - - // Write the reply to the data array - for (i = 0; i < length; i++) { - data[length - i - 1] = bytes[i]; - } - - // Rewriting old values to prevent interrupting manual covering. Imitating unchanged values and state. - writeInt(TMC4361A_COVER_HIGH_WR, old_high); - shadowRegister[TMC4361A_COVER_LOW_WR] = old_low; -} - -TMC4361A::TMC4361A(/* args */) { - m_channel = 0; - m_driver_ic_type = IC_TMC2130; - memset(&m_config, 0, sizeof(m_config)); - 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) { // - PRV_FIELD_WRITE(address, mask, shift, value); -} -void TMC4361A::setAcceleration(float accelerationpps2) { - /** - * @brief - * TMC4361A_AMAX:有两种模式，这里使用的是频率模式 - * - * Frequency mode: [pulses per sec2] - - * 22 digits and 2 decimal places: 250 mpps^2 ≤ AMAX ≤ 4 Mpps^2 - * Direct mode: [∆v per clk cycle] - * a[∆v per clk_cycle]= AMAX / 2^37 - * AMAX [pps2] = AMAX / 237 • fCLK^2 - */ - - int32_t acc = (int32_t)accelerationpps2; - writeInt(TMC4361A_AMAX, acc << 2); -} -void TMC4361A::setDeceleration(float accelerationpps2) { - /** - * @brief - * TMC4361A_DMAX:有两种模式，这里使用的是频率模式 - * - * Frequency mode: [pulses per sec2] - * 22 digits and 2 decimal places: 250 mpps^2 ≤ AMAX ≤ 4 Mpps^2 - * Direct mode: [∆v per clk cycle] - * a[∆v per clk_cycle]= AMAX / 2^37 - * AMAX [pps2] = AMAX / 237 • fCLK^2 - */ - - int32_t acc = (int32_t)accelerationpps2; - writeInt(TMC4361A_DMAX, acc << 2); -} - -void TMC4361A::initialize(uint8_t channel, driver_ic_type_t driver_ic_type, TMC4361AConfig_t *config) { - m_channel = channel; - m_port = config->m_port; - m_driver_ic_type = driver_ic_type; - m_registerAccessTable = &tmc4361A_defaultRegisterAccess[0]; - m_defaultRegisterResetState = &tmc4361A_defaultRegisterResetState[0]; - - memset(shadowRegister, 0, sizeof(shadowRegister)); - memcpy(&m_config, config, sizeof(TMC4361AConfig_t)); - - m_config.m_port->TMC4361APort_setResetPinState(m_channel, true); - m_config.m_port->TMC4361APort_setFREEZEPinState(m_channel, true); - m_config.m_port->TMC4361APort_setENNPinState(m_channel, true); - - switch (m_driver_ic_type) { - case IC_TMC2160: - case IC_TMC2130: - m_driverIC = &m_tmc2160; - break; - default: - break; - } - TMC_ASSERT(m_driverIC != NULL, "TMC4361A::initialize() m_driverIC is NULL"); - reset(); - - m_driverIC->initialize(0, this); - m_driverIC->reset(); - - baseInit(&config->base_config); - encoder_init(&config->encoder_config); - close_loop_init(&config->close_loop_config); - - setAcceleration(100000); - setDeceleration(100000); - - enableIC(true); - m_port->TMC4361APort_sleepus(300 * 1000); - if (m_config.close_loop_config.enable_closed_loop) calibrateClosed(); - m_port->TMC4361APort_sleepus(300 * 1000); - - getDriverIC()->setIHOLD_IRUN(1, 3, 0); // 注意要先设置IHOLD再设置IRUN,否则电机跑不起来 -} -uint8_t TMC4361A::reset() { - // Pulse the low-active hardware reset pin - stop(); - m_config.m_port->TMC4361APort_setResetPinState(m_channel, false); - m_config.m_port->TMC4361APort_sleepus(1000); - m_config.m_port->TMC4361APort_setResetPinState(m_channel, true); - - /** - * @brief 重置芯片镜像寄存器 - * - */ - for (uint32_t add = 0; add < TMC4361A_REGISTER_COUNT; add++) { - if (!TMC_IS_RESETTABLE(m_registerAccessTable[add])) { - continue; - } - writeInt(add, m_defaultRegisterResetState[add]); - } - - uint8_t driver, dataLength; - uint32_t value; - // Setup SPI - switch (m_driver_ic_type) { - case IC_TMC2130: - case IC_TMC2160: - driver = 0x0C; - dataLength = 0; - break; - case IC_TMC2660: - driver = 0x0B; - dataLength = 0; - break; - default: - driver = 0x0F; - dataLength = 40; - break; - } - value = 0x44400040 | (dataLength << 13) | (driver << 0); - writeInt(TMC4361A_SPIOUT_CONF, value); - writeInt(TMC4361A_CURRENT_CONF, 0x00000003); - writeInt(TMC4361A_SCALE_VALUES, 0x00000000); - return 1; -} -uint8_t TMC4361A::restore() { return 1; } - -int32_t TMC4361A::getXACTUAL() { return readInt(TMC4361A_XACTUAL); } -void TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, value); } -int32_t TMC4361A::getVACTUAL() { return readInt(TMC4361A_VACTUAL); } -int32_t TMC4361A::getXTARGET() { return readInt(TMC4361A_X_TARGET); } -int32_t TMC4361A::getENC_POS() { return readInt(TMC4361A_ENC_POS); } -void TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, value); } -int32_t TMC4361A::getENC_POS_DEV() { return readInt(TMC4361A_ENC_POS_DEV_RD); } -void TMC4361A::enableIC(bool enable) { - m_config.m_port->TMC4361APort_setENNPinState(m_channel, !enable); - m_driverIC->enableIC(enable); -} - -int32_t tmc4361A_discardVelocityDecimals(int32_t value) { - if (abs(value) > 8000000) { - value = (value < 0) ? -8000000 : 8000000; - } - return value << 8; -} -void TMC4361A::rotate(int32_t velocity) { - 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; - 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); -} - -int32_t TMC4361A::readICVersion() { - int32_t value = readInt(TMC4361A_VERSION_NO_RD); - return (value & TMC4361A_VERSION_NO_MASK) >> TMC4361A_VERSION_NO_SHIFT; -} - -/** - * @brief TODO:添加闭环初始化逻辑 - */ - -void TMC4361A::baseInit(base_config_t *config) { // - writeSubRegister(TMC4361A_STEP_CONF, TMC4361A_FS_PER_REV_MASK, TMC4361A_FS_PER_REV_SHIFT, config->fs_per_rev); - writeSubRegister(TMC4361A_STEP_CONF, TMC4361A_MSTEP_PER_FS_MASK, TMC4361A_MSTEP_PER_FS_SHIFT, config->mstep_per_fs); - - /** - * @brief clear XACTUAL and ENC_POS - */ - writeInt(TMC4361A_XACTUAL, 0); - writeInt(TMC4361A_ENC_POS, 0); -} -void TMC4361A::close_loop_init(close_loop_config_t *config) { - if (!config->enable_closed_loop) { - return; - } - writeInt(TMC4361A_CL_VMIN_EMF_WR, config->gamma_v_min); /*mark*/ - writeInt(TMC4361A_CL_VADD_EMF, config->gamma_v_add); /*mark*/ - writeInt(TMC4361A_CL_BETA, (config->gamma << 16) | config->beta); /*mark*/ - writeInt(TMC4361A_CL_OFFSET, config->offset); - writeInt(TMC4361A_CL_VMAX_CALC_P_WR, config->correction_velocity_p); /*mark*/ - writeInt(TMC4361A_CL_VMAX_CALC_I_WR, config->correction_velocity_i); /*mark*/ - writeDatagram(TMC4361A_PID_I_CLIP_WR, 0, config->correction_velocity_d_clk, config->correction_velocity_i_clip >> 8, - config->correction_velocity_i_clip & 0xFF); - writeDatagram(TMC4361A_PID_I_CLIP_WR, 0, config->correction_velocity_d_clk, config->correction_velocity_i_clip >> 8, - config->correction_velocity_i_clip & 0xFF); - writeInt(TMC4361A_PID_DV_CLIP_WR, config->correction_velocity_d_clip); /*mark*/ - writeSubRegister(TMC4361A_SCALE_VALUES, TMC4361A_CL_IMIN_MASK, TMC4361A_CL_IMIN_SHIFT, config->current_scaler_minimum); /*mark*/ - writeSubRegister(TMC4361A_SCALE_VALUES, TMC4361A_CL_IMAX_MASK, TMC4361A_CL_IMAX_SHIFT, config->current_scaler_maximum); /*mark*/ - writeSubRegister(TMC4361A_SCALE_VALUES, TMC4361A_CL_START_UP_MASK, TMC4361A_CL_START_UP_SHIFT, config->current_scaler_start_up); /*mark*/ - writeInt(TMC4361A_CL_UPSCALE_DELAY, config->upscale_delay); /*mark*/ - writeInt(TMC4361A_CL_DOWNSCALE_DELAY, config->downscale_delay); /*mark*/ - writeInt(TMC4361A_CL_DELTA_P_WR, config->position_correction_p); /*mark*/ - writeInt(TMC4361A_CL_TOLERANCE_WR, config->position_correction_tolerance); /*mark*/ - writeInt(TMC4361A_CL_TR_TOLERANCE_WR, config->position_window); /*mark*/ - writeDatagram(TMC4361A_ENC_VMEAN_WAIT_WR, config->enc_v_mean_int >> 8, config->enc_v_mean_int & 0xFF, config->enc_v_mean_filter, config->enc_v_mean_wait); - writeDatagram(TMC4361A_ENC_COMP_XOFFSET, 0, config->encoder_correction_y_offset, 0, 0); - - PRV_FIELD_WRITE(TMC4361A_ENC_IN_CONF, TMC4361A_CL_VELOCITY_MODE_EN_MASK, TMC4361A_CL_VELOCITY_MODE_EN_SHIFT, 1); -} - -uint8_t TMC4361A::tmc4361A_calibrateClosedLoop(int &state, uint32_t &oldRamp, uint8_t worker0master1) { - uint32_t amax = 0; - uint32_t dmax = 0; - - if (worker0master1 && state == 0) state = 1; - - switch (state) { - case 1: - amax = readInt(TMC4361A_AMAX); - dmax = readInt(TMC4361A_DMAX); - - // Set ramp and motion parameters - oldRamp = readInt(TMC4361A_RAMPMODE); - writeInt(TMC4361A_RAMPMODE, TMC4361A_RAMP_POSITION | TMC4361A_RAMP_HOLD); - writeInt(TMC4361A_AMAX, MAX(amax, 1000)); - writeInt(TMC4361A_DMAX, MAX(dmax, 1000)); - writeInt(TMC4361A_VMAX, 0); - - state = 2; - break; - case 2: - // Clear encoder calibration bit - PRV_FIELD_WRITE(TMC4361A_ENC_IN_CONF, TMC4361A_CL_CALIBRATION_EN_MASK, TMC4361A_CL_CALIBRATION_EN_SHIFT, 0); - - // Disable internal data regulation for closed loop operation in encoder config - PRV_FIELD_WRITE(TMC4361A_ENC_IN_CONF, TMC4361A_REGULATION_MODUS_MASK, TMC4361A_REGULATION_MODUS_SHIFT, 1); - - if (tmc4361A_moveToNextFullstep()) // move to next fullstep, motor must be stopped, poll until finished - state = 3; - break; - case 3: - // Start encoder calibration - PRV_FIELD_WRITE(TMC4361A_ENC_IN_CONF, TMC4361A_CL_CALIBRATION_EN_MASK, TMC4361A_CL_CALIBRATION_EN_SHIFT, 1); - - state = 4; - break; - case 4: - if (worker0master1) break; - - // Stop encoder calibration - PRV_FIELD_WRITE(TMC4361A_ENC_IN_CONF, TMC4361A_CL_CALIBRATION_EN_MASK, TMC4361A_CL_CALIBRATION_EN_SHIFT, 0); - // Enable closed loop in encoder config - PRV_FIELD_WRITE(TMC4361A_ENC_IN_CONF, TMC4361A_REGULATION_MODUS_MASK, TMC4361A_REGULATION_MODUS_SHIFT, 1); - // Restore old ramp mode, enable position mode - writeInt(TMC4361A_RAMPMODE, TMC4361A_RAMP_POSITION | oldRamp); - - state = 5; - break; - case 5: - state = 0; - return 1; - break; - default: - break; - } - return 0; -} - -uint8_t TMC4361A::tmc4361A_moveToNextFullstep() { - int32_t stepCount; - - // Motor must be stopped - if (readInt(TMC4361A_VACTUAL) != 0) { - // Not stopped - return 0; - } - - // Position mode, hold mode, low velocity - writeInt(TMC4361A_RAMPMODE, 4); - writeInt(TMC4361A_VMAX, 10000 << 8); - - // Current step count - stepCount = PRV_FIELD_READ(TMC4361A_MSCNT_RD, TMC4361A_MSCNT_MASK, TMC4361A_MSCNT_SHIFT); - // Get microstep value of step count (lowest 8 bits) - stepCount = stepCount % 256; - // Assume: 256 microsteps -> Fullsteps are at 128 + n*256 - stepCount = 128 - stepCount; - - if (stepCount == 0) { - // Fullstep reached - return 1; - } - - // Fullstep not reached -> calculate next fullstep position - stepCount += readInt(TMC4361A_XACTUAL); - // Move to next fullstep position - writeInt(TMC4361A_X_TARGET, stepCount); - - return 0; -} - -void TMC4361A::calibrateClosed() { - /** - * @brief - * 这个方法必须在电机闭环运动前调用，并且保证电机当前没有运行 - */ - int state = 0; - uint32_t oldRamp = 0; - tmc4361A_calibrateClosedLoop(state, oldRamp, 1); - int calibrateClosedLoopTimes = 0; - while (!tmc4361A_calibrateClosedLoop(state, oldRamp, 0)) { - for (size_t i = 0; i < 10; i++) { - m_config.m_port->TMC4361APort_sleepus(1000); - } - calibrateClosedLoopTimes++; - if (calibrateClosedLoopTimes > 100) { - /** - * @brief TODO:添加错误处理 - */ - return; - } - } - return; -} - -void TMC4361A::encoder_init(encoder_config_t *config) { - if (config->encoder_mode == ENCODER_MODE_INCREMENTAL) { - writeSubRegister(TMC4361A_GENERAL_CONF, TMC4361A_SERIAL_ENC_IN_MODE_MASK, TMC4361A_SERIAL_ENC_IN_MODE_SHIFT, 0); - } else { - // Not support now - TMC_ASSERT(false, "Not support now"); - return; - } - writeSubRegister(TMC4361A_GENERAL_CONF, TMC4361A_DIFF_ENC_IN_DISABLE_MASK, TMC4361A_DIFF_ENC_IN_DISABLE_SHIFT, config->diff_enc_in_disable); - writeSubRegister(TMC4361A_ENC_IN_CONF, TMC4361A_INVERT_ENC_DIR_MASK, TMC4361A_INVERT_ENC_DIR_SHIFT, config->invert_enc_dir); - writeSubRegister(TMC4361A_ENC_IN_RES_WR, TMC4361A_ENC_IN_RES_MASK, TMC4361A_ENC_IN_RES_SHIFT, config->enc_in_res); -} -TMC4361A::TMC4361AConfig_t *TMC4361A::createDeafultTMC4361AConfig(TMC4361APort *m_port) { - // base config - static TMC4361A::TMC4361AConfig_t _config; - TMC4361A::TMC4361AConfig_t *config = &_config; - memset(config, 0, sizeof(TMC4361A::TMC4361AConfig_t)); - - config->base_config.fs_per_rev = 200; - config->base_config.mstep_per_fs = 0; - - // encoder config - config->encoder_config.enable_encoder = true; - config->encoder_config.encoder_mode = TMC4361A::ENCODER_MODE_INCREMENTAL; - config->encoder_config.diff_enc_in_disable = false; - config->encoder_config.invert_enc_dir = false; - config->encoder_config.enc_in_res = 4000; - - // close loop config - config->close_loop_config.enable_closed_loop = true; - config->close_loop_config.gamma_v_min = 300000; - config->close_loop_config.gamma_v_add = 600000; - config->close_loop_config.gamma = 255; - config->close_loop_config.beta = 256; - config->close_loop_config.offset = 0; - config->close_loop_config.current_scaler_minimum = 100; - config->close_loop_config.current_scaler_maximum = 240; - config->close_loop_config.current_scaler_start_up = 0; - config->close_loop_config.upscale_delay = 1000; - config->close_loop_config.downscale_delay = 10000; - config->close_loop_config.correction_velocity_p = 10000; - config->close_loop_config.correction_velocity_i = 20; - config->close_loop_config.correction_velocity_i_clip = 10; - config->close_loop_config.correction_velocity_d_clk = 0; - config->close_loop_config.correction_velocity_d_clip = 1073741823; - config->close_loop_config.position_correction_p = 65536; - config->close_loop_config.position_correction_tolerance = 255; - config->close_loop_config.position_window = 255; - config->close_loop_config.enc_v_mean_wait = 0; - config->close_loop_config.enc_v_mean_filter = 7; - config->close_loop_config.enc_v_mean_int = 500; - config->close_loop_config.encoder_correction_y_offset = 0; - - config->autoReadEventRegister = true; - config->autoReadEventRegisterInterval = 33; - config->m_port = m_port; - - return config; -} -TMCDriverICInterface *TMC4361A::getDriverIC() { return m_driverIC; } -int32_t TMC4361A::readSubICVersion() { return m_driverIC->readICVersion(); } -void TMC4361A::TMCDriverICPort_setResetPinState(uint16_t __, bool state) { - // m_port->TMC4361APort_setResetPinState(channel, state); -} -void TMC4361A::TMCDriverICPort_setENNPinState(uint16_t __, bool state) { m_port->TMC4361APort_setSubICENNPinState(m_channel, state); } -void TMC4361A::TMCDriverICPort_sleepus(int32_t us) { m_port->TMC4361APort_sleepus(us); } -void TMC4361A::TMCDriverICPort_readWriteArray(uint16_t __, uint8_t *data, size_t length) { readWriteCover(data, 5); } -/******************************************************************************* - * 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); - if ((value & TMC4361A_TARGET_REACHED_F_MASK) > 0) { - return true; - } else { - return false; - } -} -void TMC4361A::periodicJob(uint32_t now) {} -#endif -#endif \ No newline at end of file diff --git a/components/tmc/ic/old/ztmc4361A.hpp b/components/tmc/ic/old/ztmc4361A.hpp deleted file mode 100644 index ea4fc44..0000000 --- a/components/tmc/ic/old/ztmc4361A.hpp +++ /dev/null @@ -1,194 +0,0 @@ -#pragma once -#if 0 - -#include -#include -#include -#include - -#include "../basic/tmc_ic_interface.hpp" -#include "sdk/hal/zhal.hpp" -#include "tmc_driver_ic.hpp" -#include "ztmc2160.hpp" -extern "C" { -#include "TMC2160\TMC2160.h" -#include "TMC4361A\TMC4361A.h" -} -namespace iflytop { -#define TMC4361A_LISTENER_MAX 5 -class TMC4361APort { - public: - /** - * @brief 复位引脚，低有效。 - * - * @param state - */ - virtual void TMC4361APort_setResetPinState(uint16_t channel, bool state) = 0; - /** - * @brief 制动引脚，拉低后设备制动，按照用户指定的斜坡进行制动。 - * 参考手册:TMC4361A_datasheet_rev1.26_01.pdf:127 - * @param state - */ - virtual void TMC4361APort_setFREEZEPinState(uint16_t channel, bool state) = 0; - /** - * @brief 使能引脚，低有效。 - * - * @param state - */ - virtual void TMC4361APort_setENNPinState(uint16_t channel, bool state) = 0; - - /** - * @brief 目标到达引脚，低有效。 - * - * @return true - * @return false - */ - virtual bool TMC4361APort_getTargetReachedPinState(uint16_t channel) = 0; - /** - * @brief us级别延时函数。 - * - * @param us - */ - virtual void TMC4361APort_sleepus(int32_t us) = 0; - /** - * @brief SPI读写接口 - * - * @param data - * @param length - */ - virtual void TMC4361APort_readWriteArray(uint16_t channel, uint8_t *data, size_t length) = 0; - - /********************************************************* - * 子芯片 * - *********************************************************/ - /** - * @brief 使能引脚，低有效。 - * - * @param state - */ - virtual void TMC4361APort_setSubICENNPinState(uint16_t channel, bool state) = 0; -}; - -class TMC4361A : public IStepperMotor { - public: - typedef enum { - IC_TMC2130 = 0, // - IC_TMC2160, - IC_TMC2660, - } driver_ic_type_t; - - public: - protected: - TMC2160 m_tmc2160; - - // TMC4361ATypeDef m_tmc4361A; - int32_t shadowRegister[TMC_REGISTER_COUNT]; - const uint8_t *m_registerAccessTable; - const int32_t *m_defaultRegisterResetState; - - TMCDriverICInterface *m_driverIC; - uint8_t m_channel; - driver_ic_type_t m_driver_ic_type; - TMC4361AConfig_t m_config; - uint32_t lastCallPeriodicJobTick; - // TMC4361APort *m_port; - uint8_t m_status; - - bool m_reachtarget; - - public: - TMC4361A(/* args */); - - /******************************************************************************* - * 初始化相关方法 * - *******************************************************************************/ - - /** - * @brief 静态方法，创建默认的TMC4361A配置参数,使用时只需修改自己需要的参数即可 - * - * 注意: - * 1. 该方法内部使用的是一个静态变量，所以每次调用该方法时，返回的都是同一个对象的地址。 - * 2. 该方法返回的数值不需要被释放。 - * @param config - */ - static TMC4361AConfig_t *createDeafultTMC4361AConfig(TMC4361APort *m_port); - - void initialize(uint8_t channel, driver_ic_type_t driver_ic_type, TMC4361AConfig_t *config); - void periodicJob(uint32_t ticket); - uint8_t getChannel() { return m_channel; } - void enableIC(bool enable); - /******************************************************************************* - * 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); // 设置最大加速度 - virtual void setDeceleration(float accelerationpps2); // 设置最大减速度 - - /******************************************************************************* - * 驱动器初始化方法 * - *******************************************************************************/ - /** - * @brief 初始化方法 - * - * @param channel SPI通道号 - * @param driver_ic_type 驱动芯片的类型 - */ - int32_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(); - TMCDriverICInterface *getDriverIC(); - - virtual void TMCDriverICPort_setResetPinState(uint16_t channel, bool state); - virtual void TMCDriverICPort_setENNPinState(uint16_t channel, bool state); - virtual void TMCDriverICPort_sleepus(int32_t us); - virtual void TMCDriverICPort_readWriteArray(uint16_t channel, uint8_t *data, size_t length); - - virtual bool isReachTarget(); - - public: - // only used in tmc4361A.cpp - void tmc4361AConfigCallback(ConfigState state); - void readWriteArray(uint8_t *data, size_t length); - - private: - uint8_t tmc4361A_calibrateClosedLoop(int &state, uint32_t &oldRamp, uint8_t worker0master1); - uint8_t tmc4361A_moveToNextFullstep(); - - void baseInit(base_config_t *baseconfig); - void close_loop_init(close_loop_config_t *config); - void encoder_init(encoder_config_t *config); - void calibrateClosed(); - uint32_t haspassedms(uint32_t now, uint32_t last); - // void callOnEventCallback(StepperMotorEvent event); -}; -} // namespace iflytop - -#endif diff --git a/components/tmc/ic/ztmc4361A.cpp b/components/tmc/ic/ztmc4361A.cpp index cbf9739..20fcc04 100644 --- a/components/tmc/ic/ztmc4361A.cpp +++ b/components/tmc/ic/ztmc4361A.cpp @@ -159,15 +159,22 @@ void TMC4361A::setDeceleration(float accelerationpps2) { int32_t acc = (int32_t)accelerationpps2; writeInt(TMC4361A_DMAX, acc << 2); } - +#define INIT_GPIO(m_pin, pin, ...) \ + if (pin != PinNull) { \ + m_pin = new ZGPIO(); \ + ZASSERT(m_pin != nullptr); \ + m_pin->initAsOutput(pin, __VA_ARGS__); \ + } else { \ + m_pin = nullptr; \ + } void TMC4361A::initialize(cfg_t *cfg) { - m_spi = cfg->spi; - m_csgpio = cfg->csgpio; - m_resetPin = cfg->resetPin; - m_fREEZEPin = cfg->fREEZEPin; - m_ennPin = cfg->ennPin; - m_driverIC_resetPin = cfg->driverIC_resetPin; - m_driverIC_ennPin = cfg->driverIC_ennPin; + m_spi = cfg->spi; + INIT_GPIO(m_csgpio, cfg->csgpio, ZGPIO::kMode_nopull, false, true); + INIT_GPIO(m_resetPin, cfg->resetPin, ZGPIO::kMode_nopull, false, true); + INIT_GPIO(m_fREEZEPin, cfg->fREEZEPin, ZGPIO::kMode_nopull, false, true); + INIT_GPIO(m_ennPin, cfg->ennPin, ZGPIO::kMode_nopull, false, true); + INIT_GPIO(m_driverIC_resetPin, cfg->driverIC_resetPin, ZGPIO::kMode_nopull, false, true); + INIT_GPIO(m_driverIC_ennPin, cfg->driverIC_ennPin, ZGPIO::kMode_nopull, false, true); m_driver_ic_type = IC_TMC2160; m_registerAccessTable = &tmc4361A_defaultRegisterAccess[0]; diff --git a/components/tmc/ic/ztmc4361A.hpp b/components/tmc/ic/ztmc4361A.hpp index 0a66a98..00e4863 100644 --- a/components/tmc/ic/ztmc4361A.hpp +++ b/components/tmc/ic/ztmc4361A.hpp @@ -25,17 +25,28 @@ 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; + 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; // + Pin_t csgpio = PinNull; // + Pin_t resetPin = PinNull; // + Pin_t fREEZEPin = PinNull; // + Pin_t ennPin = PinNull; // + Pin_t driverIC_ennPin = PinNull; // + Pin_t driverIC_resetPin = PinNull; // + }; typedef enum { kvelmode, @@ -44,22 +55,22 @@ class TMC4361A : public IStepperMotor { private: int32_t shadowRegister[TMC_REGISTER_COUNT]; - const uint8_t *m_registerAccessTable; - const int32_t *m_defaultRegisterResetState; + const uint8_t *m_registerAccessTable = nullptr; + const int32_t *m_defaultRegisterResetState = nullptr; driver_ic_type_t m_driver_ic_type; uint32_t m_lastCallPeriodicJobTick; uint8_t m_status; - SPI_HandleTypeDef *m_spi; + SPI_HandleTypeDef *m_spi = nullptr; - ZGPIO *m_csgpio; - ZGPIO *m_resetPin; - ZGPIO *m_fREEZEPin; - ZGPIO *m_ennPin; + ZGPIO *m_csgpio = nullptr; + ZGPIO *m_resetPin = nullptr; + ZGPIO *m_fREEZEPin = nullptr; + ZGPIO *m_ennPin = nullptr; - ZGPIO *m_driverIC_ennPin; - ZGPIO *m_driverIC_resetPin; + ZGPIO *m_driverIC_ennPin = nullptr; + ZGPIO *m_driverIC_resetPin = nullptr; motor_mode_t m_motor_mode = kvelmode;