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.
437 lines
15 KiB
437 lines
15 KiB
#include "tmc51x0.hpp"
|
|
|
|
#include "reg/TMC5130_Constants.h"
|
|
#include "reg/TMC5130_Fields.h"
|
|
#include "reg/TMC5130_Register.h"
|
|
//
|
|
|
|
using namespace iflytop;
|
|
|
|
#if 0
|
|
// Register access permissions:
|
|
// 0x00: none (reserved)
|
|
// 0x01: read
|
|
// 0x02: write
|
|
// 0x03: read/write
|
|
// 0x13: read/write, separate functions/values for reading or writing
|
|
// 0x21: read, flag register (read to clear)
|
|
// 0x42: write, has hardware presets on reset
|
|
static const uint8_t tmc5130_defaultRegisterAccess[TMC5130_REGISTER_COUNT] = {
|
|
// 0 1 2 3 4 5 6 7 8 9 A B C D E F
|
|
0x03, 0x21, 0x01, 0x02, 0x13, 0x02, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, // 0x00 - 0x0F
|
|
0x02, 0x02, 0x01, 0x02, 0x02, 0x02, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, // 0x10 - 0x1F
|
|
0x03, 0x03, 0x01, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, ____, 0x02, 0x02, 0x02, 0x03, ____, ____, // 0x20 - 0x2F
|
|
____, ____, ____, 0x02, 0x03, 0x21, 0x01, ____, 0x03, 0x03, 0x02, 0x21, 0x01, ____, ____, ____, // 0x30 - 0x3F
|
|
____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, // 0x40 - 0x4F
|
|
____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, // 0x50 - 0x5F
|
|
0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x01, 0x01, 0x03, 0x02, 0x02, 0x01, // 0x60 - 0x6F
|
|
0x42, 0x01, 0x02, 0x01, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____ // 0x70 - 0x7F
|
|
};
|
|
#endif
|
|
|
|
/***********************************************************************************************************************
|
|
* 工具宏 *
|
|
***********************************************************************************************************************/
|
|
|
|
#define FIELD_SET(data, mask, shift, value) (data = (((data) & (~(mask))) | (((value) << (shift)) & (mask))))
|
|
#define FIELD_GET(data, mask, shift) (((data) & (mask)) >> (shift))
|
|
#define BYTE(value, n) (((value) >> ((n) << 3)) & 0xFF)
|
|
#define NIBBLE(value, n) (((value) >> ((n) << 2)) & 0x0F)
|
|
#define SHORT(value, n) (((value) >> ((n) << 4)) & 0xFFFF)
|
|
#define WORD(value, n) (((value) >> ((n) << 5)) & 0xFFFFFFFF)
|
|
|
|
#define TMC51x0_ADDRESS(x) ((x) & (TMC5130_ADDRESS_MASK))
|
|
#define TAG "TMC51X0"
|
|
void TMC51X0::initialize(TMC51X0Cfg cfg) {
|
|
m_cfg = cfg;
|
|
m_hspi = cfg.hspi;
|
|
|
|
ZLOGI(TAG, "SPI:%p CSPin:%s,enPIn:%s", cfg.hspi, pinname(cfg.csnpin), pinname(cfg.ennpin));
|
|
m_mutex.init();
|
|
|
|
m_csnpin.initAsOutput(m_cfg.csnpin, kxs_gpio_nopull, false, true);
|
|
m_ennpin.initAsOutput(m_cfg.ennpin, kxs_gpio_nopull, false, true);
|
|
|
|
enable(false);
|
|
|
|
stop();
|
|
|
|
writeInt(TMC5130_PWMCONF, 0x000500C8);
|
|
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_TZEROWAIT, 0);
|
|
|
|
setVstart(100);
|
|
setA1(30);
|
|
setAmax(50);
|
|
setV1(500);
|
|
setDmax(50);
|
|
setD1(30);
|
|
setVstop(100);
|
|
setTzerowait(100);
|
|
|
|
setIHOLD_IRUN(0, 30, 100);
|
|
enable(false);
|
|
}
|
|
|
|
/*******************************************************************************
|
|
* 驱动器寄存器读写方法 *
|
|
*******************************************************************************/
|
|
|
|
void TMC51X0::readWriteArray(uint8_t *data, size_t length) {
|
|
portENTER_CRITICAL();
|
|
m_csnpin.write(false);
|
|
HAL_SPI_TransmitReceive(m_hspi, data, data, length, 100);
|
|
m_csnpin.write(true);
|
|
portEXIT_CRITICAL();
|
|
}
|
|
void TMC51X0::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>(TMC5130_WRITE_BIT)), x1, x2, x3, x4};
|
|
readWriteArray(&data[0], 5);
|
|
}
|
|
void TMC51X0::writeInt(uint8_t address, int32_t value) { //
|
|
writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0));
|
|
}
|
|
|
|
void TMC51X0::writeField(uint8_t add, uint32_t mask, uint32_t shift, uint32_t value) {
|
|
uint32_t regval = readUInt(add);
|
|
regval = (regval & ~mask) | ((value << shift) & mask);
|
|
writeInt(add, regval);
|
|
}
|
|
|
|
int32_t TMC51X0::readInt(uint8_t address) {
|
|
/**
|
|
* @WARNING:
|
|
* 这里没有判断寄存器是否可读，有些寄存器是不可读所以，所以可能读回来的数值和写入数值是不相等的
|
|
*/
|
|
address = TMC51x0_ADDRESS(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];
|
|
}
|
|
|
|
uint32_t TMC51X0::readUInt(uint8_t address) {
|
|
int32_t val = readInt(address);
|
|
uint32_t ret;
|
|
memcpy(&ret, &val, sizeof(uint32_t));
|
|
return ret;
|
|
}
|
|
|
|
/***********************************************************************************************************************
|
|
* BASIC *
|
|
***********************************************************************************************************************/
|
|
|
|
bool TMC51X0::ping() {
|
|
zlock_guard lkg(m_mutex);
|
|
uint32_t regval = readUInt(TMC5130_IOIN);
|
|
uint32_t chipId = FIELD_GET(regval, TMC5130_VERSION_MASK, TMC5130_VERSION_SHIFT);
|
|
return chipId != 0;
|
|
}
|
|
void TMC51X0::enable(bool enable) {
|
|
zlock_guard lkg(m_mutex);
|
|
|
|
m_ennpin.write(!enable);
|
|
}
|
|
|
|
int32_t TMC51X0::to_motor_acc(int32_t acc) { //
|
|
int32_t val = acc / 60.0 * m_onecirclepulse; // 65535
|
|
if (val > 65535) val = 65535;
|
|
return val;
|
|
}
|
|
int32_t TMC51X0::to_motor_vel(int32_t vel) { //
|
|
int32_t val = vel / 60.0 * m_onecirclepulse;
|
|
if (val > 8388095 /*2^23-512*/) {
|
|
val = 8388095;
|
|
}
|
|
return val;
|
|
} // rpm
|
|
int32_t TMC51X0::to_motor_pos(int32_t pos) { //
|
|
int32_t val = pos * 1.0 / m_scale * m_scale_deceleration * m_onecirclepulse + 0.5;
|
|
return val;
|
|
} //
|
|
|
|
int32_t TMC51X0::to_user_pos(int32_t pos) { //
|
|
int32_t val = pos / m_onecirclepulse * m_scale / m_scale_deceleration + 0.5;
|
|
// ZLOGI("TMC5130", "to_user_pos %d,%d", pos, val);
|
|
return val;
|
|
} //
|
|
int32_t TMC51X0::to_user_vel(int32_t vel) { //
|
|
int32_t val = vel * 60.0 / m_onecirclepulse;
|
|
return val;
|
|
}
|
|
/***********************************************************************************************************************
|
|
* CTRL *
|
|
***********************************************************************************************************************/
|
|
void TMC51X0::stop() {
|
|
zlock_guard lkg(m_mutex);
|
|
|
|
rotate(0);
|
|
}
|
|
void TMC51X0::rotate(int32_t velocity) {
|
|
zlock_guard lkg(m_mutex);
|
|
|
|
velocity = to_motor_vel(velocity);
|
|
writeInt(TMC5130_VMAX, abs(velocity));
|
|
writeInt(TMC5130_RAMPMODE, (velocity >= 0) ? TMC5130_MODE_VELPOS : TMC5130_MODE_VELNEG);
|
|
}
|
|
void TMC51X0::moveTo(int32_t position, uint32_t velocityMax) {
|
|
zlock_guard lkg(m_mutex);
|
|
|
|
position = to_motor_pos(position);
|
|
velocityMax = to_motor_vel(velocityMax);
|
|
writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
|
|
writeInt(TMC5130_VMAX, velocityMax);
|
|
writeInt(TMC5130_XTARGET, position);
|
|
}
|
|
void TMC51X0::moveToEnd(int32_t direction, uint32_t velocityMax) {
|
|
zlock_guard lkg(m_mutex);
|
|
|
|
if (direction >= 0) {
|
|
writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
|
|
writeInt(TMC5130_VMAX, to_motor_vel(velocityMax));
|
|
writeInt(TMC5130_XTARGET, INT32_MAX / 1.5 - 1000);
|
|
} else {
|
|
writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
|
|
writeInt(TMC5130_VMAX, to_motor_vel(velocityMax));
|
|
writeInt(TMC5130_XTARGET, INT32_MIN / 1.5 + 1000);
|
|
}
|
|
}
|
|
|
|
void TMC51X0::moveBy(int32_t relativePosition, uint32_t velocityMax) { // determine actual position and add numbers of ticks to move
|
|
zlock_guard lkg(m_mutex);
|
|
|
|
int32_t pos = getXACTUAL();
|
|
int32_t target = pos + relativePosition;
|
|
moveTo(target, velocityMax);
|
|
}
|
|
|
|
/***********************************************************************************************************************
|
|
* GET STATE *
|
|
***********************************************************************************************************************/
|
|
|
|
int32_t TMC51X0::getXACTUAL() {
|
|
zlock_guard lkg(m_mutex);
|
|
|
|
return to_user_pos(readInt(TMC5130_XACTUAL));
|
|
}
|
|
int32_t TMC51X0::getVACTUAL() {
|
|
zlock_guard lkg(m_mutex);
|
|
return to_user_pos(readInt(TMC5130_VACTUAL));
|
|
}
|
|
int32_t TMC51X0::getEncVal() {
|
|
zlock_guard lkg(m_mutex);
|
|
int32_t enc_val = to_user_pos(readInt(TMC5130_XENC));
|
|
if (m_enc_resolution < 0) {
|
|
enc_val = -enc_val;
|
|
}
|
|
return enc_val;
|
|
}
|
|
TMC5130RampStat TMC51X0::getRampStat() {
|
|
zlock_guard lkg(m_mutex);
|
|
int32_t val = readInt(TMC5130_RAMPSTAT);
|
|
return TMC5130RampStat(val);
|
|
}
|
|
|
|
bool TMC51X0::isReachTarget(TMC5130RampStat *state) {
|
|
zlock_guard lkg(m_mutex);
|
|
int mode = readInt(TMC5130_RAMPMODE);
|
|
if (mode == TMC5130_MODE_POSITION) {
|
|
return state->isSetted(TMC5130RampStat::ktmc5130_rs_posreached);
|
|
} else {
|
|
return state->isSetted(TMC5130RampStat::ktmc5130_rs_vzero) && state->isSetted(TMC5130RampStat::ktmc5130_rs_velreached);
|
|
}
|
|
}
|
|
bool TMC51X0::isTMC5130() {
|
|
zlock_guard lkg(m_mutex);
|
|
uint32_t regval = readUInt(TMC5130_IOIN);
|
|
uint32_t chipId = FIELD_GET(regval, TMC5130_VERSION_MASK, TMC5130_VERSION_SHIFT);
|
|
return chipId == kTMC5130;
|
|
}
|
|
|
|
TMC5130DevStatusReg_t TMC51X0::getDevStatus() { // R 读后不清
|
|
zlock_guard lkg(m_mutex);
|
|
static_assert(sizeof(TMC5130DevStatusReg_t) == 4);
|
|
uint32_t value = readInt(TMC5130_DRVSTATUS);
|
|
TMC5130DevStatusReg_t val;
|
|
memcpy(&val, &value, sizeof(TMC5130DevStatusReg_t));
|
|
return val;
|
|
}
|
|
TMC5130GState_t TMC51X0::getGState() { // R+C 读后自动清除
|
|
zlock_guard lkg(m_mutex);
|
|
static_assert(sizeof(TMC5130GState_t) == 4);
|
|
uint32_t value = readInt(TMC5130_GSTAT);
|
|
TMC5130GState_t val;
|
|
memcpy(&val, &value, sizeof(TMC5130GState_t));
|
|
return val;
|
|
}
|
|
int32_t TMC51X0::readICVersion() {
|
|
zlock_guard lkg(m_mutex);
|
|
uint32_t regval = readUInt(TMC5130_IOIN);
|
|
uint32_t chipId = FIELD_GET(regval, TMC5130_VERSION_MASK, TMC5130_VERSION_SHIFT);
|
|
return chipId;
|
|
}
|
|
|
|
/***********************************************************************************************************************
|
|
* set state *
|
|
***********************************************************************************************************************/
|
|
|
|
void TMC51X0::setXACTUAL(int32_t value) {
|
|
zlock_guard lkg(m_mutex);
|
|
writeInt(TMC5130_XACTUAL, to_motor_pos(value));
|
|
}
|
|
void TMC51X0::setMotorShaft(bool reverse) {
|
|
zlock_guard lkg(m_mutex);
|
|
uint32_t regval = readUInt(TMC5130_GCONF);
|
|
FIELD_SET(regval, TMC5130_SHAFT_MASK, TMC5130_SHAFT_SHIFT, reverse ? 1 : 0);
|
|
writeInt(TMC5130_GCONF, regval);
|
|
// uint32_t readbak = readUInt(TMC5130_GCONF);
|
|
// ZLOGI(TAG, "%x,%x", regval, readbak);
|
|
}
|
|
void TMC51X0::setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) {
|
|
zlock_guard lkg(m_mutex);
|
|
writeInt(TMC5130_IHOLD_IRUN, (iholddelay << TMC5130_IHOLDDELAY_SHIFT) | (irun << TMC5130_IRUN_SHIFT) | (ihold << TMC5130_IHOLD_SHIFT));
|
|
}
|
|
void TMC51X0::setGlobalScale(uint8_t globalscale) {
|
|
#define TMC5160_GLOBAL_SCALER 0x0B
|
|
#define TMC5160_GLOBAL_SCALER_MASK 0xFF
|
|
#define TMC5160_GLOBAL_SCALER_SHIFT 0
|
|
zlock_guard lkg(m_mutex);
|
|
if (isTMC5130()) {
|
|
return;
|
|
}
|
|
|
|
if (globalscale == 0) {
|
|
globalscale = 0;
|
|
} else if (globalscale <= 31 && globalscale >= 1) {
|
|
globalscale = 32;
|
|
} else {
|
|
globalscale = globalscale;
|
|
}
|
|
writeInt(TMC5160_GLOBAL_SCALER, (readInt(TMC5160_GLOBAL_SCALER) & ~TMC5160_GLOBAL_SCALER_MASK) | (globalscale << TMC5160_GLOBAL_SCALER_SHIFT));
|
|
}
|
|
void TMC51X0::setScale(int32_t scale) {
|
|
zlock_guard lkg(m_mutex);
|
|
m_scale = scale;
|
|
}
|
|
void TMC51X0::setScaleDenominator(int32_t scale) {
|
|
zlock_guard lkg(m_mutex);
|
|
m_scale_deceleration = scale;
|
|
}
|
|
void TMC51X0::setVstart(int32_t val) {
|
|
zlock_guard lkg(m_mutex);
|
|
writeInt(TMC5130_VSTART, to_motor_vel(val));
|
|
}
|
|
void TMC51X0::setA1(int32_t val) {
|
|
zlock_guard lkg(m_mutex);
|
|
writeInt(TMC5130_A1, to_motor_acc(val));
|
|
}
|
|
void TMC51X0::setAmax(int32_t val) {
|
|
zlock_guard lkg(m_mutex);
|
|
writeInt(TMC5130_AMAX, to_motor_acc(val));
|
|
}
|
|
void TMC51X0::setV1(int32_t val) {
|
|
zlock_guard lkg(m_mutex);
|
|
writeInt(TMC5130_V1, to_motor_vel(val));
|
|
}
|
|
void TMC51X0::setDmax(int32_t val) {
|
|
zlock_guard lkg(m_mutex);
|
|
writeInt(TMC5130_DMAX, to_motor_acc(val));
|
|
}
|
|
void TMC51X0::setD1(int32_t val) {
|
|
zlock_guard lkg(m_mutex);
|
|
writeInt(TMC5130_D1, to_motor_acc(val));
|
|
}
|
|
void TMC51X0::setVstop(int32_t val) {
|
|
zlock_guard lkg(m_mutex);
|
|
writeInt(TMC5130_VSTOP, to_motor_vel(val));
|
|
}
|
|
void TMC51X0::setTzerowait(int32_t val) {
|
|
zlock_guard lkg(m_mutex);
|
|
writeInt(TMC5130_TZEROWAIT, val);
|
|
}
|
|
bool TMC51X0::setEncResolution(int32_t enc_resolution) {
|
|
zlock_guard lkg(m_mutex);
|
|
/**
|
|
* @brief
|
|
*
|
|
* 1.假设电机是256细分
|
|
* 2.假设TMC5130_ENC_CONST是十进制模式
|
|
* 3.只支持指定分辨率的编码器
|
|
*/
|
|
|
|
int32_t enc_resolution_tmp = enc_resolution * 4;
|
|
int16_t enc_const_integral = 0;
|
|
int16_t enc_const_fractional = 0;
|
|
|
|
switch (abs(enc_resolution_tmp)) {
|
|
case 1000:
|
|
enc_const_integral = 51;
|
|
enc_const_fractional = 0.2 * 10000;
|
|
break;
|
|
case 1024:
|
|
enc_const_integral = 50;
|
|
enc_const_fractional = 0 * 10000;
|
|
break;
|
|
case 4000:
|
|
enc_const_integral = 12;
|
|
enc_const_fractional = 0.8 * 10000;
|
|
break;
|
|
case 4096:
|
|
enc_const_integral = 12;
|
|
enc_const_fractional = 0.5 * 10000;
|
|
break;
|
|
case 16384:
|
|
enc_const_integral = 3;
|
|
enc_const_fractional = 0.125 * 10000;
|
|
break;
|
|
case 0:
|
|
m_enc_resolution = 0;
|
|
return true;
|
|
|
|
default:
|
|
return false;
|
|
break;
|
|
}
|
|
|
|
m_enc_resolution = enc_resolution;
|
|
uint32_t setval = 0;
|
|
uint8_t *psetval = (uint8_t *)&setval;
|
|
|
|
memcpy(psetval + 2, &enc_const_integral, 2);
|
|
memcpy(psetval, &enc_const_fractional, 2);
|
|
|
|
writeInt(TMC5130_ENCMODE, 0x1 << 10);
|
|
writeInt(TMC5130_ENC_CONST, setval);
|
|
return true;
|
|
}
|
|
void TMC51X0::setEncVal(int32_t enc_val) {
|
|
zlock_guard lkg(m_mutex);
|
|
int32_t enc_set_val = to_motor_pos(enc_val);
|
|
if (m_enc_resolution < 0) {
|
|
enc_set_val = -enc_set_val;
|
|
}
|
|
writeInt(TMC5130_XENC, enc_set_val);
|
|
}
|
|
|
|
/***********************************************************************************************************************
|
|
* reg operation *
|
|
***********************************************************************************************************************/
|
|
void TMC51X0::writeIntExt(uint8_t address, int32_t value) {
|
|
zlock_guard lkg(m_mutex);
|
|
writeInt(address, value);
|
|
}
|
|
int32_t TMC51X0::readIntExt(uint8_t address) {
|
|
zlock_guard lkg(m_mutex);
|
|
return readInt(address);
|
|
}
|