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

update ztmc4361

master
zhaohe 2 years ago
parent
a39ee1ce5a
commit
bfc177e91c
6 changed files with 44 additions and 868 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. 76
      components/tmc/ic/old/ztmc2160.cpp
  2. 46
      components/tmc/ic/old/ztmc2160.hpp
  3. 526
      components/tmc/ic/old/ztmc4361A.cpp
  4. 194
      components/tmc/ic/old/ztmc4361A.hpp
  5. 23
      components/tmc/ic/ztmc4361A.cpp
  6. 47
      components/tmc/ic/ztmc4361A.hpp

76
components/tmc/ic/old/ztmc2160.cpp
View File

@ -1,76 +0,0 @@
#if 0
#include "ztmc2160.hpp"
#include "TMC2160/TMC2160.h"
using namespace iflytop;
#define ZFIELD_READ(address, mask, shift) FIELD_GET(readInt(address), mask, shift)
#define ZFIELD_WRITE(address, mask, shift, value) (writeInt(address, FIELD_SET(readInt(address), mask, shift, value)))
void TMC2160::initialize(uint8_t channel, TMCDriverICPort *port) {
m_channel = channel;
m_port = port;
}
void TMC2160::setMotorShaft(bool reverse) { //
int32_t val = reverse ? 1 : 0;
ZFIELD_WRITE(TMC2160_GCONF, TMC2160_SHAFT_MASK, TMC2160_SHAFT_SHIFT, val);
}
uint8_t TMC2160::reset() {
m_port->TMCDriverICPort_setResetPinState(m_channel, false);
m_port->TMCDriverICPort_sleepus(1000);
m_port->TMCDriverICPort_setResetPinState(m_channel, true);
// Reset the dirty bits
int index = 0;
while (true) {
while ((index < TMC2160_REGISTER_COUNT) && !TMC_IS_RESTORABLE(tmc2160_defaultRegisterAccess[index])) {
index++;
}
if (index >= TMC2160_REGISTER_COUNT) {
break;
}
// printf("Resetting register %d\n", index);
writeInt(index, tmc2160_defaultRegisterResetState[index]);
index++;
}
writeInt(TMC2160_PWMCONF, 0xC40C001E);
writeInt(TMC2160_DRV_CONF, 0x00080400);
// tmc2160_writeInt(tmc2160, TMC2160_PWMCONF, 0x000504C8);
return true;
}
uint8_t TMC2160::restore() { return 1; }
void TMC2160::enableIC(bool enable) { m_port->TMCDriverICPort_setENNPinState(m_channel, !enable); }
void TMC2160::setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) {
writeInt(TMC2160_IHOLD_IRUN, (iholddelay << TMC2160_IHOLDDELAY_SHIFT) | (irun << TMC2160_IRUN_SHIFT) | (ihold << TMC2160_IHOLD_SHIFT));
}
void TMC2160::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};
m_port->TMCDriverICPort_readWriteArray(m_channel, &data[0], 5);
// int value = ((uint32_t)x1 << 24) | ((uint32_t)x2 << 16) | (x3 << 8) | x4;
}
void TMC2160::writeInt(uint8_t address, int32_t value) { //
writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0));
}
int32_t TMC2160::readInt(uint8_t address) {
address = TMC_ADDRESS(address);
// register not readable -> shadow register copy
if (!TMC_IS_READABLE(tmc2160_defaultRegisterAccess[address])) return 0;
uint8_t data[5];
data[0] = address;
m_port->TMCDriverICPort_readWriteArray(m_channel, &data[0], 5);
data[0] = address;
m_port->TMCDriverICPort_readWriteArray(m_channel, &data[0], 5);
return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4];
}
int32_t TMC2160::readICVersion() {
int32_t value = readInt(TMC2160_IOIN___OUTPUT);
return (value & TMC2160_VERSION_MASK) >> TMC2160_VERSION_SHIFT;
}
#endif

46
components/tmc/ic/old/ztmc2160.hpp
View File

