From 09a2e4d56041cc988ddb766185f323ede4d713b4 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Tue, 10 Sep 2024 19:08:18 +0800 Subject: [PATCH] tmc51x0 add refreshcfg --- tmcdriver/tmc/tmc_reg_cache.hpp | 83 +++++++++++++++++++++++++++++++++++++++++ tmcdriver/tmc51x0/tmc51x0.cpp | 46 +++++++++++++++++++---- tmcdriver/tmc51x0/tmc51x0.hpp | 6 ++- 3 files changed, 126 insertions(+), 9 deletions(-) create mode 100644 tmcdriver/tmc/tmc_reg_cache.hpp diff --git a/tmcdriver/tmc/tmc_reg_cache.hpp b/tmcdriver/tmc/tmc_reg_cache.hpp new file mode 100644 index 0000000..8517808 --- /dev/null +++ b/tmcdriver/tmc/tmc_reg_cache.hpp @@ -0,0 +1,83 @@ +#pragma once +#include +#include +#include +#include +#include + +#include + +// +namespace iflytop { +using namespace std; +class TMCReg { + private: + /* data */ + public: + uint8_t flag = 0; + uint8_t reg = 0; + uint8_t val = 0; + + void setActiveFlag() { flag = flag | 0x01; } + bool isActive() { return flag & 0x01; } + + void setUpdateFlag() { flag = flag | 0x02; } + bool isUpdate() { return flag & 0x02; } +}; + +class TMCRegCache { + public: + TMCReg cache[50]; + + public: + void foreach (function fn) { + for (size_t i = 0; i < sizeof(cache) / sizeof(cache[0]); i++) { + if (cache[i].isActive() && cache[i].isUpdate()) { + fn(&cache[i]); + } + } + } + + bool activeReg(uint8_t reg) { + TMCReg* item = findReg(reg); + if (item) return true; + + item = findPassive(); + if (item) { + item->reg = reg; + item->setActiveFlag(); + return true; + } + + return false; + } + + void setReg(uint8_t reg, uint8_t val) { + TMCReg* item = findReg(reg); + if (item) { + item->val = val; + item->setUpdateFlag(); + } + } + + private: + TMCReg* findPassive() { + for (size_t i = 0; i < sizeof(cache) / sizeof(cache[0]); i++) { + if (!cache[i].isActive()) { + return &cache[i]; + } + } + return nullptr; + } + + TMCReg* findReg(uint8_t reg) { + for (size_t i = 0; i < sizeof(cache) / sizeof(cache[0]); i++) { + if (cache[i].isActive() && cache[i].reg == reg) { + return &cache[i]; + } + } + return nullptr; + } +}; + +} // namespace iflytop \ No newline at end of file diff --git a/tmcdriver/tmc51x0/tmc51x0.cpp b/tmcdriver/tmc51x0/tmc51x0.cpp index b230387..6041775 100644 --- a/tmcdriver/tmc51x0/tmc51x0.cpp +++ b/tmcdriver/tmc51x0/tmc51x0.cpp @@ -28,7 +28,9 @@ static const uint8_t tmc5130_defaultRegisterAccess[TMC5130_REGISTER_COUNT] = { 0x42, 0x01, 0x02, 0x01, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____ // 0x70 - 0x7F }; #endif - +#define TMC5160_GLOBAL_SCALER 0x0B +#define TMC5160_GLOBAL_SCALER_MASK 0xFF +#define TMC5160_GLOBAL_SCALER_SHIFT 0 /*********************************************************************************************************************** * 工具�? * ***********************************************************************************************************************/ @@ -57,6 +59,25 @@ void TMC51X0::initialize(int mid, TMC51X0Cfg cfg) { 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); @@ -67,11 +88,11 @@ void TMC51X0::initialize(int mid, TMC51X0Cfg cfg) { writeInt(TMC5130_TZEROWAIT, 0); setVstart(100); - setA1(30); - setAmax(50); + setA1(15); + setAmax(15); setV1(500); - setDmax(50); - setD1(30); + setDmax(15); + setD1(15); setVstop(100); setTzerowait(100); @@ -79,6 +100,13 @@ void TMC51X0::initialize(int mid, TMC51X0Cfg cfg) { 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); + }); +} + /******************************************************************************* * 驱动器寄存器读写方法 * *******************************************************************************/ @@ -95,6 +123,7 @@ void TMC51X0::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, 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)); } @@ -206,6 +235,10 @@ void TMC51X0::moveTo(int32_t position, uint32_t velocityMax) { 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)); @@ -321,9 +354,6 @@ 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)); } void TMC51X0::setGlobalScale(uint8_t globalscale) { -#define TMC5160_GLOBAL_SCALER 0x0B -#define TMC5160_GLOBAL_SCALER_MASK 0xFF -#define TMC5160_GLOBAL_SCALER_SHIFT 0 zlock_guard lkg(m_mutex); if (isTMC5130()) { return; diff --git a/tmcdriver/tmc51x0/tmc51x0.hpp b/tmcdriver/tmc51x0/tmc51x0.hpp index 34dab70..02aefe2 100644 --- a/tmcdriver/tmc51x0/tmc51x0.hpp +++ b/tmcdriver/tmc51x0/tmc51x0.hpp @@ -7,6 +7,7 @@ // #include "../tmc/tmc_type.h" +#include "../tmc/tmc_reg_cache.hpp" #include "reg/TMC5130_Type.h" #include "stm32basic/stm32basic.hpp" namespace iflytop { @@ -24,9 +25,11 @@ class TMC51X0Cfg { TMC51X0Cfg() {} }; + class TMC51X0 { protected: - TMC51X0Cfg m_cfg; + TMC51X0Cfg m_cfg; + TMCRegCache m_cache; SPI_HandleTypeDef *m_hspi = NULL; ZGPIO m_csnpin; @@ -56,6 +59,7 @@ class TMC51X0 { public: bool ping(); void enable(bool enable); + void refreshcfg(); void setdriErr(bool val) { m_driErr = val; } bool getdriErr() { return m_driErr; }