diff --git a/components/tmc/ic/ztmc2160.cpp b/components/tmc/ic/old/ztmc2160.cpp similarity index 99% rename from components/tmc/ic/ztmc2160.cpp rename to components/tmc/ic/old/ztmc2160.cpp index b85ee4f..e17d002 100644 --- a/components/tmc/ic/ztmc2160.cpp +++ b/components/tmc/ic/old/ztmc2160.cpp @@ -1,3 +1,4 @@ +#if 0 #include "ztmc2160.hpp" #include "TMC2160/TMC2160.h" @@ -72,3 +73,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/ztmc2160.hpp b/components/tmc/ic/old/ztmc2160.hpp similarity index 98% rename from components/tmc/ic/ztmc2160.hpp rename to components/tmc/ic/old/ztmc2160.hpp index 37d2181..48225f2 100644 --- a/components/tmc/ic/ztmc2160.hpp +++ b/components/tmc/ic/old/ztmc2160.hpp @@ -1,3 +1,4 @@ +#if 0 #pragma once #include #include @@ -42,3 +43,4 @@ class TMC2160 : public TMCDriverICInterface { public: }; } // namespace iflytop +#endif diff --git a/components/tmc/ic/old/ztmc4361A.cpp b/components/tmc/ic/old/ztmc4361A.cpp new file mode 100644 index 0000000..91da210 --- /dev/null +++ b/components/tmc/ic/old/ztmc4361A.cpp @@ -0,0 +1,526 @@ +#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 new file mode 100644 index 0000000..ea4fc44 --- /dev/null +++ b/components/tmc/ic/old/ztmc4361A.hpp @@ -0,0 +1,194 @@ +#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 ae42e4a..6736d01 100644 --- a/components/tmc/ic/ztmc4361A.cpp +++ b/components/tmc/ic/ztmc4361A.cpp @@ -1,16 +1,27 @@ +#if 1 +#include "ztmc4361A.hpp" + #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) +#define SET_PIN(pin, val) \ + if (pin) { \ + pin->setState(val); \ + } + #if 1 -void TMC4361A::readWriteArray(uint8_t *data, size_t length) { m_config.m_port->TMC4361APort_readWriteArray(m_channel, data, length); } +void TMC4361A::readWriteArray(uint8_t *data, size_t length) { + m_csgpio->setState(false); + chip_delay_us(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) { CriticalContext cc; @@ -70,7 +81,7 @@ void TMC4361A::readWriteCover(uint8_t *data, size_t length) { 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); + chip_delay_us(10 * 1000); // Read the reply if (length > 4) { @@ -97,11 +108,9 @@ void TMC4361A::readWriteCover(uint8_t *data, size_t length) { } TMC4361A::TMC4361A(/* args */) { - m_channel = 0; - m_driver_ic_type = IC_TMC2130; - memset(&m_config, 0, sizeof(m_config)); - lastCallPeriodicJobTick = 0; - m_reachtarget = false; + m_driver_ic_type = IC_TMC2130; + m_lastCallPeriodicJobTick = 0; + m_reachtarget = false; } /** * @brief @@ -147,54 +156,44 @@ void TMC4361A::setDeceleration(float 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; +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_driver_ic_type = IC_TMC2160; 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); + SET_PIN(m_resetPin, true); + SET_PIN(m_fREEZEPin, true); + SET_PIN(m_ennPin, 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); + driverIC_reset(); 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); + chip_delay_us(300 * 1000); + chip_delay_us(300 * 1000); - getDriverIC()->setIHOLD_IRUN(1, 3, 0); // 注意要先设置IHOLD再设置IRUN,否则电机跑不起来 + driverIC_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); + SET_PIN(m_resetPin, false); + chip_delay_us(1000); + SET_PIN(m_resetPin, true); /** * @brief 重置芯片镜像寄存器 @@ -241,8 +240,8 @@ 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); + SET_PIN(m_ennPin, !enable); + SET_PIN(m_driverIC_ennPin, !enable); } int32_t tmc4361A_discardVelocityDecimals(int32_t value) { @@ -273,231 +272,7 @@ int32_t TMC4361A::readICVersion() { 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); } +int32_t TMC4361A::readSubICVersion() { return driverIC_readICVersion(); } /******************************************************************************* * 2160 function end * *******************************************************************************/ @@ -512,7 +287,6 @@ uint32_t TMC4361A::haspassedms(uint32_t now, uint32_t last) { return 0xFFFFFFFF - last + now; } } - bool TMC4361A::isReachTarget() { uint32_t value = readInt(TMC4361A_STATUS); if ((value & TMC4361A_TARGET_REACHED_F_MASK) > 0) { @@ -521,5 +295,61 @@ bool TMC4361A::isReachTarget() { return false; } } -void TMC4361A::periodicJob(uint32_t now) {} +/******************************************************************************* + * DRIVER_IC * + *******************************************************************************/ +#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() { + SET_PIN(m_driverIC_resetPin, false); + chip_delay_us(1000); + SET_PIN(m_driverIC_resetPin, 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); + driverIC_writeInt(index, tmc2160_defaultRegisterResetState[index]); + index++; + } + + driverIC_writeInt(TMC2160_PWMCONF, 0xC40C001E); + driverIC_writeInt(TMC2160_DRV_CONF, 0x00080400); +} + +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); +} +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) { + 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; + readWriteCover(&data[0], 5); + + data[0] = address; + readWriteCover(&data[0], 5); + + return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4]; +} +void TMC4361A::driverIC_setMotorShaft(bool reverse) { + // + int32_t val = reverse ? 1 : 0; + DRIVER_ID_FIELD_WRITE(TMC2160_GCONF, TMC2160_SHAFT_MASK, TMC2160_SHAFT_SHIFT, val); +} + #endif +#endif \ No newline at end of file diff --git a/components/tmc/ic/ztmc4361A.hpp b/components/tmc/ic/ztmc4361A.hpp index 4b32d47..4a2b87d 100644 --- a/components/tmc/ic/ztmc4361A.hpp +++ b/components/tmc/ic/ztmc4361A.hpp @@ -1,4 +1,6 @@ #pragma once +#if 1 + #include #include #include @@ -7,69 +9,13 @@ #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 TMCDriverICPort, // - public IStepperMotor { +class TMC4361A : public IStepperMotor { public: typedef enum { IC_TMC2130 = 0, // @@ -77,116 +23,36 @@ class TMC4361A : public TMCDriverICPort, // IC_TMC2660, } driver_ic_type_t; - typedef enum { - ktarget_reached = TMC4361A_TARGET_REACHED_MASK, - kpos_comp_reached = TMC4361A_POS_COMP_REACHED_MASK, - kvel_reached = TMC4361A_VEL_REACHED_MASK, - kvel_state_00 = TMC4361A_VEL_STATE_00_MASK, - kvel_state_01 = TMC4361A_VEL_STATE_01_MASK, - kvel_state_10 = TMC4361A_VEL_STATE_10_MASK, - kramp_state_00 = TMC4361A_RAMP_STATE_00_MASK, - kramp_state_01 = TMC4361A_RAMP_STATE_01_MASK, - kramp_state_10 = TMC4361A_RAMP_STATE_10_MASK, - kmax_phase_trap = TMC4361A_MAX_PHASE_TRAP_MASK, - kfrozen = TMC4361A_FROZEN_MASK, - kstopl_event = TMC4361A_STOPL_EVENT_MASK, - kstopr_event = TMC4361A_STOPR_EVENT_MASK, - kvstopl_active = TMC4361A_VSTOPL_ACTIVE_MASK, - khome_error = TMC4361A_HOME_ERROR_MASK, - kxlatch_done = TMC4361A_XLATCH_DONE_MASK, - kfs_active = TMC4361A_FS_ACTIVE_MASK, - kenc_fail = TMC4361A_ENC_FAIL_MASK, - kn_active = TMC4361A_N_ACTIVE_MASK, - kenc_done = TMC4361A_ENC_DONE_MASK, - kser_enc_data_fail = TMC4361A_SER_ENC_DATA_FAIL_MASK, - kser_data_done = TMC4361A_SER_DATA_DONE_MASK, - kserial_enc_flags = TMC4361A_SERIAL_ENC_FLAGS_MASK, - kcover_done = TMC4361A_COVER_DONE_MASK, - kenc_vel0 = TMC4361A_ENC_VEL0_MASK, - kcl_max = TMC4361A_CL_MAX_MASK, - kcl_fit = TMC4361A_CL_FIT_MASK, - kstop_on_stall = TMC4361A_STOP_ON_STALL_MASK, - kmotor_ev = TMC4361A_MOTOR_EV_MASK, - krst_ev = TMC4361A_RST_EV_MASK, - } event_mask_t; - - typedef enum { - ENCODER_MODE_INCREMENTAL = 0, - } encoder_mode_t; - - typedef struct { - uint32_t fs_per_rev; // 电机单圈分辨率，fs_per_rev = 360/步进角 - uint32_t mstep_per_fs; // 细分 0:256，1:128，2:64，3:32，4:16，5:8，6:4，7:2，8:1 - } base_config_t; - - /******************************************************************************* - * 编码器配置 * - *******************************************************************************/ - typedef struct { - bool enable_encoder; // 是否使能编码器 - encoder_mode_t encoder_mode; // 编码器配置: 编码器模式,ABN/SSI/SPI - bool diff_enc_in_disable; // 编码器配置: 是否失能编码器差分模式 - bool invert_enc_dir; // 编码器配置: 编码器方向是否反转 - uint32_t enc_in_res; // 编码器配置: 编码器分辨率，一般为编码器分辨率的4倍，例如1000线分辨率的编码器，这里填4000 - } encoder_config_t; - - /** - * @brief 闭环电机配置参数1 - * - * 该配置主要参考libtrinamic\IFLYTOP-TMC-API\tmc\ic\TMC43xx\TMC43xx.c - */ typedef struct { - bool enable_closed_loop; // 是否使能闭环，如果不使能闭环，电机将以开环模式运行，此时可以查看编码器是否配置正确 - - uint32_t gamma_v_min; - uint32_t gamma_v_add; - uint8_t gamma; - uint16_t beta; - uint32_t offset; - uint8_t current_scaler_minimum; - uint8_t current_scaler_maximum; - uint8_t current_scaler_start_up; - uint32_t upscale_delay; - uint32_t downscale_delay; - uint32_t correction_velocity_p; - uint32_t correction_velocity_i; - uint32_t correction_velocity_i_clip; - uint32_t correction_velocity_d_clk; - uint32_t correction_velocity_d_clip; - uint32_t position_correction_p; - uint32_t position_correction_tolerance; - uint32_t position_window; - uint8_t enc_v_mean_wait; - uint8_t enc_v_mean_filter; - uint32_t enc_v_mean_int; - int8_t encoder_correction_y_offset; - } close_loop_config_t; + SPI_HandleTypeDef *spi; - typedef struct { - base_config_t base_config; // 基础配置 - encoder_config_t encoder_config; // 编码器配置 - close_loop_config_t close_loop_config; // 闭环配置 - bool autoReadEventRegister; // 是否自动读取事件寄存器 - uint32_t autoReadEventRegisterInterval; // 自动读取事件寄存器的时间间隔，单位为ms - TMC4361APort *m_port; - } TMC4361AConfig_t; + STM32_GPIO *csgpio; + STM32_GPIO *resetPin; + STM32_GPIO *fREEZEPin; + STM32_GPIO *ennPin; - public: - protected: - TMC2160 m_tmc2160; + STM32_GPIO *driverIC_ennPin; + STM32_GPIO *driverIC_resetPin; + } cfg_t; - // TMC4361ATypeDef m_tmc4361A; + private: 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; + driver_ic_type_t m_driver_ic_type; + uint32_t m_lastCallPeriodicJobTick; + uint8_t m_status; + + SPI_HandleTypeDef *m_spi; + + STM32_GPIO *m_csgpio; + STM32_GPIO *m_resetPin; + STM32_GPIO *m_fREEZEPin; + STM32_GPIO *m_ennPin; + + STM32_GPIO *m_driverIC_ennPin; + STM32_GPIO *m_driverIC_resetPin; bool m_reachtarget; @@ -205,12 +71,9 @@ class TMC4361A : public TMCDriverICPort, // * 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); + void initialize(cfg_t *cfg); + void enableIC(bool enable); /******************************************************************************* * IStepperMotor impl * *******************************************************************************/ @@ -257,14 +120,7 @@ class TMC4361A : public TMCDriverICPort, // /******************************************************* * 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); - + int32_t readSubICVersion(); virtual bool isReachTarget(); public: @@ -273,14 +129,18 @@ class TMC4361A : public TMCDriverICPort, // 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); + + void driverIC_reset(); + void driverIC_enableIC(bool enable); + void driverIC_setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay); + uint32_t driverIC_readICVersion(); + void driverIC_writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4); + void driverIC_writeInt(uint8_t address, int32_t value); + int32_t driverIC_readInt(uint8_t address); + void driverIC_setMotorShaft(bool reverse); }; -} // namespace iflytop \ No newline at end of file +} // namespace iflytop + +#endif