hand_acid_mainboard/stm32components/tmcdriver/tmc51x0/tmc51x0.cpp

#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 TMC5160_GLOBAL_SCALER       0x0B
#define TMC5160_GLOBAL_SCALER_MASK  0xFF
#define TMC5160_GLOBAL_SCALER_SHIFT 0
/***********************************************************************************************************************
 *                                                      工具��??                                                         *
 ***********************************************************************************************************************/

#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(int mid, TMC51X0Cfg cfg) {
  m_mid  = mid;
  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();

  ZASSERT(m_cache.activeReg(TMC5130_CHOPCONF));
  ZASSERT(m_cache.activeReg(TMC5130_PWMCONF));
  ZASSERT(m_cache.activeReg(TMC5130_GCONF));
  ZASSERT(m_cache.activeReg(TMC5130_IHOLD_IRUN));
  ZASSERT(m_cache.activeReg(TMC5160_GLOBAL_SCALER));

  ZASSERT(m_cache.activeReg(TMC5130_VMAX));
  ZASSERT(m_cache.activeReg(TMC5130_VSTART));
  ZASSERT(m_cache.activeReg(TMC5130_A1));
  ZASSERT(m_cache.activeReg(TMC5130_AMAX));
  ZASSERT(m_cache.activeReg(TMC5130_V1));
  ZASSERT(m_cache.activeReg(TMC5130_DMAX));
  ZASSERT(m_cache.activeReg(TMC5130_D1));
  ZASSERT(m_cache.activeReg(TMC5130_VSTOP));
  ZASSERT(m_cache.activeReg(TMC5130_TZEROWAIT));
  ZASSERT(m_cache.activeReg(TMC5130_ENCMODE));
  ZASSERT(m_cache.activeReg(TMC5130_ENC_CONST));
  ZASSERT(m_cache.activeReg(TMC5130_XENC));

  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(15);
  setAmax(15);
  setV1(500);
  setDmax(15);
  setD1(15);
  setVstop(100);
  setTzerowait(100);

  setIHOLD_IRUN(0, 30, 100);
  enable(false);
}

void TMC51X0::refreshcfg() {
  m_cache.foreach ([this](TMCReg *item) {
    ZLOGI(TAG, "refreshcfg %x,%x", item->reg, item->val);
    writeInt(item->reg, item->val);
  });
}

/*******************************************************************************
 *                             驱动器寄存器读写方法                              *
 *******************************************************************************/

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) {  //
  m_cache.setReg(address, 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);
  int         mode = readInt(TMC5130_RAMPMODE);
  if (mode == TMC5130_MODE_POSITION) {
    // writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
    rotate(0);
    while (getVACTUAL() > 0) {
      osDelay(2);
    }
    writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
    writeInt(TMC5130_VMAX, 1);
    writeInt(TMC5130_XTARGET, readInt(TMC5130_XACTUAL));
  } else {
    rotate(0);
  }
  m_isMoveToEnd = false;
  // 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);
  m_isMoveToEnd = false;
}
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);
  m_isMoveToEnd = false;
}
void TMC51X0::moveToEnd(int32_t direction, uint32_t velocityMax) {
  zlock_guard lkg(m_mutex);

  // @WARNING:
  //    the motor will stop running(1000RPM) for about 10 minutes
  // If you really need to use this feature, reset XACTUAL every 30s
  // setXACTUAL(0);
  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);
  }
  m_isMoveToEnd = true;
}

void TMC51X0::refreshMoveToEnd() { setXACTUAL(0); }
bool TMC51X0::isMoveToEnd() { return m_isMoveToEnd; }

void TMC51X0::moveBy(int32_t relativePosition, uint32_t velocityMax) {  // determine actual position and add numbers of ticks to move
  zlock_guard lkg(m_mutex);

  // m_mid

  // ZLOGI(TAG, "m[%d] moveBy %d,%d", m_mid, relativePosition, velocityMax);

  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::getXactualRAW() {
  zlock_guard lkg(m_mutex);
  return 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) {
  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
   *
   */

  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);
}