p_graphite_digester_sdk/components/tmc/ic/ztmc5130.cpp

#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, 100);
  writeInt(TMC5130_A1, 1000);
  writeInt(TMC5130_V1, 0);

  writeInt(TMC5130_D1, 1000);
  writeInt(TMC5130_VSTOP, 100);
  writeInt(TMC5130_TZEROWAIT, 0);

  // writeInt(TMC5130_VSTART, 100);
  // writeInt(TMC5130_V1, 0);
  // writeInt(TMC5130_VSTOP, 100);
  // writeInt(TMC5130_TZEROWAIT, 0);

  setAcceleration(100);
  setDeceleration(100);
  setIHOLD_IRUN(2, 10, 1);
  enableIC(true);
}

void TMC5130::setMRES(mres_type_t value) {
  PRV_FIELD_WRITE(TMC5130_CHOPCONF, TMC5130_MRES_MASK, TMC5130_MRES_SHIFT, value);

  m_MRES = value;
  if (m_MRES == kmres_256) {
    m_onecirclepulse = 51200;
  } else if (m_MRES == kmres_128) {
    m_onecirclepulse = 25600;
  } else if (m_MRES == kmres_64) {
    m_onecirclepulse = 12800;
  } else if (m_MRES == kmres_32) {
    m_onecirclepulse = 6400;
  } else if (m_MRES == kmres_16) {
    m_onecirclepulse = 3200;
  } else if (m_MRES == kmres_8) {
    m_onecirclepulse = 1600;
  } else if (m_MRES == kmres_4) {
    m_onecirclepulse = 800;
  } else if (m_MRES == kmres_2) {
    m_onecirclepulse = 400;
  } else if (m_MRES == kmres_1) {
    m_onecirclepulse = 200;
  } else {
    ZASSERT(false);
  }
}

void TMC5130::setNoAccLimit(bool enable) {
  if (!enable) {
    writeInt(TMC5130_VSTART, 100);
    writeInt(TMC5130_VSTOP, 100);
  } else {
    writeInt(TMC5130_VSTART, 262144 - 1);  // 2^18-1
    writeInt(TMC5130_VSTOP, 262144 - 1);   // 2^18-1
  }
}

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)); }
void TMC5130::setENCVAL(int32_t val) { writeInt(TMC5130_XENC, to_motor_pos(val)); }

int32_t TMC5130::getVACTUAL() { return to_user_pos(readInt(TMC5130_VACTUAL)); }
int32_t TMC5130::getENCVAL() { return to_user_pos(readInt(TMC5130_XENC)); }

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

#define TMC5160_GLOBAL_SCALER       0x0B
#define TMC5160_GLOBAL_SCALER_MASK  0xFF
#define TMC5160_GLOBAL_SCALER_SHIFT 0

void TMC5130::setGlobalScale(uint8_t globalscale) {  //
  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));
}
uint8_t TMC5130::getGlobalScale() {  //
  return PRV_FIELD_READ(TMC5160_GLOBAL_SCALER, TMC5160_GLOBAL_SCALER_MASK, TMC5160_GLOBAL_SCALER_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));
}
  void    TMC5130::writeInt(uint8_t address, int32_t mask, int32_t shift, int32_t value){
    CriticalContext cc;
    writeInt(address, FIELD_SET(readInt(address), mask, shift, value));
  
  }

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 * m_onecirclepulse;  // 65535
  if (val > 65535) val = 65535;
  return val;
}
int32_t TMC5130::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 TMC5130::to_motor_pos(int32_t pos) {  //
  int32_t val = pos * 1.0 / m_scale * m_scale_deceleration * m_onecirclepulse;
  return val;
}  //

int32_t TMC5130::to_user_pos(int32_t pos) {  //
  int32_t val = pos / m_onecirclepulse * m_scale / m_scale_deceleration;
  return val;
}  //
int32_t TMC5130::to_user_vel(int32_t vel) {  //
  int32_t val = vel * 60.0 / m_onecirclepulse;
  return val;
}
#endif