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.
206 lines
7.4 KiB
206 lines
7.4 KiB
#include "ztmc5130.hpp"
|
|
|
|
#include "sdk/os/zos.hpp"
|
|
|
|
#ifdef HAL_SPI_MODULE_ENABLED
|
|
|
|
using namespace iflytop;
|
|
|
|
/**
|
|
* @brief 静态方法，创建默认的TMC5130配置参数,使用时只需修改自己需要的参数即可
|
|
*
|
|
* 注意:
|
|
* 1. 该方法内部使用的是一个静态变量，所以每次调用该方法时，返回的都是同一个对象的地址??
|
|
* 2. 该方法返回的数值不需要被释放??
|
|
* @param config
|
|
*/
|
|
#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); \
|
|
}
|
|
TMC5130::TMC5130(/* args */) {}
|
|
void TMC5130::initialize(cfg_t *cfg) {
|
|
m_cfg = *cfg;
|
|
|
|
// m_channel = channel;
|
|
// m_config = config;
|
|
// m_port = config->m_port;
|
|
m_registerAccessTable = &tmc5130_defaultRegisterAccess[0];
|
|
m_defaultRegisterResetState = &tmc5130_defaultRegisterResetState[0];
|
|
if (cfg->csn_pin != PinNull) {
|
|
m_csnpin = new ZGPIO();
|
|
m_csnpin->initAsOutput(cfg->csn_pin, ZGPIO::kMode_nopull, false, true);
|
|
ZASSERT(m_csnpin);
|
|
}
|
|
if (cfg->enn_pin != PinNull) {
|
|
m_ennpin = new ZGPIO();
|
|
m_ennpin->initAsOutput(cfg->enn_pin, ZGPIO::kMode_nopull, false, true);
|
|
ZASSERT(m_ennpin);
|
|
}
|
|
m_hspi = cfg->hspi;
|
|
|
|
enableIC(false);
|
|
// tmc5130_init(&m_TMC5130, channel, &m_tmc_api_config, &tmc5130_defaultRegisterResetState[0]);
|
|
// tmc5130_setCallback(&m_TMC5130, pri_tmc4361A_callback);
|
|
|
|
reset();
|
|
|
|
writeInt(TMC5130_PWMCONF, 0x000500C8);
|
|
// writeInt( TMC5130_GCONF, 0x00000004);
|
|
writeInt(TMC5130_CHOPCONF, 0x000100c3);
|
|
writeInt(TMC5130_IHOLD_IRUN, 0x00051A00);
|
|
writeInt(TMC5130_PWMCONF, 0x000401c8);
|
|
|
|
writeInt(TMC5130_XTARGET, 0);
|
|
writeInt(TMC5130_XACTUAL, 0x00000000);
|
|
writeInt(TMC5130_VACTUAL, 0x00000000);
|
|
|
|
writeInt(TMC5130_VSTART, 5);
|
|
writeInt(TMC5130_A1, 1000);
|
|
writeInt(TMC5130_V1, 0);
|
|
|
|
writeInt(TMC5130_D1, 1000);
|
|
writeInt(TMC5130_VSTOP, 10);
|
|
writeInt(TMC5130_TZEROWAIT, 1000);
|
|
|
|
setAcceleration(100000);
|
|
setDeceleration(100000);
|
|
setIHOLD_IRUN(2, 10, 1);
|
|
enableIC(true);
|
|
}
|
|
|
|
void TMC5130::enableIC(bool enable) {
|
|
// m_port->TMC5130Port_setENNPinState(m_channel, !enable);
|
|
SET_PIN(m_ennpin, !enable);
|
|
}
|
|
uint8_t TMC5130::reset() {
|
|
stop();
|
|
// m_port->TMC5130Port_setResetNPinState(m_channel, false);
|
|
SET_PIN(m_csnpin, false);
|
|
// m_port->TMC5130Port_sleepus(1000);
|
|
zchip_clock_early_delayus(1000);
|
|
// m_port->TMC5130Port_setResetNPinState(m_channel, true);
|
|
SET_PIN(m_csnpin, true);
|
|
|
|
for (uint32_t add = 0; add < TMC5130_REGISTER_COUNT; add++) {
|
|
if (!TMC_IS_RESETTABLE(m_registerAccessTable[add])) {
|
|
continue;
|
|
}
|
|
writeInt(add, m_defaultRegisterResetState[add]);
|
|
}
|
|
return 0;
|
|
}
|
|
int32_t TMC5130::getXACTUAL() { return readInt(TMC5130_XACTUAL) / m_scale; }
|
|
void TMC5130::setXACTUAL(int32_t value) { writeInt(TMC5130_XACTUAL, value * m_scale); }
|
|
int32_t TMC5130::getVACTUAL() { return readInt(TMC5130_VACTUAL) / m_scale; }
|
|
void TMC5130::setAcceleration(float accelerationpps2) { writeInt(TMC5130_AMAX, (int32_t)(accelerationpps2 * m_scale)); } // 设置最大加速度
|
|
void TMC5130::setDeceleration(float accelerationpps2) { writeInt(TMC5130_DMAX, (int32_t)(accelerationpps2 * m_scale)); } // 设置最大减速度
|
|
void TMC5130::setMotorShaft(bool reverse) { PRV_FIELD_WRITE(TMC5130_GCONF, TMC5130_SHAFT_MASK, TMC5130_SHAFT_SHIFT, reverse); }
|
|
void TMC5130::setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) {
|
|
writeInt(TMC5130_IHOLD_IRUN, (iholddelay << TMC5130_IHOLDDELAY_SHIFT) | (irun << TMC5130_IRUN_SHIFT) | (ihold << TMC5130_IHOLD_SHIFT));
|
|
}
|
|
uint32_t TMC5130::readChipVERSION() {
|
|
uint32_t chipID = PRV_FIELD_READ(TMC5130_IOIN, TMC5130_VERSION_MASK, TMC5130_VERSION_SHIFT);
|
|
return chipID;
|
|
}
|
|
uint32_t TMC5130::getTMC5130_RAMPSTAT() { return readInt(TMC5130_RAMPSTAT); }
|
|
Tmc5130RampStat TMC5130::getTMC5130_RAMPSTAT2() {
|
|
uint32_t value = getTMC5130_RAMPSTAT();
|
|
return Tmc5130RampStat(value);
|
|
}
|
|
void TMC5130::stop() { rotate(0); }
|
|
void TMC5130::rotate(int32_t velocity) {
|
|
velocity *= m_scale;
|
|
writeInt(TMC5130_VMAX, abs(velocity));
|
|
writeInt(TMC5130_RAMPMODE, (velocity >= 0) ? TMC5130_MODE_VELPOS : TMC5130_MODE_VELNEG);
|
|
}
|
|
void TMC5130::right(int32_t velocity) { rotate(velocity); }
|
|
void TMC5130::left(int32_t velocity) { rotate(-velocity); }
|
|
void TMC5130::moveTo(int32_t position, uint32_t velocityMax) {
|
|
position *= m_scale;
|
|
velocityMax *= m_scale;
|
|
writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
|
|
writeInt(TMC5130_VMAX, velocityMax);
|
|
writeInt(TMC5130_XTARGET, position);
|
|
}
|
|
void TMC5130::moveBy(int32_t relativePosition, uint32_t velocityMax) { // determine actual position and add numbers of ticks to move
|
|
relativePosition += readInt(TMC5130_XACTUAL);
|
|
moveTo(relativePosition, velocityMax);
|
|
}
|
|
uint32_t TMC5130::readXTARGET() { return readInt(TMC5130_XTARGET); }
|
|
uint32_t TMC5130::haspassedms(uint32_t now, uint32_t last) {
|
|
if (now >= last) {
|
|
return now - last;
|
|
} else {
|
|
return 0xFFFFFFFF - last + now;
|
|
}
|
|
}
|
|
bool TMC5130::isReachTarget() {
|
|
/**
|
|
* @brief
|
|
*/
|
|
int mode = readInt(TMC5130_RAMPMODE);
|
|
if (mode == TMC5130_MODE_POSITION) {
|
|
uint32_t state = getTMC5130_RAMPSTAT();
|
|
Tmc5130RampStat event = Tmc5130RampStat(state);
|
|
return event.isSetted(Tmc5130RampStat::ktmc5130_rs_posreached);
|
|
} else {
|
|
uint32_t state = getTMC5130_RAMPSTAT();
|
|
Tmc5130RampStat event = Tmc5130RampStat(state);
|
|
return event.isSetted(Tmc5130RampStat::ktmc5130_rs_vzero) && event.isSetted(Tmc5130RampStat::ktmc5130_rs_velreached);
|
|
}
|
|
}
|
|
|
|
void TMC5130::setScale(float scale) { m_scale = scale; }
|
|
|
|
/*******************************************************************************
|
|
* basic *
|
|
*******************************************************************************/
|
|
// void TMC5130::writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value) {
|
|
// CriticalContext cc;
|
|
// writeInt(address, readInt(address) & ~mask | value << shift);
|
|
// }
|
|
void TMC5130::readWriteArray(uint8_t *data, size_t length) {
|
|
CriticalContext cc;
|
|
// m_port->TMC5130Port_readWriteArray(m_channel, data, length);
|
|
// m_csnpin
|
|
SET_PIN(m_csnpin, false);
|
|
HAL_SPI_TransmitReceive(m_hspi, data, data, length, 100);
|
|
SET_PIN(m_csnpin, true);
|
|
}
|
|
void TMC5130::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) {
|
|
CriticalContext cc;
|
|
uint8_t data[5] = {static_cast<uint8_t>(address | static_cast<uint8_t>(TMC5130_WRITE_BIT)), x1, x2, x3, x4};
|
|
readWriteArray(&data[0], 5);
|
|
|
|
int32_t 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);
|
|
m_shadowRegister[address] = value;
|
|
}
|
|
void TMC5130::writeInt(uint8_t address, int32_t value) {
|
|
CriticalContext cc;
|
|
writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0));
|
|
}
|
|
int32_t TMC5130::readInt(uint8_t address) {
|
|
CriticalContext cc;
|
|
address = TMC_ADDRESS(address);
|
|
|
|
// register not readable -> shadow register copy
|
|
if (!TMC_IS_READABLE(tmc5130_defaultRegisterAccess[address])) return m_shadowRegister[address];
|
|
|
|
uint8_t data[5] = {0, 0, 0, 0, 0};
|
|
|
|
data[0] = address;
|
|
readWriteArray(&data[0], 5);
|
|
|
|
data[0] = address;
|
|
readWriteArray(&data[0], 5);
|
|
|
|
return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4];
|
|
}
|
|
#endif
|