@ -1,46 +0,0 @@
#if 0
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
extern "C" {
#include "TMC2160/TMC2160.h"
#include "tmc_driver_ic.hpp"
}
#include "../basic/tmc_ic_interface.hpp"
#include "sdk/hal/zhal.hpp"
namespace iflytop {
class TMC2160 : public TMCDriverICInterface {
protected:
TMC2160TypeDef m_tmc_handler;
uint8_t m_channel;
TMCDriverICPort* m_port;
public:
TMC2160(/* args */){};
virtual ~TMC2160(){};
virtual void initialize(uint8_t channel, TMCDriverICPort* port);
virtual uint8_t reset();
virtual uint8_t restore();
virtual void enableIC(bool enable);
virtual void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay);
// register access
virtual void writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4);
virtual void writeInt(uint8_t address, int32_t value);
virtual int32_t readInt(uint8_t address);
/**
* @brief 读取芯片版本
*
* @return int32_t 默认为0x30
*/
virtual int32_t readICVersion();
virtual void setMotorShaft(bool reverse);
public:
};
} // namespace iflytop
#endif

526
components/tmc/ic/old/ztmc4361A.cpp
View File

@ -1,526 +0,0 @@
#if 0
#include <stdarg.h>
#include "../basic/basic.hpp"
#include "./TMC4361A/TMC4361A.h"
#include "ztmc4361A.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)
#if 1
void TMC4361A::readWriteArray(uint8_t *data, size_t length) { m_config.m_port->TMC4361APort_readWriteArray(m_channel, data, length); }
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) {
CriticalContext cc;
int value;
uint8_t data[5];
address = TMC_ADDRESS(address);
if (!TMC_IS_READABLE(m_registerAccessTable[address])) return m_defaultRegisterResetState[address];
data[0] = address;
readWriteArray(&data[0], 5);
data[0] = address;
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) {
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);
m_status = data[0];
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);
shadowRegister[address] = value;
}
void TMC4361A::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];
int32_t old_low = shadowRegister[TMC4361A_COVER_LOW_WR];
// Check if datagram length is valid
if (length == 0 || length > 8) return;
uint8_t bytes[8] = {0};
uint32_t tmp;
size_t i;
// Copy data into buffer of maximum cover datagram length (8 bytes)
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]);
m_config.m_port->TMC4361APort_sleepus(10 * 1000);
// Read the reply
if (length > 4) {
tmp = 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);
bytes[0] = BYTE(tmp, 0);
bytes[1] = BYTE(tmp, 1);
bytes[2] = BYTE(tmp, 2);
bytes[3] = BYTE(tmp, 3);
// Write the reply to the data array
for (i = 0; i < length; i++) {
data[length - i - 1] = bytes[i];
}
// Rewriting old values to prevent interrupting manual covering. Imitating unchanged values and state.
writeInt(TMC4361A_COVER_HIGH_WR, old_high);
shadowRegister[TMC4361A_COVER_LOW_WR] = old_low;
}
TMC4361A::TMC4361A(/* args */) {
m_channel = 0;
m_driver_ic_type = IC_TMC2130;
memset(&m_config, 0, sizeof(m_config));
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) { //
PRV_FIELD_WRITE(address, mask, shift, value);
}
void TMC4361A::setAcceleration(float accelerationpps2) {
/**
* @brief
* TMC4361A_AMAX:有两种模式，这里使用的是频率模式
*
* Frequency mode: [pulses per sec2]
* 22 digits and 2 decimal places: 250 mpps^2 ≤ AMAX ≤ 4 Mpps^2
* Direct mode: [∆v per clk cycle]
* a[∆v per clk_cycle]= AMAX / 2^37
* AMAX [pps2] = AMAX / 237 • fCLK^2
*/
int32_t acc = (int32_t)accelerationpps2;
writeInt(TMC4361A_AMAX, acc << 2);
}
void TMC4361A::setDeceleration(float accelerationpps2) {
/**
* @brief
* TMC4361A_DMAX:有两种模式，这里使用的是频率模式
*
* Frequency mode: [pulses per sec2]
* 22 digits and 2 decimal places: 250 mpps^2 ≤ AMAX ≤ 4 Mpps^2
* Direct mode: [∆v per clk cycle]
* a[∆v per clk_cycle]= AMAX / 2^37
* AMAX [pps2] = AMAX / 237 • fCLK^2
*/
int32_t acc = (int32_t)accelerationpps2;
writeInt(TMC4361A_DMAX, acc << 2);
}
void TMC4361A::initialize(uint8_t channel, driver_ic_type_t driver_ic_type, TMC4361AConfig_t *config) {
m_channel = channel;
m_port = config->m_port;
m_driver_ic_type = driver_ic_type;
m_registerAccessTable = &tmc4361A_defaultRegisterAccess[0];
m_defaultRegisterResetState = &tmc4361A_defaultRegisterResetState[0];
memset(shadowRegister, 0, sizeof(shadowRegister));
memcpy(&m_config, config, sizeof(TMC4361AConfig_t));
m_config.m_port->TMC4361APort_setResetPinState(m_channel, true);
m_config.m_port->TMC4361APort_setFREEZEPinState(m_channel, true);
m_config.m_port->TMC4361APort_setENNPinState(m_channel, true);
switch (m_driver_ic_type) {
case IC_TMC2160:
case IC_TMC2130:
m_driverIC = &m_tmc2160;
break;
default:
break;
}
TMC_ASSERT(m_driverIC != NULL, "TMC4361A::initialize() m_driverIC is NULL");
reset();
m_driverIC->initialize(0, this);
m_driverIC->reset();
baseInit(&config->base_config);
encoder_init(&config->encoder_config);
close_loop_init(&config->close_loop_config);
setAcceleration(100000);
setDeceleration(100000);
enableIC(true);
m_port->TMC4361APort_sleepus(300 * 1000);
if (m_config.close_loop_config.enable_closed_loop) calibrateClosed();
m_port->TMC4361APort_sleepus(300 * 1000);
getDriverIC()->setIHOLD_IRUN(1, 3, 0); // 注意要先设置IHOLD再设置IRUN,否则电机跑不起来
}
uint8_t TMC4361A::reset() {
// Pulse the low-active hardware reset pin
stop();
m_config.m_port->TMC4361APort_setResetPinState(m_channel, false);
m_config.m_port->TMC4361APort_sleepus(1000);
m_config.m_port->TMC4361APort_setResetPinState(m_channel, true);
/**
* @brief 重置芯片镜像寄存器
*
*/
for (uint32_t add = 0; add < TMC4361A_REGISTER_COUNT; add++) {
if (!TMC_IS_RESETTABLE(m_registerAccessTable[add])) {
continue;
}
writeInt(add, m_defaultRegisterResetState[add]);
}
uint8_t driver, dataLength;
uint32_t value;
// Setup SPI
switch (m_driver_ic_type) {
case IC_TMC2130:
case IC_TMC2160:
driver = 0x0C;
dataLength = 0;
break;
case IC_TMC2660:
driver = 0x0B;
dataLength = 0;
break;
default:
driver = 0x0F;
dataLength = 40;
break;
}
value = 0x44400040 | (dataLength << 13) | (driver << 0);
writeInt(TMC4361A_SPIOUT_CONF, value);
writeInt(TMC4361A_CURRENT_CONF, 0x00000003);
writeInt(TMC4361A_SCALE_VALUES, 0x00000000);
return 1;
}
uint8_t TMC4361A::restore() { return 1; }
int32_t TMC4361A::getXACTUAL() { return readInt(TMC4361A_XACTUAL); }
void TMC4361A::setXACTUAL(int32_t value) { writeInt(TMC4361A_XACTUAL, value); }
int32_t TMC4361A::getVACTUAL() { return readInt(TMC4361A_VACTUAL); }
int32_t TMC4361A::getXTARGET() { return readInt(TMC4361A_X_TARGET); }
int32_t TMC4361A::getENC_POS() { return readInt(TMC4361A_ENC_POS); }
void TMC4361A::setENC_POS(int32_t value) { writeInt(TMC4361A_ENC_POS, value); }
int32_t TMC4361A::getENC_POS_DEV() { return readInt(TMC4361A_ENC_POS_DEV_RD); }
void TMC4361A::enableIC(bool enable) {
m_config.m_port->TMC4361APort_setENNPinState(m_channel, !enable);
m_driverIC->enableIC(enable);
}
int32_t tmc4361A_discardVelocityDecimals(int32_t value) {
if (abs(value) > 8000000) {
value = (value < 0) ? -8000000 : 8000000;
}
return value << 8;
}
void TMC4361A::rotate(int32_t velocity) {
PRV_FIELD_WRITE(TMC4361A_RAMPMODE, TMC4361A_OPERATION_MODE_MASK, TMC4361A_OPERATION_MODE_SHIFT, 0);
writeInt(TMC4361A_VMAX, tmc4361A_discardVelocityDecimals(velocity));
}
void TMC4361A::stop() { rotate(0); }
void TMC4361A::moveTo(int32_t position, uint32_t velocityMax) {
m_reachtarget = false;
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);
}
void TMC4361A::moveBy(int32_t relativePosition, uint32_t velocityMax) {
m_reachtarget = false;
relativePosition += 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;
}
/**
* @brief TODO:添加闭环初始化逻辑
*/
void TMC4361A::baseInit(base_config_t *config) { //
writeSubRegister(TMC4361A_STEP_CONF, TMC4361A_FS_PER_REV_MASK, TMC4361A_FS_PER_REV_SHIFT, config->fs_per_rev);
writeSubRegister(TMC4361A_STEP_CONF, TMC4361A_MSTEP_PER_FS_MASK, TMC4361A_MSTEP_PER_FS_SHIFT, config->mstep_per_fs);
/**
* @brief clear XACTUAL and ENC_POS
*/
writeInt(TMC4361A_XACTUAL, 0);
writeInt(TMC4361A_ENC_POS, 0);
}
void TMC4361A::close_loop_init(close_loop_config_t *config) {
if (!config->enable_closed_loop) {
return;
}
writeInt(TMC4361A_CL_VMIN_EMF_WR, config->gamma_v_min); /*mark*/
writeInt(TMC4361A_CL_VADD_EMF, config->gamma_v_add); /*mark*/
writeInt(TMC4361A_CL_BETA, (config->gamma << 16) | config->beta); /*mark*/
writeInt(TMC4361A_CL_OFFSET, config->offset);
writeInt(TMC4361A_CL_VMAX_CALC_P_WR, config->correction_velocity_p); /*mark*/
writeInt(TMC4361A_CL_VMAX_CALC_I_WR, config->correction_velocity_i); /*mark*/
writeDatagram(TMC4361A_PID_I_CLIP_WR, 0, config->correction_velocity_d_clk, config->correction_velocity_i_clip >> 8,
config->correction_velocity_i_clip & 0xFF);
writeDatagram(TMC4361A_PID_I_CLIP_WR, 0, config->correction_velocity_d_clk, config->correction_velocity_i_clip >> 8,
config->correction_velocity_i_clip & 0xFF);
writeInt(TMC4361A_PID_DV_CLIP_WR, config->correction_velocity_d_clip); /*mark*/
writeSubRegister(TMC4361A_SCALE_VALUES, TMC4361A_CL_IMIN_MASK, TMC4361A_CL_IMIN_SHIFT, config->current_scaler_minimum); /*mark*/
writeSubRegister(TMC4361A_SCALE_VALUES, TMC4361A_CL_IMAX_MASK, TMC4361A_CL_IMAX_SHIFT, config->current_scaler_maximum); /*mark*/
writeSubRegister(TMC4361A_SCALE_VALUES, TMC4361A_CL_START_UP_MASK, TMC4361A_CL_START_UP_SHIFT, config->current_scaler_start_up); /*mark*/
writeInt(TMC4361A_CL_UPSCALE_DELAY, config->upscale_delay); /*mark*/
writeInt(TMC4361A_CL_DOWNSCALE_DELAY, config->downscale_delay); /*mark*/
writeInt(TMC4361A_CL_DELTA_P_WR, config->position_correction_p); /*mark*/
writeInt(TMC4361A_CL_TOLERANCE_WR, config->position_correction_tolerance); /*mark*/
writeInt(TMC4361A_CL_TR_TOLERANCE_WR, config->position_window); /*mark*/
writeDatagram(TMC4361A_ENC_VMEAN_WAIT_WR, config->enc_v_mean_int >> 8, config->enc_v_mean_int & 0xFF, config->enc_v_mean_filter, config->enc_v_mean_wait);
writeDatagram(TMC4361A_ENC_COMP_XOFFSET, 0, config->encoder_correction_y_offset, 0, 0);
PRV_FIELD_WRITE(TMC4361A_ENC_IN_CONF, TMC4361A_CL_VELOCITY_MODE_EN_MASK, TMC4361A_CL_VELOCITY_MODE_EN_SHIFT, 1);
}
uint8_t TMC4361A::tmc4361A_calibrateClosedLoop(int &state, uint32_t &oldRamp, uint8_t worker0master1) {
uint32_t amax = 0;
uint32_t dmax = 0;
if (worker0master1 && state == 0) state = 1;
switch (state) {
case 1:
amax = readInt(TMC4361A_AMAX);
dmax = readInt(TMC4361A_DMAX);
// Set ramp and motion parameters
oldRamp = readInt(TMC4361A_RAMPMODE);
writeInt(TMC4361A_RAMPMODE, TMC4361A_RAMP_POSITION | TMC4361A_RAMP_HOLD);
writeInt(TMC4361A_AMAX, MAX(amax, 1000));
writeInt(TMC4361A_DMAX, MAX(dmax, 1000));
writeInt(TMC4361A_VMAX, 0);
state = 2;
break;
case 2:
// Clear encoder calibration bit
PRV_FIELD_WRITE(TMC4361A_ENC_IN_CONF, TMC4361A_CL_CALIBRATION_EN_MASK, TMC4361A_CL_CALIBRATION_EN_SHIFT, 0);
// Disable internal data regulation for closed loop operation in encoder config
PRV_FIELD_WRITE(TMC4361A_ENC_IN_CONF, TMC4361A_REGULATION_MODUS_MASK, TMC4361A_REGULATION_MODUS_SHIFT, 1);
if (tmc4361A_moveToNextFullstep()) // move to next fullstep, motor must be stopped, poll until finished
state = 3;
break;
case 3:
// Start encoder calibration
PRV_FIELD_WRITE(TMC4361A_ENC_IN_CONF, TMC4361A_CL_CALIBRATION_EN_MASK, TMC4361A_CL_CALIBRATION_EN_SHIFT, 1);
state = 4;
break;
case 4:
if (worker0master1) break;
// Stop encoder calibration
PRV_FIELD_WRITE(TMC4361A_ENC_IN_CONF, TMC4361A_CL_CALIBRATION_EN_MASK, TMC4361A_CL_CALIBRATION_EN_SHIFT, 0);
// Enable closed loop in encoder config
PRV_FIELD_WRITE(TMC4361A_ENC_IN_CONF, TMC4361A_REGULATION_MODUS_MASK, TMC4361A_REGULATION_MODUS_SHIFT, 1);
// Restore old ramp mode, enable position mode
writeInt(TMC4361A_RAMPMODE, TMC4361A_RAMP_POSITION | oldRamp);
state = 5;
break;
case 5:
state = 0;
return 1;
break;
default:
break;
}
return 0;
}
uint8_t TMC4361A::tmc4361A_moveToNextFullstep() {
int32_t stepCount;
// Motor must be stopped
if (readInt(TMC4361A_VACTUAL) != 0) {
// Not stopped
return 0;
}
// Position mode, hold mode, low velocity
writeInt(TMC4361A_RAMPMODE, 4);
writeInt(TMC4361A_VMAX, 10000 << 8);
// Current step count
stepCount = PRV_FIELD_READ(TMC4361A_MSCNT_RD, TMC4361A_MSCNT_MASK, TMC4361A_MSCNT_SHIFT);
// Get microstep value of step count (lowest 8 bits)
stepCount = stepCount % 256;
// Assume: 256 microsteps -> Fullsteps are at 128 + n*256
stepCount = 128 - stepCount;
if (stepCount == 0) {
// Fullstep reached
return 1;
}
// Fullstep not reached -> calculate next fullstep position
stepCount += readInt(TMC4361A_XACTUAL);
// Move to next fullstep position
writeInt(TMC4361A_X_TARGET, stepCount);
return 0;
}
void TMC4361A::calibrateClosed() {
/**
* @brief
* 这个方法必须在电机闭环运动前调用，并且保证电机当前没有运行
*/
int state = 0;
uint32_t oldRamp = 0;
tmc4361A_calibrateClosedLoop(state, oldRamp, 1);
int calibrateClosedLoopTimes = 0;
while (!tmc4361A_calibrateClosedLoop(state, oldRamp, 0)) {
for (size_t i = 0; i < 10; i++) {
m_config.m_port->TMC4361APort_sleepus(1000);
}
calibrateClosedLoopTimes++;
if (calibrateClosedLoopTimes > 100) {
/**
* @brief TODO:添加错误处理
*/
return;
}
}
return;
}
void TMC4361A::encoder_init(encoder_config_t *config) {
if (config->encoder_mode == ENCODER_MODE_INCREMENTAL) {
writeSubRegister(TMC4361A_GENERAL_CONF, TMC4361A_SERIAL_ENC_IN_MODE_MASK, TMC4361A_SERIAL_ENC_IN_MODE_SHIFT, 0);
} else {
// Not support now
TMC_ASSERT(false, "Not support now");
return;
}
writeSubRegister(TMC4361A_GENERAL_CONF, TMC4361A_DIFF_ENC_IN_DISABLE_MASK, TMC4361A_DIFF_ENC_IN_DISABLE_SHIFT, config->diff_enc_in_disable);
writeSubRegister(TMC4361A_ENC_IN_CONF, TMC4361A_INVERT_ENC_DIR_MASK, TMC4361A_INVERT_ENC_DIR_SHIFT, config->invert_enc_dir);
writeSubRegister(TMC4361A_ENC_IN_RES_WR, TMC4361A_ENC_IN_RES_MASK, TMC4361A_ENC_IN_RES_SHIFT, config->enc_in_res);
}
TMC4361A::TMC4361AConfig_t *TMC4361A::createDeafultTMC4361AConfig(TMC4361APort *m_port) {
// base config
static TMC4361A::TMC4361AConfig_t _config;
TMC4361A::TMC4361AConfig_t *config = &_config;
memset(config, 0, sizeof(TMC4361A::TMC4361AConfig_t));
config->base_config.fs_per_rev = 200;
config->base_config.mstep_per_fs = 0;
// encoder config
config->encoder_config.enable_encoder = true;
config->encoder_config.encoder_mode = TMC4361A::ENCODER_MODE_INCREMENTAL;
config->encoder_config.diff_enc_in_disable = false;
config->encoder_config.invert_enc_dir = false;
config->encoder_config.enc_in_res = 4000;
// close loop config
config->close_loop_config.enable_closed_loop = true;
config->close_loop_config.gamma_v_min = 300000;
config->close_loop_config.gamma_v_add = 600000;
config->close_loop_config.gamma = 255;
config->close_loop_config.beta = 256;
config->close_loop_config.offset = 0;
config->close_loop_config.current_scaler_minimum = 100;
config->close_loop_config.current_scaler_maximum = 240;
config->close_loop_config.current_scaler_start_up = 0;
config->close_loop_config.upscale_delay = 1000;
config->close_loop_config.downscale_delay = 10000;
config->close_loop_config.correction_velocity_p = 10000;
config->close_loop_config.correction_velocity_i = 20;
config->close_loop_config.correction_velocity_i_clip = 10;
config->close_loop_config.correction_velocity_d_clk = 0;
config->close_loop_config.correction_velocity_d_clip = 1073741823;
config->close_loop_config.position_correction_p = 65536;
config->close_loop_config.position_correction_tolerance = 255;
config->close_loop_config.position_window = 255;
config->close_loop_config.enc_v_mean_wait = 0;
config->close_loop_config.enc_v_mean_filter = 7;
config->close_loop_config.enc_v_mean_int = 500;
config->close_loop_config.encoder_correction_y_offset = 0;
config->autoReadEventRegister = true;
config->autoReadEventRegisterInterval = 33;
config->m_port = m_port;
return config;
}
TMCDriverICInterface *TMC4361A::getDriverIC() { return m_driverIC; }
int32_t TMC4361A::readSubICVersion() { return m_driverIC->readICVersion(); }
void TMC4361A::TMCDriverICPort_setResetPinState(uint16_t __, bool state) {
// m_port->TMC4361APort_setResetPinState(channel, state);
}
void TMC4361A::TMCDriverICPort_setENNPinState(uint16_t __, bool state) { m_port->TMC4361APort_setSubICENNPinState(m_channel, state); }
void TMC4361A::TMCDriverICPort_sleepus(int32_t us) { m_port->TMC4361APort_sleepus(us); }
void TMC4361A::TMCDriverICPort_readWriteArray(uint16_t __, uint8_t *data, size_t length) { readWriteCover(data, 5); }
/*******************************************************************************
* 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);
if ((value & TMC4361A_TARGET_REACHED_F_MASK) > 0) {
return true;
} else {
return false;
}
}
void TMC4361A::periodicJob(uint32_t now) {}
#endif
#endif

194
components/tmc/ic/old/ztmc4361A.hpp
View File

@ -1,194 +0,0 @@
#pragma once
#if 0
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include "../basic/tmc_ic_interface.hpp"
#include "sdk/hal/zhal.hpp"
#include "tmc_driver_ic.hpp"
#include "ztmc2160.hpp"
extern "C" {
#include "TMC2160\TMC2160.h"
#include "TMC4361A\TMC4361A.h"
}
namespace iflytop {
#define TMC4361A_LISTENER_MAX 5
class TMC4361APort {
public:
/**
* @brief 复位引脚，低有效。
*
* @param state
*/
virtual void TMC4361APort_setResetPinState(uint16_t channel, bool state) = 0;
/**
* @brief 制动引脚，拉低后设备制动，按照用户指定的斜坡进行制动。
* 参考手册:TMC4361A_datasheet_rev1.26_01.pdf:127
* @param state
*/
virtual void TMC4361APort_setFREEZEPinState(uint16_t channel, bool state) = 0;
/**
* @brief 使能引脚，低有效。
*
* @param state
*/
virtual void TMC4361APort_setENNPinState(uint16_t channel, bool state) = 0;
/**
* @brief 目标到达引脚，低有效。
*
* @return true
* @return false
*/
virtual bool TMC4361APort_getTargetReachedPinState(uint16_t channel) = 0;
/**
* @brief us级别延时函数。
*
* @param us
*/
virtual void TMC4361APort_sleepus(int32_t us) = 0;
/**
* @brief SPI读写接口
*
* @param data
* @param length
*/
virtual void TMC4361APort_readWriteArray(uint16_t channel, uint8_t *data, size_t length) = 0;
/*********************************************************
* 子芯片 *
*********************************************************/
/**
* @brief 使能引脚，低有效。
*
* @param state
*/
virtual void TMC4361APort_setSubICENNPinState(uint16_t channel, bool state) = 0;
};
class TMC4361A : public IStepperMotor {
public:
typedef enum {
IC_TMC2130 = 0, //
IC_TMC2160,
IC_TMC2660,
} driver_ic_type_t;
public:
protected:
TMC2160 m_tmc2160;
// TMC4361ATypeDef m_tmc4361A;
int32_t shadowRegister[TMC_REGISTER_COUNT];
const uint8_t *m_registerAccessTable;
const int32_t *m_defaultRegisterResetState;
TMCDriverICInterface *m_driverIC;
uint8_t m_channel;
driver_ic_type_t m_driver_ic_type;
TMC4361AConfig_t m_config;
uint32_t lastCallPeriodicJobTick;
// TMC4361APort *m_port;
uint8_t m_status;
bool m_reachtarget;
public:
TMC4361A(/* args */);
/*******************************************************************************
* 初始化相关方法 *
*******************************************************************************/
/**
* @brief 静态方法，创建默认的TMC4361A配置参数,使用时只需修改自己需要的参数即可
*
* 注意:
* 1. 该方法内部使用的是一个静态变量，所以每次调用该方法时，返回的都是同一个对象的地址。
* 2. 该方法返回的数值不需要被释放。
* @param config
*/
static TMC4361AConfig_t *createDeafultTMC4361AConfig(TMC4361APort *m_port);
void initialize(uint8_t channel, driver_ic_type_t driver_ic_type, TMC4361AConfig_t *config);
void periodicJob(uint32_t ticket);
uint8_t getChannel() { return m_channel; }
void enableIC(bool enable);
/*******************************************************************************
* 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); // 设置最大加速度
virtual void setDeceleration(float accelerationpps2); // 设置最大减速度
/*******************************************************************************
* 驱动器初始化方法 *
*******************************************************************************/
/**
* @brief 初始化方法
*
* @param channel SPI通道号
* @param driver_ic_type 驱动芯片的类型
*/
int32_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();
TMCDriverICInterface *getDriverIC();
virtual void TMCDriverICPort_setResetPinState(uint16_t channel, bool state);
virtual void TMCDriverICPort_setENNPinState(uint16_t channel, bool state);
virtual void TMCDriverICPort_sleepus(int32_t us);
virtual void TMCDriverICPort_readWriteArray(uint16_t channel, uint8_t *data, size_t length);
virtual bool isReachTarget();
public:
// only used in tmc4361A.cpp
void tmc4361AConfigCallback(ConfigState state);
void readWriteArray(uint8_t *data, size_t length);
private:
uint8_t tmc4361A_calibrateClosedLoop(int &state, uint32_t &oldRamp, uint8_t worker0master1);
uint8_t tmc4361A_moveToNextFullstep();
void baseInit(base_config_t *baseconfig);
void close_loop_init(close_loop_config_t *config);
void encoder_init(encoder_config_t *config);
void calibrateClosed();
uint32_t haspassedms(uint32_t now, uint32_t last);
// void callOnEventCallback(StepperMotorEvent event);
};
} // namespace iflytop
#endif

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

