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

整理TMC4361

master
zhaohe 1 year ago
parent
e2dffc30e3
commit
9aee8f2a2c
7 changed files with 153 additions and 323 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. 1
      components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
  2. 99
      components/tmc/basic/tmc_ic_interface.hpp
  3. 18
      components/tmc/ic/README.md
  4. 56
      components/tmc/ic/tmc_driver_ic.hpp
  5. 150
      components/tmc/ic/ztmc4361A.cpp
  6. 133
      components/tmc/ic/ztmc4361A.hpp
  7. 19
      components/tmc/ic/ztmc5130.hpp

1
components/step_motor_ctrl_module/step_motor_ctrl_module.cpp
View File

@ -247,6 +247,7 @@ int32_t StepMotorCtrlModule::step_motor_active_cfg() {
m_stepM1->setScale(m_cfg.motor_one_circle_pulse);
m_stepM1->setScaleDenominator(m_cfg.motor_one_circle_pulse_denominator);
m_stepM1->setMotorShaft(m_cfg.motor_shaft);
// stepmotor_iglobalscaler
if (m_state.enable) {
m_stepM1->enable(true);
}

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

@ -3,71 +3,6 @@
namespace iflytop {
using namespace std;
class IStepperMotor;
class IStepperMotor {
public:
IStepperMotor() {}
virtual ~IStepperMotor() {}
// virtual void registerListener(MotorEventListener* listener) = 0;
/****************************************************
* 速度模式 *
****************************************************/
/**
* @brief 旋转电机，正转为正值，负转为负值
*
* @param velocity 电机旋转速度，单位为pps
*/
virtual void rotate(int32_t velocity) = 0;
/****************************************************
* 位置模式 *
****************************************************/
/**
* @brief 电机运动到指定位置
*
* @param position 目标位置
* @param velocityMax 最大速度 电机旋转速度，单位为pps
*/
virtual void moveTo(int32_t position, uint32_t velocityMax) = 0;
/**
* @brief 电机相对于当前位置移动指定的距离
*
* @param relativePosition 相对于当前位置的距离
* @param velocityMax 最大速度 电机旋转速度，单位为pps
*/
virtual void moveBy(int32_t relativePosition, uint32_t velocityMax) = 0;
/**
* @brief 停止电机
*/
virtual void stop() = 0;
virtual int32_t getXACTUAL() = 0;
virtual void setXACTUAL(int32_t value) = 0;
virtual int32_t getVACTUAL() = 0;
virtual int32_t getXTARGET() { return 0; };
virtual int32_t getENC_POS() = 0;
virtual void setENC_POS(int32_t value) = 0;
virtual void setAcceleration(float accelerationpps2) = 0; // 设置最大加速度
virtual void setDeceleration(float accelerationpps2) = 0; // 设置最大减速度
virtual void setMotorShaft(bool reverse) = 0;
virtual bool isReachTarget() = 0;
virtual bool isStoped() = 0;
virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) = 0;
virtual void setGlobalScale(uint8_t globalscale) = 0;
virtual void setScale(int32_t scale) = 0;
virtual void setScaleDenominator(int32_t scale) = 0;
virtual void enable(bool enable) = 0;
virtual void setNoAccLimit(bool enable){};
};
typedef enum {
kmres_256 = 0x00,
@ -81,4 +16,38 @@ typedef enum {
kmres_1 = 0x08,
} mres_type_t;
typedef enum {
kTMC4361A = 0x2,
kTMC2160 = 0x30,
} tmcic_id_t;
class IStepperMotor {
public:
IStepperMotor() {}
virtual ~IStepperMotor() {}
virtual void enable(bool enable) = 0;
virtual void rotate(int32_t velocity) = 0;
virtual void moveTo(int32_t position, uint32_t velocityMax) = 0;
virtual void moveBy(int32_t relativePosition, uint32_t velocityMax) = 0;
virtual void stop() = 0;
virtual void setXACTUAL(int32_t value) = 0; // 设置当前位置
virtual int32_t getXACTUAL() = 0; // 当前位置
virtual int32_t getVACTUAL() = 0; // 当前速度
virtual void setAcceleration(float accelerationpps2) = 0; // 设置最大加速度
virtual void setDeceleration(float accelerationpps2) = 0; // 设置最大减速度
virtual void setMotorShaft(bool reverse) = 0;
virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) = 0;
virtual void setGlobalScale(uint8_t globalscale) = 0;
virtual void setScale(int32_t scale) = 0;
virtual void setScaleDenominator(int32_t scale) = 0;
virtual bool isReachTarget() = 0; // 是否到达目标位置
virtual bool isStoped() = 0; // 是否停止运动
virtual void setNoAccLimit(bool enable) = 0;
};
} // namespace iflytop

18
components/tmc/ic/README.md
View File

@ -0,0 +1,18 @@
5130
```
/**
* @brief
*
* 注意事项：
* 1. 速度单位
* velocity的单位是:p/t
* t = 2^24/fin (fin为外接时钟，当时钟频率为16MHz时，t = 1.048576s)
* 此时 ppt约等于pps
*
* 参考TMC5130A_datasheet_rev1.20%20(1)%20(3).pdf 38页 蓝框中的字
*
* 2. 芯片复位方法
* reset(); 复位芯片，同时会将芯片的寄存器设置为默认值。一般情况下不需要用户主动调用该方法，因为在初始化时会自动调用该方法。
*
*/
```

56
components/tmc/ic/tmc_driver_ic.hpp
View File

@ -1,56 +0,0 @@
#pragma once
#include <stdint.h>
class TMCDriverICPort {
public:
public:
/**
* @brief 复位引脚，低有效。
*
* @param state
*/
virtual void TMCDriverICPort_setResetPinState(uint16_t channel, bool state) = 0;
/**
* @brief 使能引脚，低有效。
*
* @param state
*/
virtual void TMCDriverICPort_setENNPinState(uint16_t channel, bool state) = 0;
/**
* @brief us级别延时函数。
*
* @param us
*/
virtual void TMCDriverICPort_sleepus(int32_t us) = 0;
/**
* @brief SPI读写接口
*
* @param data
* @param length
*/
virtual void TMCDriverICPort_readWriteArray(uint16_t channel, uint8_t *data, size_t length) = 0;
};
class TMCDriverICInterface {
public:
virtual void initialize(uint8_t channel, TMCDriverICPort *port) = 0;
virtual uint8_t reset() = 0;
virtual uint8_t restore() = 0;
virtual void enableIC(bool enable) = 0;
virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) = 0;
// register access
virtual void writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) = 0;
virtual void writeInt(uint8_t address, int32_t value) = 0;
virtual int32_t readInt(uint8_t address) = 0;
/**
* @brief 读取芯片版本
*
* @return int32_t 默认为0x30
*/
virtual int32_t readICVersion() = 0;
virtual void setMotorShaft(bool reverse) = 0;
virtual ~TMCDriverICInterface(){};
};

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

@ -10,8 +10,8 @@
#include "sdk/os/zos.hpp"
using namespace iflytop;
#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 PRV_FIELD_WRITE(address, mask, shift, value) (tmc4361_writeInt(address, FIELD_SET(tmc4361_readInt(address), mask, shift, value)))
#define PRV_FIELD_READ(address, mask, shift) FIELD_GET(tmc4361_readInt(address), mask, shift)
#define SET_PIN(pin, val) \
if (pin) { \
@ -19,15 +19,15 @@ using namespace iflytop;
}
#if 1
void TMC4361A::readWriteArray(uint8_t *data, size_t length) {
void TMC4361A::tmc4361_readWriteArray(uint8_t *data, size_t length) {
m_csgpio->setState(false);
// zchip_clock_early_delayus(10);
zchip_clock_early_delayus(10);
HAL_SPI_TransmitReceive(m_spi, data, data, length, 1000);
m_csgpio->setState(true);
}
void TMC4361A::writeInt(uint8_t address, int32_t value) { writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0)); }
int32_t TMC4361A::readInt(uint8_t address) {
void TMC4361A::tmc4361_writeInt(uint8_t address, int32_t value) { tmc4361_writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0)); }
int32_t TMC4361A::tmc4361_readInt(uint8_t address) {
CriticalContext cc;
int value;
@ -38,23 +38,23 @@ int32_t TMC4361A::readInt(uint8_t address) {
if (!TMC_IS_READABLE(m_registerAccessTable[address])) return m_defaultRegisterResetState[address];
data[0] = address;
readWriteArray(&data[0], 5);
tmc4361_readWriteArray(&data[0], 5);
data[0] = address;
readWriteArray(&data[0], 5);
tmc4361_readWriteArray(&data[0], 5);
m_status = data[0];
value = ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4];
return value;
}
void TMC4361A::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) {
void TMC4361A::tmc4361_writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) {
CriticalContext cc;
int value;
uint8_t data[5] = {static_cast<uint8_t>(address | static_cast<uint8_t>(TMC4361A_WRITE_BIT)), x1, x2, x3, x4};
readWriteArray(&data[0], 5);
tmc4361_readWriteArray(&data[0], 5);
m_status = data[0];
@ -65,7 +65,7 @@ void TMC4361A::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3
shadowRegister[address] = value;
}
void TMC4361A::readWriteCover(uint8_t *data, size_t length) {
void TMC4361A::tmc4361_readWriteCover(uint8_t *data, size_t length) {
CriticalContext cc;
// Buffering old values to not interrupt manual covering
int32_t old_high = shadowRegister[TMC4361A_COVER_HIGH_WR];
@ -82,20 +82,20 @@ void TMC4361A::readWriteCover(uint8_t *data, size_t length) {
for (i = 0; i < length; i++) bytes[i] = data[length - i - 1];
// Send the datagram
if (length > 4) writeDatagram(TMC4361A_COVER_HIGH_WR, bytes[7], bytes[6], bytes[5], bytes[4]);
writeDatagram(TMC4361A_COVER_LOW_WR, bytes[3], bytes[2], bytes[1], bytes[0]);
if (length > 4) tmc4361_writeDatagram(TMC4361A_COVER_HIGH_WR, bytes[7], bytes[6], bytes[5], bytes[4]);
tmc4361_writeDatagram(TMC4361A_COVER_LOW_WR, bytes[3], bytes[2], bytes[1], bytes[0]);
zos_delay(10);
// Read the reply
if (length > 4) {
tmp = readInt(TMC4361A_COVER_DRV_HIGH_RD);
tmp = tmc4361_readInt(TMC4361A_COVER_DRV_HIGH_RD);
bytes[4] = BYTE(tmp, 0);
bytes[5] = BYTE(tmp, 1);
bytes[6] = BYTE(tmp, 2);
bytes[7] = BYTE(tmp, 3);
}
tmp = readInt(TMC4361A_COVER_DRV_LOW_RD);
tmp = tmc4361_readInt(TMC4361A_COVER_DRV_LOW_RD);
bytes[0] = BYTE(tmp, 0);
bytes[1] = BYTE(tmp, 1);
bytes[2] = BYTE(tmp, 2);
@ -107,7 +107,7 @@ void TMC4361A::readWriteCover(uint8_t *data, size_t length) {
}
// Rewriting old values to prevent interrupting manual covering. Imitating unchanged values and state.
writeInt(TMC4361A_COVER_HIGH_WR, old_high);
tmc4361_writeInt(TMC4361A_COVER_HIGH_WR, old_high);
shadowRegister[TMC4361A_COVER_LOW_WR] = old_low;
}
@ -116,16 +116,8 @@ TMC4361A::TMC4361A(/* args */) {
m_lastCallPeriodicJobTick = 0;
// m_reachtarget = false;
}
/**
* @brief
* 当调用TMC-API中的tmc4361A_reset/restore时候，TMC-API中的方法会初始化整个芯片的寄存器
* 当寄存器初始化完成之后，会调用这个方法??
* 我们可以在这个方法中，对芯片的部分寄存器进行初始化??
* @param state
*/
void TMC4361A::tmc4361AConfigCallback(ConfigState state) {}
void TMC4361A::writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value) { //
void TMC4361A::tmc4361_writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value) { //
PRV_FIELD_WRITE(address, mask, shift, value);
}
void TMC4361A::setAcceleration(float accelerationpps2) {
@ -143,7 +135,7 @@ void TMC4361A::setAcceleration(float accelerationpps2) {
accelerationpps2 = to_motor_acc(accelerationpps2);
int32_t acc = (int32_t)accelerationpps2;
writeInt(TMC4361A_AMAX, acc << 2);
tmc4361_writeInt(TMC4361A_AMAX, acc << 2);
}
void TMC4361A::setDeceleration(float accelerationpps2) {
/**
@ -158,7 +150,7 @@ void TMC4361A::setDeceleration(float accelerationpps2) {
*/
accelerationpps2 = to_motor_acc(accelerationpps2);
int32_t acc = (int32_t)accelerationpps2;
writeInt(TMC4361A_DMAX, acc << 2);
tmc4361_writeInt(TMC4361A_DMAX, acc << 2);
}
#define INIT_GPIO(m_pin, pin, ...) \
if (pin != PinNull) { \
@ -194,7 +186,7 @@ void TMC4361A::initialize(cfg_t *cfg) {
setAcceleration(500000);
setDeceleration(500000);
enableIC(false);
enable(false);
zchip_clock_early_delayus(300 * 1000);
zchip_clock_early_delayus(300 * 1000);
@ -219,7 +211,7 @@ uint8_t TMC4361A::reset() {
if (!TMC_IS_RESETTABLE(m_registerAccessTable[add])) {
continue;
}
writeInt(add, m_defaultRegisterResetState[add]);
tmc4361_writeInt(add, m_defaultRegisterResetState[add]);
}
uint8_t driver, dataLength;
@ -241,21 +233,17 @@ uint8_t TMC4361A::reset() {
break;
}
value = 0x44400040 | (dataLength << 13) | (driver << 0);
writeInt(TMC4361A_SPIOUT_CONF, value);
writeInt(TMC4361A_CURRENT_CONF, 0x00000003);
writeInt(TMC4361A_SCALE_VALUES, 0x00000000);
tmc4361_writeInt(TMC4361A_SPIOUT_CONF, value);
tmc4361_writeInt(TMC4361A_CURRENT_CONF, 0x00000003);
tmc4361_writeInt(TMC4361A_SCALE_VALUES, 0x00000000);
return 1;
}
uint8_t TMC4361A::restore() { return 1; }
int32_t TMC4361A::getXACTUAL() { return to_user_pos(readInt(TMC4361A_XACTUAL)); }
void TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, to_motor_pos(value)); }
int32_t TMC4361A::getVACTUAL() { return to_user_pos(readInt(TMC4361A_VACTUAL)); }
int32_t TMC4361A::getXTARGET() { return to_user_pos(readInt(TMC4361A_X_TARGET)); }
int32_t TMC4361A::getENC_POS() { return to_user_pos(readInt(TMC4361A_ENC_POS)); }
void TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, to_motor_pos(value)); }
int32_t TMC4361A::getENC_POS_DEV() { return to_user_pos(readInt(TMC4361A_ENC_POS_DEV_RD)); }
void TMC4361A::enableIC(bool enable) {
int32_t TMC4361A::getXACTUAL() { return to_user_pos(tmc4361_readInt(TMC4361A_XACTUAL)); }
void TMC4361A::setXACTUAL(int32_t value) { tmc4361_writeInt(TMC4361A_XACTUAL, to_motor_pos(value)); }
int32_t TMC4361A::getVACTUAL() { return to_user_pos(tmc4361_readInt(TMC4361A_VACTUAL)); }
void TMC4361A::enable(bool enable) {
SET_PIN(m_ennPin, !enable);
SET_PIN(m_driverIC_ennPin, !enable);
}
@ -271,7 +259,7 @@ void TMC4361A::rotate(int32_t velocity) {
velocity = to_motor_vel(velocity);
m_motor_mode = kvelmode;
PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 0);
writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocity));
tmc4361_writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocity));
}
void TMC4361A::stop() { rotate(0); }
void TMC4361A::moveTo(int32_t position, uint32_t velocityMax) {
@ -283,46 +271,37 @@ void TMC4361A::moveTo(int32_t position, uint32_t velocityMax) {
m_motor_mode = kposmode;
PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 1);
writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocityMax));
writeInt(TMC4361A_X_TARGET, position);
tmc4361_writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocityMax));
tmc4361_writeInt(TMC4361A_X_TARGET, position);
}
void TMC4361A::moveBy(int32_t relativePosition, uint32_t velocityMax) {
relativePosition += readInt(TMC4361A_XACTUAL);
relativePosition += tmc4361_readInt(TMC4361A_XACTUAL);
moveTo(relativePosition, velocityMax);
}
int32_t TMC4361A::readICVersion() {
int32_t value = readInt(TMC4361A_VERSION_NO_RD);
return (value & TMC4361A_VERSION_NO_MASK) >> TMC4361A_VERSION_NO_SHIFT;
tmcic_id_t TMC4361A::readICVersion() {
int32_t value = tmc4361_readInt(TMC4361A_VERSION_NO_RD);
return (tmcic_id_t)((value & TMC4361A_VERSION_NO_MASK) >> TMC4361A_VERSION_NO_SHIFT);
}
int32_t TMC4361A::readSubICVersion() { return driverIC_readICVersion(); }
/*******************************************************************************
* 2160 function end *
*******************************************************************************/
uint32_t TMC4361A::readEVENTS() {
uint32_t value = readInt(TMC4361A_EVENTS);
return value;
}
uint32_t TMC4361A::haspassedms(uint32_t now, uint32_t last) {
if (now >= last) {
return now - last;
} else {
return 0xFFFFFFFF - last + now;
}
}
bool TMC4361A::isReachTarget() {
uint32_t value = readInt(TMC4361A_STATUS);
// printf("TMC4361A_STATUS:%08x\n", value);
uint32_t value = tmc4361_readInt(TMC4361A_STATUS);
if (m_motor_mode == kposmode) {
return (value & TMC4361A_TARGET_REACHED_F_MASK) > 0;
} else {
return (value & TMC4361A_VEL_REACHED_F_MASK) && (readInt(TMC4361A_VMAX) == 0);
return (value & TMC4361A_VEL_REACHED_F_MASK) && (tmc4361_readInt(TMC4361A_VMAX) == 0);
}
}
/*******************************************************************************
* DRIVER_IC *
*******************************************************************************/
void TMC4361A::setGLOBAL_SCALER(int32_t value) {
if (value > 255) value = 255;
if (value != 0 && value < 32) value = 32;
driverIC_writeInt(TMC2160_GLOBAL_SCALER, value);
}
/***********************************************************************************************************************
* DRIVERIC_OPERATION *
***********************************************************************************************************************/
#define DRIVER_ID_FIELD_READ(address, mask, shift) FIELD_GET(driverIC_readInt(address), mask, shift)
#define DRIVER_ID_FIELD_WRITE(address, mask, shift, value) (driverIC_writeInt(address, FIELD_SET(driverIC_readInt(address), mask, shift, value)))
void TMC4361A::driverIC_reset() {
@ -350,7 +329,7 @@ void TMC4361A::driverIC_reset() {
void TMC4361A::driverIC_enableIC(bool enable) { SET_PIN(m_driverIC_ennPin, !enable); }
void TMC4361A::driverIC_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>(TMC2160_WRITE_BIT)), x1, x2, x3, x4};
readWriteCover(&data[0], 5);
tmc4361_readWriteCover(&data[0], 5);
}
void TMC4361A::driverIC_writeInt(uint8_t address, int32_t value) { driverIC_writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0)); }
int32_t TMC4361A::driverIC_readInt(uint8_t address) {
@ -361,10 +340,10 @@ int32_t TMC4361A::driverIC_readInt(uint8_t address) {
uint8_t data[5];
data[0] = address;
readWriteCover(&data[0], 5);
tmc4361_readWriteCover(&data[0], 5);
data[0] = address;
readWriteCover(&data[0], 5);
tmc4361_readWriteCover(&data[0], 5);
return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4];
}
@ -380,24 +359,9 @@ uint32_t TMC4361A::driverIC_readICVersion() {
}
void TMC4361A::driverIC_setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_writeInt(TMC2160_IHOLD_IRUN, (iholddelay << TMC2160_IHOLDDELAY_SHIFT) | (irun << TMC2160_IRUN_SHIFT) | (ihold << TMC2160_IHOLD_SHIFT)); }
void TMC4361A::setGLOBAL_SCALER(int32_t value) {
if (value > 255) value = 255;
if (value != 0 && value < 32) value = 32;
driverIC_writeInt(TMC2160_GLOBAL_SCALER, value);
}
// Left Virtual Limit Switch XACTUAL ≤ VIRT_STOP_LEFT 时触发
void TMC4361A::setLeftVirtualLimitSwitch(bool enable, int32_t position) {
PRV_FIELD_WRITE(TMC4361A_REFERENCE_CONF, TMC4361A_VIRTUAL_LEFT_LIMIT_EN_MASK, TMC4361A_VIRTUAL_LEFT_LIMIT_EN_SHIFT, enable ? 1 : 0);
writeInt(TMC4361A_VIRT_STOP_LEFT, position);
}
// Right Virtual Limit Switch XACTUAL ≥ VIRT_STOP_RIGHT 时触发
void TMC4361A::setRightVirtualLimitSwitch(bool enable, int32_t position) {
PRV_FIELD_WRITE(TMC4361A_REFERENCE_CONF, TMC4361A_VIRTUAL_RIGHT_LIMIT_EN_MASK, TMC4361A_VIRTUAL_RIGHT_LIMIT_EN_SHIFT, enable ? 1 : 0);
writeInt(TMC4361A_VIRT_STOP_RIGHT, position);
}
/***********************************************************************************************************************
* POS_CONVERT *
***********************************************************************************************************************/
int32_t TMC4361A::to_motor_acc(int32_t acc) { //
int32_t val = acc / 60.0 * 51200;
return val;
@ -419,7 +383,13 @@ int32_t TMC4361A::to_user_vel(int32_t vel) { //
int32_t val = vel * 60.0 / 51200.0;
return val;
}
uint32_t TMC4361A::haspassedms(uint32_t now, uint32_t last) {
if (now >= last) {
return now - last;
} else {
return 0xFFFFFFFF - last + now;
}
}
#endif
#endif

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

@ -8,7 +8,6 @@
#include "../basic/tmc_ic_interface.hpp"
#include "sdk/os/zos.hpp"
#include "tmc_driver_ic.hpp"
extern "C" {
#include "TMC2160\TMC2160.h"
#include "TMC4361A\TMC4361A.h"
@ -25,18 +24,6 @@ class TMC4361A : public IStepperMotor {
IC_TMC2660,
} driver_ic_type_t;
#if 0
typedef struct {
SPI_HandleTypeDef *spi; //
ZGPIO *csgpio; //
ZGPIO *resetPin; //
ZGPIO *fREEZEPin; //
ZGPIO *ennPin; //
ZGPIO *driverIC_ennPin; //
ZGPIO *driverIC_resetPin; //
} cfg_t;
#endif
class cfg_t {
public:
SPI_HandleTypeDef *spi = nullptr; //
@ -80,96 +67,56 @@ class TMC4361A : public IStepperMotor {
public:
TMC4361A(/* args */);
/*******************************************************************************
* 初始化相关方 *
*******************************************************************************/
/**
* @brief 静态方法，创建默认的TMC4361A配置参数,使用时只需修改自己需要的参数即可
*
* 注意:
* 1. 该方法内部使用的是一个静态变量，所以每次调用该方法时，返回的都是同一个对象的地址
* 2. 该方法返回的数值不需要被释放
* @param config
*/
void initialize(cfg_t *cfg);
void initialize(cfg_t *cfg);
void enableIC(bool enable);
virtual void enable(bool enable) { enableIC(enable); }
/***********************************************************************************************************************
* IStepperMotor *
***********************************************************************************************************************/
/*******************************************************************************
* IStepperMotor impl *
*******************************************************************************/
// virtual void registerListener(MotorEventListener *listener);
virtual void rotate(int32_t velocity);
virtual void moveTo(int32_t position, uint32_t velocityMax);
virtual void moveBy(int32_t relativePosition, uint32_t velocityMax);
virtual void stop();
virtual int32_t getXACTUAL();
virtual int32_t getXTARGET();
virtual void setXACTUAL(int32_t value);
virtual int32_t getVACTUAL();
virtual int32_t getENC_POS();
virtual void setENC_POS(int32_t value);
virtual void setAcceleration(float accelerationpps2); // 设置最大加速度,最大值4000000
virtual void setDeceleration(float accelerationpps2); // 设置最大减速度,最大值4000000
virtual void enable(bool enable);
virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) { driverIC_setIHOLD_IRUN(ihold, irun, iholddelay); }
virtual void setMotorShaft(bool reverse) { driverIC_setMotorShaft(reverse); }
virtual void rotate(int32_t velocity);
virtual void moveTo(int32_t position, uint32_t velocityMax);
virtual void moveBy(int32_t relativePosition, uint32_t velocityMax);
virtual void stop();
//????????????
void setGLOBAL_SCALER(int32_t value);
virtual void setXACTUAL(int32_t value); // 设置当前位置
virtual int32_t getXACTUAL(); // 当前位置
virtual int32_t getVACTUAL(); // 当前速度
virtual void setAcceleration(float accelerationpps2); // 设置最大加速度
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 setScale(int32_t scale);
virtual void setScaleDenominator(int32_t scale);
// Left Virtual Limit Switch XACTUAL ≤ VIRT_STOP_LEFT 时触发
void setLeftVirtualLimitSwitch(bool enable, int32_t position);
virtual bool isReachTarget();
virtual bool isStoped() { return isReachTarget(); }
virtual void setNoAccLimit(bool enable) { return; }; // TODO:impl it
// Right Virtual Limit Switch XACTUAL ≥ VIRT_STOP_RIGHT 时触发
void setRightVirtualLimitSwitch(bool enable, int32_t position);
void setGLOBAL_SCALER(int32_t value);
/*******************************************************************************
* 驱动器初始化方法 *
*******************************************************************************/
/**
* @brief 初始化方
*
* @param channel SPI通道
* @param driver_ic_type 驱动芯片的类
*
* TMC4361A:0x2 DriverIC:0x30
*/
int32_t readICVersion();
uint8_t reset();
uint8_t restore();
/***********************************************************************************************************************
* 4361Opera *
***********************************************************************************************************************/
tmcic_id_t readICVersion();
uint8_t reset();
uint8_t restore();
/*******************************************************************************
* 驱动器寄存器读写方法 *
*******************************************************************************/
void writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4);
void writeInt(uint8_t address, int32_t value);
int32_t readInt(uint8_t address);
void readWriteCover(uint8_t *data, size_t length);
void writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value);
// ????????????
int32_t getENC_POS_DEV(); // ENC_POS和XACTUAL的偏差
uint32_t readEVENTS(); // 读取事件寄存
/*******************************************************
* driverIc function *
*******************************************************/
int32_t readSubICVersion();
virtual bool isReachTarget();
virtual bool isStoped() { return isReachTarget(); }
void tmc4361_writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4);
void tmc4361_writeInt(uint8_t address, int32_t value);
int32_t tmc4361_readInt(uint8_t address);
void tmc4361_readWriteCover(uint8_t *data, size_t length);
void tmc4361_writeSubRegister(uint8_t address, uint32_t mask, uint32_t shift, uint32_t value);
public:
// only used in tmc4361A.cpp
void tmc4361AConfigCallback(ConfigState state);
void readWriteArray(uint8_t *data, size_t length);
private:
uint32_t haspassedms(uint32_t now, uint32_t last);
void tmc4361_readWriteArray(uint8_t *data, size_t length);
public:
void driverIC_reset();
void driverIC_enableIC(bool enable);
void driverIC_setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay);
@ -180,12 +127,12 @@ class TMC4361A : public IStepperMotor {
uint32_t driverIC_readICVersion();
private:
int32_t to_motor_acc(int32_t acc); // rpm/s^2
int32_t to_motor_vel(int32_t vel); // rpm
int32_t to_motor_pos(int32_t pos); //
int32_t to_user_pos(int32_t pos); //
int32_t to_user_vel(int32_t vel);
int32_t to_motor_acc(int32_t acc /* rpm/s^2*/);
int32_t to_motor_vel(int32_t vel /*rpm*/);
int32_t to_motor_pos(int32_t pos);
int32_t to_user_pos(int32_t pos);
int32_t to_user_vel(int32_t vel);
uint32_t haspassedms(uint32_t now, uint32_t last);
};
} // namespace iflytop

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

@ -13,26 +13,10 @@ extern "C" {
#ifdef HAL_SPI_MODULE_ENABLED
namespace iflytop {
#define TMC5130_LISTENER_MAX 5
/**
* @brief
*
* 注意事项：
* 1. 速度单位
* velocity的单位是:p/t
* t = 2^24/fin (fin为外接时钟，当时钟频率为16MHz时，t = 1.048576s)
* 此时 ppt约等于pps
*
* 参考TMC5130A_datasheet_rev1.20%20(1)%20(3).pdf 38页 蓝框中的字
*
* 2. 芯片复位方法
* reset(); 复位芯片，同时会将芯片的寄存器设置为默认值。一般情况下不需要用户主动调用该方法，因为在初始化时会自动调用该方法。
*
*/
class Tmc5130RampStat {
public:
uint32_t m_state;
// 参考TMC5130A_datasheet_rev1.20%20(1).pdf 40页
typedef enum {
/**
* @brief
@ -116,9 +100,6 @@ class TMC5130 : public IStepperMotor {
virtual int32_t getVACTUAL(); // 读取电机当前位置,与编码器的位置值不同，该值是电机在不丢步的情况下的位置
uint32_t readXTARGET(); // 读取驱动器目标位置
virtual int32_t getENC_POS() { return 0; } // TODO impl it
virtual void setENC_POS(int32_t value) {} // TODO impl it
virtual void setScale(int32_t scale);
virtual void setScaleDenominator(int32_t scale);

Write Preview
Loading…
Cancel
Save