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