8 changed files with 980 additions and 566 deletions
-
2components/tmc/ic/old/ztmc2160.cpp
-
2components/tmc/ic/old/ztmc2160.hpp
-
526components/tmc/ic/old/ztmc4361A.cpp
-
194components/tmc/ic/old/ztmc4361A.hpp
-
366components/tmc/ic/ztmc4361A.cpp
-
220components/tmc/ic/ztmc4361A.hpp
@ -0,0 +1,526 @@ |
|||
#if 0
|
|||
#include <stdarg.h>
|
|||
|
|||
#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<uint8_t>(address | static_cast<uint8_t>(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
|
|||
@ -0,0 +1,194 @@ |
|||
#pragma once
|
|||
#if 0
|
|||
|
|||
#include <stdbool.h>
|
|||
#include <stdint.h>
|
|||
#include <stdio.h>
|
|||
#include <stdlib.h>
|
|||
|
|||
#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
|
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue