You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
419 lines
14 KiB
419 lines
14 KiB
#if 1
|
|
#include "ztmc4361A.hpp"
|
|
|
|
#ifdef HAL_SPI_MODULE_ENABLED
|
|
|
|
#include <stdarg.h>
|
|
|
|
#include "../basic/basic.hpp"
|
|
#include "./TMC4361A/TMC4361A.h"
|
|
#include "sdk/os/zos.hpp"
|
|
|
|
using namespace iflytop;
|
|
#define PRV_FIELD_WRITE(address, mask, shift, value) (writeInt(address, FIELD_SET(readInt(address), mask, shift, value)))
|
|
#define PRV_FIELD_READ(address, mask, shift) FIELD_GET(readInt(address), mask, shift)
|
|
|
|
#define SET_PIN(pin, val) \
|
|
if (pin) { \
|
|
pin->setState(val); \
|
|
}
|
|
|
|
#if 1
|
|
void TMC4361A::readWriteArray(uint8_t *data, size_t length) {
|
|
m_csgpio->setState(false);
|
|
// zchip_clock_early_delayus(10);
|
|
zchip_clock_early_delayus(10);
|
|
HAL_SPI_TransmitReceive(m_spi, data, data, length, 1000);
|
|
m_csgpio->setState(true);
|
|
}
|
|
void TMC4361A::writeInt(uint8_t address, int32_t value) { writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0)); }
|
|
int32_t TMC4361A::readInt(uint8_t address) {
|
|
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]);
|
|
|
|
zos_delay(10);
|
|
|
|
// 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_driver_ic_type = IC_TMC2130;
|
|
m_lastCallPeriodicJobTick = 0;
|
|
// m_reachtarget = false;
|
|
}
|
|
/**
|
|
* @brief
|
|
* 当调用TMC-API中的tmc4361A_reset/restore时候，TMC-API中的方法会初始化整个芯片的寄存器
|
|
* 当寄存器初始化完成之后，会调用这个方法??
|
|
* 我们可以在这个方法中，对芯片的部分寄存器进行初始化??
|
|
* @param state
|
|
*/
|
|
void TMC4361A::tmc4361AConfigCallback(ConfigState state) {}
|
|
|
|
void TMC4361A::writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value) { //
|
|
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
|
|
*/
|
|
|
|
accelerationpps2 = to_motor_acc(accelerationpps2);
|
|
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
|
|
*/
|
|
accelerationpps2 = to_motor_acc(accelerationpps2);
|
|
int32_t acc = (int32_t)accelerationpps2;
|
|
writeInt(TMC4361A_DMAX, acc << 2);
|
|
}
|
|
#define INIT_GPIO(m_pin, pin, ...) \
|
|
if (pin != PinNull) { \
|
|
m_pin = new ZGPIO(); \
|
|
ZASSERT(m_pin != nullptr); \
|
|
m_pin->initAsOutput(pin, __VA_ARGS__); \
|
|
} else { \
|
|
m_pin = nullptr; \
|
|
}
|
|
void TMC4361A::initialize(cfg_t *cfg) {
|
|
m_spi = cfg->spi;
|
|
INIT_GPIO(m_csgpio, cfg->csgpio, ZGPIO::kMode_nopull, false, true);
|
|
INIT_GPIO(m_resetPin, cfg->resetPin, ZGPIO::kMode_nopull, false, true);
|
|
INIT_GPIO(m_fREEZEPin, cfg->fREEZEPin, ZGPIO::kMode_nopull, false, true);
|
|
INIT_GPIO(m_ennPin, cfg->ennPin, ZGPIO::kMode_nopull, false, true);
|
|
INIT_GPIO(m_driverIC_resetPin, cfg->driverIC_resetPin, ZGPIO::kMode_nopull, false, true);
|
|
INIT_GPIO(m_driverIC_ennPin, cfg->driverIC_ennPin, ZGPIO::kMode_nopull, false, true);
|
|
|
|
m_driver_ic_type = IC_TMC2160;
|
|
m_registerAccessTable = &tmc4361A_defaultRegisterAccess[0];
|
|
m_defaultRegisterResetState = &tmc4361A_defaultRegisterResetState[0];
|
|
|
|
memset(shadowRegister, 0, sizeof(shadowRegister));
|
|
|
|
SET_PIN(m_resetPin, true);
|
|
SET_PIN(m_fREEZEPin, true);
|
|
SET_PIN(m_ennPin, true);
|
|
|
|
reset();
|
|
|
|
driverIC_reset();
|
|
|
|
setAcceleration(500000);
|
|
setDeceleration(500000);
|
|
|
|
enableIC(true);
|
|
zchip_clock_early_delayus(300 * 1000);
|
|
zchip_clock_early_delayus(300 * 1000);
|
|
|
|
driverIC_setIHOLD_IRUN(1, 3, 0);
|
|
}
|
|
|
|
void TMC4361A::setScale(int32_t scale) { m_scale = scale; }
|
|
|
|
uint8_t TMC4361A::reset() {
|
|
// Pulse the low-active hardware reset pin
|
|
stop();
|
|
SET_PIN(m_resetPin, false);
|
|
zchip_clock_early_delayus(1000);
|
|
SET_PIN(m_resetPin, 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 to_user_pos(readInt(TMC4361A_XACTUAL)); }
|
|
void TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, to_motor_pos(value)); }
|
|
int32_t TMC4361A::getVACTUAL() { return to_user_pos(readInt(TMC4361A_VACTUAL)); }
|
|
int32_t TMC4361A::getXTARGET() { return to_user_pos(readInt(TMC4361A_X_TARGET)); }
|
|
int32_t TMC4361A::getENC_POS() { return to_user_pos(readInt(TMC4361A_ENC_POS)); }
|
|
void TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, to_motor_pos(value)); }
|
|
int32_t TMC4361A::getENC_POS_DEV() { return to_user_pos(readInt(TMC4361A_ENC_POS_DEV_RD)); }
|
|
void TMC4361A::enableIC(bool enable) {
|
|
SET_PIN(m_ennPin, !enable);
|
|
SET_PIN(m_driverIC_ennPin, !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) {
|
|
// velocity *= m_scale;
|
|
velocity = to_motor_vel(velocity);
|
|
m_motor_mode = kvelmode;
|
|
PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 0);
|
|
writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocity));
|
|
}
|
|
void TMC4361A::stop() { rotate(0); }
|
|
void TMC4361A::moveTo(int32_t position, uint32_t velocityMax) {
|
|
// position *= m_scale;
|
|
// velocityMax *= m_scale;
|
|
|
|
position = to_motor_pos(position);
|
|
velocityMax = to_motor_vel(velocityMax);
|
|
|
|
m_motor_mode = kposmode;
|
|
PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 1);
|
|
writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocityMax));
|
|
writeInt(TMC4361A_X_TARGET, position);
|
|
}
|
|
void TMC4361A::moveBy(int32_t relativePosition, uint32_t velocityMax) {
|
|
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;
|
|
}
|
|
|
|
int32_t TMC4361A::readSubICVersion() { return driverIC_readICVersion(); }
|
|
/*******************************************************************************
|
|
* 2160 function end *
|
|
*******************************************************************************/
|
|
uint32_t TMC4361A::readEVENTS() {
|
|
uint32_t value = readInt(TMC4361A_EVENTS);
|
|
return value;
|
|
}
|
|
uint32_t TMC4361A::haspassedms(uint32_t now, uint32_t last) {
|
|
if (now >= last) {
|
|
return now - last;
|
|
} else {
|
|
return 0xFFFFFFFF - last + now;
|
|
}
|
|
}
|
|
bool TMC4361A::isReachTarget() {
|
|
uint32_t value = readInt(TMC4361A_STATUS);
|
|
// printf("TMC4361A_STATUS:%08x\n", value);
|
|
if (m_motor_mode == kposmode) {
|
|
return (value & TMC4361A_TARGET_REACHED_F_MASK) > 0;
|
|
} else {
|
|
return (value & TMC4361A_VEL_REACHED_F_MASK) && (readInt(TMC4361A_VMAX) == 0);
|
|
}
|
|
}
|
|
/*******************************************************************************
|
|
* 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);
|
|
zchip_clock_early_delayus(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<uint8_t>(address | static_cast<uint8_t>(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);
|
|
}
|
|
|
|
uint32_t TMC4361A::driverIC_readICVersion() {
|
|
int32_t value = driverIC_readInt(TMC2160_IOIN___OUTPUT);
|
|
return (value & TMC2160_VERSION_MASK) >> TMC2160_VERSION_SHIFT;
|
|
}
|
|
void TMC4361A::driverIC_setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_writeInt(TMC2160_IHOLD_IRUN, (iholddelay << TMC2160_IHOLDDELAY_SHIFT) | (irun << TMC2160_IRUN_SHIFT) | (ihold << TMC2160_IHOLD_SHIFT)); }
|
|
|
|
// Left Virtual Limit Switch XACTUAL ≤ VIRT_STOP_LEFT 时触发
|
|
void TMC4361A::setLeftVirtualLimitSwitch(bool enable, int32_t position) {
|
|
PRV_FIELD_WRITE(TMC4361A_REFERENCE_CONF, TMC4361A_VIRTUAL_LEFT_LIMIT_EN_MASK, TMC4361A_VIRTUAL_LEFT_LIMIT_EN_SHIFT, enable ? 1 : 0);
|
|
writeInt(TMC4361A_VIRT_STOP_LEFT, position);
|
|
}
|
|
|
|
// Right Virtual Limit Switch XACTUAL ≥ VIRT_STOP_RIGHT 时触发
|
|
void TMC4361A::setRightVirtualLimitSwitch(bool enable, int32_t position) {
|
|
PRV_FIELD_WRITE(TMC4361A_REFERENCE_CONF, TMC4361A_VIRTUAL_RIGHT_LIMIT_EN_MASK, TMC4361A_VIRTUAL_RIGHT_LIMIT_EN_SHIFT, enable ? 1 : 0);
|
|
writeInt(TMC4361A_VIRT_STOP_RIGHT, position);
|
|
}
|
|
|
|
int32_t TMC4361A::to_motor_acc(int32_t acc) { //
|
|
int32_t val = acc / 60.0 * 51200;
|
|
return val;
|
|
}
|
|
int32_t TMC4361A::to_motor_vel(int32_t vel) { //
|
|
int32_t val = vel / 60.0 * 51200;
|
|
return val;
|
|
} // rpm
|
|
int32_t TMC4361A::to_motor_pos(int32_t pos) { //
|
|
int32_t val = pos * 1.0 / m_scale * 51200.0;
|
|
return val;
|
|
} //
|
|
|
|
int32_t TMC4361A::to_user_pos(int32_t pos) { //
|
|
int32_t val = pos / 51200.0 * m_scale;
|
|
return val;
|
|
} //
|
|
int32_t TMC4361A::to_user_vel(int32_t vel) { //
|
|
int32_t val = vel * 60.0 / 51200.0;
|
|
return val;
|
|
}
|
|
|
|
#endif
|
|
#endif
|
|
|
|
#endif
|