|
|
#include "ztmc5130.hpp"
#include "sdk/os/zos.hpp"
#ifdef HAL_SPI_MODULE_ENABLED
using namespace iflytop;
/**
* @brief ��̬����������Ĭ�ϵ�TMC5130���ò���,ʹ��ʱֻ�����Լ���Ҫ�IJ������� * * ע��: * 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->csgpio != PinNull) { m_csnpin = new ZGPIO(); m_csnpin->initAsOutput(cfg->csgpio, ZGPIO::kMode_nopull, false, true); ZASSERT(m_csnpin); } if (cfg->ennPin != PinNull) { m_ennpin = new ZGPIO(); m_ennpin->initAsOutput(cfg->ennPin, ZGPIO::kMode_nopull, false, true); ZASSERT(m_ennpin); } if (cfg->spi_mode_select != PinNull) { m_spi_mode_select_gpio = new ZGPIO(); m_spi_mode_select_gpio->initAsOutput(cfg->spi_mode_select, ZGPIO::kMode_nopull, false, false); } m_hspi = cfg->spi;
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(100); setDeceleration(100); 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 to_user_pos(readInt(TMC5130_XACTUAL)); }void TMC5130::setXACTUAL(int32_t value) { writeInt(TMC5130_XACTUAL, to_motor_pos(value)); }int32_t TMC5130::getVACTUAL() { return to_user_pos(readInt(TMC5130_VACTUAL)); }void TMC5130::setAcceleration(float accelerationpps2) { writeInt(TMC5130_AMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // �����������ٶ�
void TMC5130::setDeceleration(float accelerationpps2) { writeInt(TMC5130_DMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // �����������ٶ�
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::readICVersion() { 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;
velocity = to_motor_vel(velocity); 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;
position = to_motor_pos(position); velocityMax = to_motor_vel(velocityMax); // ZLOGI("TMC5130", "moveTo %d %d", position, velocityMax);
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(int32_t scale) { m_scale = scale; }void TMC5130::setScaleDenominator(int32_t scale) { m_scale_deceleration = 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];}int32_t TMC5130::to_motor_acc(int32_t acc) { //
int32_t val = acc / 60.0 * 51200; return val;}int32_t TMC5130::to_motor_vel(int32_t vel) { //
int32_t val = vel / 60.0 * 51200; return val;} // rpm
int32_t TMC5130::to_motor_pos(int32_t pos) { //
int32_t val = pos * 1.0 / m_scale * m_scale_deceleration * 51200.0; return val;} //
int32_t TMC5130::to_user_pos(int32_t pos) { //
int32_t val = pos / 51200.0 * m_scale / m_scale_deceleration; return val;} //
int32_t TMC5130::to_user_vel(int32_t vel) { //
int32_t val = vel * 60.0 / 51200.0; return val;}#endif
|