@ -159,15 +159,22 @@ void TMC4361A::setDeceleration(float accelerationpps2) {
int32_t acc = (int32_t)accelerationpps2;
writeInt(TMC4361A_DMAX, acc << 2);
}
#define INIT_GPIO(m_pin, pin, ...) \
if (pin != PinNull) { \
m_pin = new ZGPIO(); \
ZASSERT(m_pin != nullptr); \
m_pin->initAsOutput(pin, __VA_ARGS__); \
} else { \
m_pin = nullptr; \
}
void TMC4361A::initialize(cfg_t *cfg) {
m_spi = cfg->spi;
m_csgpio = cfg->csgpio;
m_resetPin = cfg->resetPin;
m_fREEZEPin = cfg->fREEZEPin;
m_ennPin = cfg->ennPin;
m_driverIC_resetPin = cfg->driverIC_resetPin;
m_driverIC_ennPin = cfg->driverIC_ennPin;
m_spi = cfg->spi;
INIT_GPIO(m_csgpio, cfg->csgpio, ZGPIO::kMode_nopull, false, true);
INIT_GPIO(m_resetPin, cfg->resetPin, ZGPIO::kMode_nopull, false, true);
INIT_GPIO(m_fREEZEPin, cfg->fREEZEPin, ZGPIO::kMode_nopull, false, true);
INIT_GPIO(m_ennPin, cfg->ennPin, ZGPIO::kMode_nopull, false, true);
INIT_GPIO(m_driverIC_resetPin, cfg->driverIC_resetPin, ZGPIO::kMode_nopull, false, true);
INIT_GPIO(m_driverIC_ennPin, cfg->driverIC_ennPin, ZGPIO::kMode_nopull, false, true);
m_driver_ic_type = IC_TMC2160;
m_registerAccessTable = &tmc4361A_defaultRegisterAccess[0];

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

@ -25,17 +25,28 @@ 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;
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; //
Pin_t csgpio = PinNull; //
Pin_t resetPin = PinNull; //
Pin_t fREEZEPin = PinNull; //
Pin_t ennPin = PinNull; //
Pin_t driverIC_ennPin = PinNull; //
Pin_t driverIC_resetPin = PinNull; //
};
typedef enum {
kvelmode,
@ -44,22 +55,22 @@ class TMC4361A : public IStepperMotor {
private:
int32_t shadowRegister[TMC_REGISTER_COUNT];
const uint8_t *m_registerAccessTable;
const int32_t *m_defaultRegisterResetState;
const uint8_t *m_registerAccessTable = nullptr;
const int32_t *m_defaultRegisterResetState = nullptr;
driver_ic_type_t m_driver_ic_type;
uint32_t m_lastCallPeriodicJobTick;
uint8_t m_status;
SPI_HandleTypeDef *m_spi;
SPI_HandleTypeDef *m_spi = nullptr;
ZGPIO *m_csgpio;
ZGPIO *m_resetPin;
ZGPIO *m_fREEZEPin;
ZGPIO *m_ennPin;
ZGPIO *m_csgpio = nullptr;
ZGPIO *m_resetPin = nullptr;
ZGPIO *m_fREEZEPin = nullptr;
ZGPIO *m_ennPin = nullptr;
ZGPIO *m_driverIC_ennPin;
ZGPIO *m_driverIC_resetPin;
ZGPIO *m_driverIC_ennPin = nullptr;
ZGPIO *m_driverIC_resetPin = nullptr;
motor_mode_t m_motor_mode = kvelmode;

Write Preview
Loading…
Cancel
Save