project_boditech_vidas_a8000_v3
/
a8000_subboard_sdk
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

修改5130 类名为51x0

master
zhaohe 1 year ago
parent
563e4ca7f4
commit
07068dc9a8
7 changed files with 58 additions and 45 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  2. 2
      components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
  3. 1
      components/tmc/basic/tmc_ic_interface.hpp
  4. 4
      components/tmc/ic/ztmc4361A.cpp
  5. 2
      components/tmc/ic/ztmc4361A.hpp
  6. 79
      components/tmc/ic/ztmc5130.cpp
  7. 5
      components/tmc/ic/ztmc5130.hpp

10
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -25,7 +25,7 @@ void StepMotorCtrlModule::initialize(int moduleid, IStepperMotor* stepM, ZGPIO i
step_motor_active_cfg();
step_motor_enable(true);
TMC5130* tmcmotor = dynamic_cast<TMC5130*>(m_stepM1);
TMC51X0* tmcmotor = dynamic_cast<TMC51X0*>(m_stepM1);
if (tmcmotor) {
tmcmotor->getGState(); // 读取状态，清空下复位标识
}
@ -152,9 +152,9 @@ int32_t StepMotorCtrlModule::step_motor_read_io_state(int32_t ioindex, int32_t*
int32_t StepMotorCtrlModule::step_motor_read_tmc5130_status(int32_t* status) {
// *errorflag = m_state.motorStatus;
TMC5130* tmc5130 = dynamic_cast<TMC5130*>(m_stepM1);
TMC51X0* tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1);
if (tmc5130) {
static_assert(sizeof(TMC5130::DevStatusReg_t) == 4);
static_assert(sizeof(TMC51X0::DevStatusReg_t) == 4);
auto devStatus = tmc5130->getDevStatus();
memcpy(status, &devStatus, sizeof(int32_t));
}
@ -193,7 +193,7 @@ void StepMotorCtrlModule::after_motor_move() {
creg.m_module_status = 0;
}
void StepMotorCtrlModule::dumpTMC5130Status(TMC5130::DevStatusReg_t* status) {
void StepMotorCtrlModule::dumpTMC5130Status(TMC51X0::DevStatusReg_t* status) {
ZLOGE(TAG, "sg_result: %d", status->sg_result);
ZLOGE(TAG, "fsactive: %d", status->fsactive);
ZLOGE(TAG, "cs_actual: %d", status->cs_actual);
@ -214,7 +214,7 @@ void StepMotorCtrlModule::setErrorFlag(int32_t ecode, uint32_t motorStatue) {
bool StepMotorCtrlModule::check_when_run() {
//
TMC5130* tmc5130 = dynamic_cast<TMC5130*>(m_stepM1);
TMC51X0* tmc5130 = dynamic_cast<TMC51X0*>(m_stepM1);
if (tmc5130) {
auto state = tmc5130->getGState();
auto devStatus = tmc5130->getDevStatus();

2
components/step_motor_ctrl_module/step_motor_ctrl_module.hpp
View File

@ -116,7 +116,7 @@ class StepMotorCtrlModule : public ZIModule, public ZIStepMotor {
int32_t pri_module_xxx_reg(int32_t param_id, bool read, int32_t& val);
private:
void dumpTMC5130Status(TMC5130::DevStatusReg_t* status);
void dumpTMC5130Status(TMC51X0::DevStatusReg_t* status);
void setErrorFlag(int32_t ecode, uint32_t motorStatue);
};
} // namespace iflytop

1
components/tmc/basic/tmc_ic_interface.hpp
View File

@ -19,6 +19,7 @@ typedef enum {
typedef enum {
kTMC4361A = 0x2,
kTMC2160 = 0x30,
kTMC5130 = 0x11,
} tmcic_id_t;
class IStepperMotor {

4
components/tmc/ic/ztmc4361A.cpp
View File

@ -248,6 +248,10 @@ void TMC4361A::enable(bool enable) {
SET_PIN(m_driverIC_ennPin, !enable);
}
void TMC4361A::setGlobalScale(uint8_t globalscale) {
};
int32_t tmc4361A_discardVelocityDecimals(int32_t value) {
if (abs(value) > 8000000) {
value = (value < 0) ? -8000000 : 8000000;

2
components/tmc/ic/ztmc4361A.hpp
View File

@ -88,7 +88,7 @@ class TMC4361A : public IStepperMotor {
virtual void setDeceleration(float accelerationpps2); // 设置最大减速度
virtual void setMotorShaft(bool reverse) { driverIC_setMotorShaft(reverse); };
virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_setIHOLD_IRUN(ihold, irun, iholddelay); }
virtual void setGlobalScale(uint8_t globalscale) { ZLOGW("TMC4361", "setGlobalScale fail, not support"); }; // TODO:impl it
virtual void setGlobalScale(uint8_t globalscale);
virtual void setScale(int32_t scale);
virtual void setScaleDenominator(int32_t scale);

79
components/tmc/ic/ztmc5130.cpp
View File

@ -21,8 +21,8 @@ using namespace iflytop;
if (pin) { \
pin->setState(val); \
}
TMC5130::TMC5130(/* args */) {}
void TMC5130::initialize(cfg_t *cfg) {
TMC51X0::TMC51X0(/* args */) {}
void TMC51X0::initialize(cfg_t *cfg) {
m_cfg = *cfg;
// m_channel = channel;
@ -50,7 +50,7 @@ void TMC5130::initialize(cfg_t *cfg) {
reset();
}
void TMC5130::setMRES(mres_type_t value) {
void TMC51X0::setMRES(mres_type_t value) {
PRV_FIELD_WRITE(TMC5130_CHOPCONF, TMC5130_MRES_MASK, TMC5130_MRES_SHIFT, value);
m_MRES = value;
@ -77,7 +77,7 @@ void TMC5130::setMRES(mres_type_t value) {
}
}
void TMC5130::setNoAccLimit(bool enable) {
void TMC51X0::setNoAccLimit(bool enable) {
if (!enable) {
writeInt(TMC5130_VSTART, 100);
writeInt(TMC5130_VSTOP, 100);
@ -87,11 +87,11 @@ void TMC5130::setNoAccLimit(bool enable) {
}
}
void TMC5130::enable(bool enable) {
void TMC51X0::enable(bool enable) {
// m_port->TMC5130Port_setENNPinState(m_channel, !enable);
SET_PIN(m_ennpin, !enable);
}
uint8_t TMC5130::reset() {
uint8_t TMC51X0::reset() {
stop();
// m_port->TMC5130Port_setResetNPinState(m_channel, false);
SET_PIN(m_csnpin, false);
@ -125,19 +125,26 @@ uint8_t TMC5130::reset() {
enable(false);
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)); }
int32_t TMC51X0::getXACTUAL() { return to_user_pos(readInt(TMC5130_XACTUAL)); }
void TMC51X0::setXACTUAL(int32_t value) { writeInt(TMC5130_XACTUAL, to_motor_pos(value)); }
int32_t TMC51X0::getVACTUAL() { return to_user_pos(readInt(TMC5130_VACTUAL)); }
void TMC51X0::setAcceleration(float accelerationpps2) { writeInt(TMC5130_AMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // 设置最大加速度
void TMC51X0::setDeceleration(float accelerationpps2) { writeInt(TMC5130_DMAX, (int32_t)(to_motor_acc(accelerationpps2))); } // 设置最大减速度
void TMC51X0::setMotorShaft(bool reverse) { PRV_FIELD_WRITE(TMC5130_GCONF, TMC5130_SHAFT_MASK, TMC5130_SHAFT_SHIFT, reverse); }
void TMC51X0::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) { //
void TMC51X0::setGlobalScale(uint8_t globalscale) { //
uint32_t chipId = readICVersion();
if (chipId == kTMC5130) {
// 5130不支持全局缩放
return;
}
if (globalscale == 0) {
globalscale = 0;
} else if (globalscale <= 31 || globalscale >= 1) {
@ -147,27 +154,27 @@ void TMC5130::setGlobalScale(uint8_t globalscale) { //
}
writeInt(TMC5160_GLOBAL_SCALER, (readInt(TMC5160_GLOBAL_SCALER) & ~TMC5160_GLOBAL_SCALER_MASK) | (globalscale << TMC5160_GLOBAL_SCALER_SHIFT));
}
uint8_t TMC5130::getGlobalScale() { //
uint8_t TMC51X0::getGlobalScale() { //
return PRV_FIELD_READ(TMC5160_GLOBAL_SCALER, TMC5160_GLOBAL_SCALER_MASK, TMC5160_GLOBAL_SCALER_SHIFT);
}
uint32_t TMC5130::readICVersion() {
uint32_t TMC51X0::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 TMC51X0::getTMC5130_RAMPSTAT() { return readInt(TMC5130_RAMPSTAT); }
Tmc5130RampStat TMC51X0::getTMC5130_RAMPSTAT2() {
uint32_t value = getTMC5130_RAMPSTAT();
return Tmc5130RampStat(value);
}
void TMC5130::stop() { rotate(0); }
void TMC5130::rotate(int32_t velocity) {
void TMC51X0::stop() { rotate(0); }
void TMC51X0::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::moveTo(int32_t position, uint32_t velocityMax) {
void TMC51X0::moveTo(int32_t position, uint32_t velocityMax) {
// position *= m_scale;
// velocityMax *= m_scale;
position = to_motor_pos(position);
@ -177,19 +184,19 @@ void TMC5130::moveTo(int32_t position, uint32_t velocityMax) {
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
void TMC51X0::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) {
uint32_t TMC51X0::readXTARGET() { return readInt(TMC5130_XTARGET); }
uint32_t TMC51X0::haspassedms(uint32_t now, uint32_t last) {
if (now >= last) {
return now - last;
} else {
return 0xFFFFFFFF - last + now;
}
}
bool TMC5130::isReachTarget() {
bool TMC51X0::isReachTarget() {
/**
* @brief
*/
@ -205,8 +212,8 @@ bool TMC5130::isReachTarget() {
}
}
void TMC5130::setScale(int32_t scale) { m_scale = scale; }
void TMC5130::setScaleDenominator(int32_t scale) { m_scale_deceleration = scale; }
void TMC51X0::setScale(int32_t scale) { m_scale = scale; }
void TMC51X0::setScaleDenominator(int32_t scale) { m_scale_deceleration = scale; }
/*******************************************************************************
* basic *
@ -215,7 +222,7 @@ void TMC5130::setScaleDenominator(int32_t scale) { m_scale_deceleration = scale;
// CriticalContext cc;
// writeInt(address, readInt(address) & ~mask | value << shift);
// }
void TMC5130::readWriteArray(uint8_t *data, size_t length) {
void TMC51X0::readWriteArray(uint8_t *data, size_t length) {
CriticalContext cc;
// m_port->TMC5130Port_readWriteArray(m_channel, data, length);
// m_csnpin
@ -223,7 +230,7 @@ void TMC5130::readWriteArray(uint8_t *data, size_t length) {
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) {
void TMC51X0::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);
@ -234,11 +241,11 @@ void TMC5130::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3,
address = TMC_ADDRESS(address);
m_shadowRegister[address] = value;
}
void TMC5130::writeInt(uint8_t address, int32_t value) {
void TMC51X0::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) {
int32_t TMC51X0::readInt(uint8_t address) {
CriticalContext cc;
address = TMC_ADDRESS(address);
@ -255,28 +262,28 @@ int32_t TMC5130::readInt(uint8_t address) {
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 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 TMC5130::to_motor_vel(int32_t vel) { //
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 TMC5130::to_motor_pos(int32_t pos) { //
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 TMC5130::to_user_pos(int32_t pos) { //
int32_t TMC51X0::to_user_pos(int32_t pos) { //
int32_t val = pos / m_onecirclepulse * m_scale / m_scale_deceleration + 0.5;
return val;
} //
int32_t TMC5130::to_user_vel(int32_t vel) { //
int32_t TMC51X0::to_user_vel(int32_t vel) { //
int32_t val = vel * 60.0 / m_onecirclepulse;
return val;
}

5
components/tmc/ic/ztmc5130.hpp
View File

@ -41,9 +41,10 @@ class Tmc5130RampStat {
Tmc5130RampStat(uint32_t state) : m_state(state) {}
bool isSetted(ramp_stat_bit_t bit) { return (m_state & bit) != 0; }
};
class TMC5130 : public IStepperMotor {
class TMC51X0 : public IStepperMotor {
public:
typedef struct {
uint32_t sg_result : 10;
@ -98,7 +99,7 @@ class TMC5130 : public IStepperMotor {
double m_onecirclepulse = 51200;
public:
TMC5130(/* args */);
TMC51X0(/* args */);
void initialize(cfg_t *cfg);

Write Preview
Loading…
Cancel
Save