953 lines
36 KiB
Python
953 lines
36 KiB
Python
import sys
|
|
import time
|
|
import math
|
|
import setenv
|
|
|
|
from machine import Pin
|
|
if setenv.MOTOR_SOFT_UART == True:
|
|
from softuart import UART
|
|
else:
|
|
from machine import UART
|
|
#-----------------------------------------------------------------------
|
|
# this file contains:
|
|
# 1. hexadecimal address of the different registers
|
|
# 2. bitposition and bitmasks of the different values of each register
|
|
#
|
|
# Example:
|
|
# the register IOIN has the address 0x06 and the first bit shows
|
|
# whether the ENABLE (EN/ENN) Pin is currently HIGH or LOW
|
|
#-----------------------------------------------------------------------
|
|
|
|
#addresses
|
|
GCONF = 0x00
|
|
GSTAT = 0x01
|
|
IFCNT = 0x02
|
|
IOIN = 0x06
|
|
IHOLD_IRUN = 0x10
|
|
TSTEP = 0x12
|
|
TCOOLTHRS = 0x14
|
|
SGTHRS = 0x40
|
|
SG_RESULT = 0x41
|
|
MSCNT = 0x6A
|
|
CHOPCONF = 0x6C
|
|
DRVSTATUS = 0x6F
|
|
VACTUAL = 0x22
|
|
|
|
#GCONF
|
|
i_scale_analog = 1<<0
|
|
internal_rsense = 1<<1
|
|
en_spreadcycle = 1<<2
|
|
shaft = 1<<3
|
|
index_otpw = 1<<4
|
|
index_step = 1<<5
|
|
mstep_reg_select = 1<<7
|
|
|
|
#GSTAT
|
|
reset = 1<<0
|
|
drv_err = 1<<1
|
|
uv_cp = 1<<2
|
|
|
|
#CHOPCONF
|
|
vsense = 1<<17
|
|
msres0 = 1<<24
|
|
msres1 = 1<<25
|
|
msres2 = 1<<26
|
|
msres3 = 1<<27
|
|
intpol = 1<<28
|
|
|
|
#IOIN
|
|
io_enn = 1<<0
|
|
io_step = 1<<7
|
|
io_spread = 1<<8
|
|
io_dir = 1<<9
|
|
|
|
#DRVSTATUS
|
|
stst = 1<<31
|
|
stealth = 1<<30
|
|
cs_actual = 31<<16
|
|
t157 = 1<<11
|
|
t150 = 1<<10
|
|
t143 = 1<<9
|
|
t120 = 1<<8
|
|
olb = 1<<7
|
|
ola = 1<<6
|
|
s2vsb = 1<<5
|
|
s2vsa = 1<<4
|
|
s2gb = 1<<3
|
|
s2ga = 1<<2
|
|
ot = 1<<1
|
|
otpw = 1<<0
|
|
|
|
#IHOLD_IRUN
|
|
ihold = 31<<0
|
|
irun = 31<<8
|
|
iholddelay = 15<<16
|
|
|
|
#SGTHRS
|
|
sgthrs = 255<<0
|
|
|
|
#others
|
|
mres_256 = 0
|
|
mres_128 = 1
|
|
mres_64 = 2
|
|
mres_32 = 3
|
|
mres_16 = 4
|
|
mres_8 = 5
|
|
mres_4 = 6
|
|
mres_2 = 7
|
|
mres_1 = 8
|
|
|
|
|
|
#-----------------------------------------------------------------------
|
|
# TMC_UART LogLevl
|
|
#-----------------------------------------------------------------------
|
|
class Loglevel():
|
|
none = 0
|
|
error = 10
|
|
info = 20
|
|
debug = 30
|
|
all = 100
|
|
|
|
|
|
#-----------------------------------------------------------------------
|
|
# TMC_UART
|
|
#
|
|
# this class is used to communicate with the TMC via UART
|
|
# it can be used to change the settings of the TMC.
|
|
# like the current or the microsteppingmode
|
|
#-----------------------------------------------------------------------
|
|
class TMC_UART:
|
|
mtr_id = 0
|
|
ser = None
|
|
rFrame = [0x55, 0, 0, 0]
|
|
wFrame = [0x55, 0, 0, 0 , 0, 0, 0, 0]
|
|
communication_pause = 0
|
|
|
|
#-----------------------------------------------------------------------
|
|
# constructor
|
|
#-----------------------------------------------------------------------
|
|
def __init__(self, rx, tx, serialport, baudrate):
|
|
if setenv.MOTOR_SOFT_UART == True:
|
|
self.ser = UART(rx=rx, tx=tx, baudrate=baudrate, id=serialport)
|
|
print("soft_tmc")
|
|
else:
|
|
self.ser = UART(serialport,baudrate,rx=Pin(rx), tx=Pin(tx))
|
|
print("hard_tmc",serialport,baudrate,rx ,tx)
|
|
|
|
self.mtr_id = 0
|
|
#self.ser.timeout = 20000/baudrate # adjust per baud and hardware. Sequential reads without some delay fail.
|
|
self.communication_pause = 500 / baudrate # adjust per baud and hardware. Sequential reads without some delay fail.
|
|
|
|
#-----------------------------------------------------------------------
|
|
# destructor
|
|
#-----------------------------------------------------------------------
|
|
def deinit(self):
|
|
self.ser.deinit()
|
|
|
|
#-----------------------------------------------------------------------
|
|
# this function calculates the crc8 parity bit
|
|
#-----------------------------------------------------------------------
|
|
def compute_crc8_atm(self, datagram, initial_value=0):
|
|
crc = initial_value
|
|
|
|
for byte in datagram: # Iterate bytes in data
|
|
for _ in range(0, 8): # Iterate bits in byte
|
|
if (crc >> 7) ^ (byte & 0x01):
|
|
crc = ((crc << 1) ^ 0x07) & 0xFF
|
|
else:
|
|
crc = (crc << 1) & 0xFF
|
|
|
|
byte = byte >> 1 # Shift to next bit
|
|
|
|
return crc
|
|
|
|
#-----------------------------------------------------------------------
|
|
# reads the registry on the TMC with a given address.
|
|
# returns the binary value of that register
|
|
#-----------------------------------------------------------------------
|
|
def read_reg(self, reg):
|
|
self.rFrame[1] = self.mtr_id
|
|
self.rFrame[2] = reg
|
|
self.rFrame[3] = self.compute_crc8_atm(self.rFrame[:-1])
|
|
rt = self.ser.write(bytes(self.rFrame))
|
|
|
|
if rt != len(self.rFrame):
|
|
# print(f"TMC2209: Err in write", file=sys.stderr)
|
|
return False
|
|
|
|
# time.sleep(self.communication_pause) # adjust per baud and hardware. Sequential reads without some delay fail.
|
|
rtn = self.ser.read() #read what it self
|
|
|
|
if rtn == None:
|
|
# print("TMC2209: Err in read")
|
|
return ""
|
|
|
|
print("rtn[7:11] :",rtn[7:11] , rtn[7] ,rtn[8] , rtn[9] , rtn[10] , " , reg : ",reg)
|
|
return rtn[7:11]
|
|
#-----------------------------------------------------------------------
|
|
# this function tries to read the registry of the TMC 10 times
|
|
# if a valid answer is returned, this function returns it as an integer
|
|
#-----------------------------------------------------------------------
|
|
def read_int(self, reg):
|
|
tries = 0
|
|
|
|
while True:
|
|
rtn = self.read_reg(reg)
|
|
tries += 1
|
|
|
|
if len(rtn) >= 4:
|
|
break
|
|
else:
|
|
pass
|
|
print(f"TMC2209: did not get the expected 4 data bytes. Instead got {str(len(rtn))} Bytes")
|
|
|
|
if tries >= 10:
|
|
print("TMC2209: after 10 tries not valid answer. exiting")
|
|
print("TMC2209: is Stepper Powersupply switched on ?")
|
|
raise SystemExit
|
|
|
|
return int.from_bytes(rtn, 'big', False)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# this function can write a value to the register of the tmc
|
|
# 1. use read_int to get the current setting of the TMC
|
|
# 2. then modify the settings as wished
|
|
# 3. write them back to the driver with this function
|
|
#-----------------------------------------------------------------------
|
|
def write_reg(self, reg, val):
|
|
#self.ser.reset_output_buffer()
|
|
#self.ser.reset_input_buffer()
|
|
|
|
self.wFrame[1] = self.mtr_id
|
|
self.wFrame[2] = reg | 0x80; # set write bit
|
|
self.wFrame[3] = 0xFF & (val>>24)
|
|
self.wFrame[4] = 0xFF & (val>>16)
|
|
self.wFrame[5] = 0xFF & (val>>8)
|
|
self.wFrame[6] = 0xFF & val
|
|
self.wFrame[7] = self.compute_crc8_atm(self.wFrame[:-1])
|
|
print("x" ,bytes(self.wFrame),self.wFrame)
|
|
|
|
rtn = self.ser.write(bytes(self.wFrame))
|
|
|
|
if rtn != len(self.wFrame):
|
|
# print(f"TMC2209: Err in write", file=sys.stderr)
|
|
return False
|
|
|
|
time.sleep(self.communication_pause)
|
|
return True
|
|
|
|
#-----------------------------------------------------------------------
|
|
# this function als writes a value to the register of the TMC
|
|
# but it also checks if the writing process was successfully by checking
|
|
# the InterfaceTransmissionCounter before and after writing
|
|
#-----------------------------------------------------------------------
|
|
def write_reg_check(self, reg, val):
|
|
ifcnt1 = self.read_int(IFCNT)
|
|
self.write_reg(reg, val)
|
|
ifcnt2 = self.read_int(IFCNT)
|
|
|
|
if ifcnt1 >= ifcnt2:
|
|
print("TMC2209: writing not successful!")
|
|
print(f"reg: {reg} val: {val}")
|
|
print(f"ifcnt: {ifcnt1} {ifcnt2}")
|
|
return False
|
|
|
|
return True
|
|
|
|
|
|
#-----------------------------------------------------------------------
|
|
# this sets a specific bit to 1
|
|
#-----------------------------------------------------------------------
|
|
def set_bit(self, value, bit):
|
|
return value | (bit)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# this sets a specific bit to 0
|
|
#-----------------------------------------------------------------------
|
|
def clear_bit(self, value, bit):
|
|
return value & ~(bit)
|
|
|
|
|
|
#-----------------------------------------------------------------------
|
|
# TMC_2209
|
|
#
|
|
# this class has two different functions:
|
|
# 1. change setting in the TMC-driver via UART
|
|
# 2. move the motor via STEP/DIR pins
|
|
#-----------------------------------------------------------------------
|
|
class TMC2209:
|
|
|
|
tmc_uart = None
|
|
pin_step = 1
|
|
p_pin_step = -1
|
|
p_pin_direction = -1
|
|
p_pin_enable = -1
|
|
debugmode = False
|
|
_micro_stepping_resolution = -1
|
|
_loglevel = Loglevel.info
|
|
|
|
#-----------------------------------------------------------------------
|
|
# constructor
|
|
#-----------------------------------------------------------------------
|
|
def __init__(self,
|
|
rx=setenv.MOTOR_RX_PIN,
|
|
tx=setenv.MOTOR_TX_PIN,
|
|
pin_step=setenv.MOTOR_STEP_PIN,
|
|
pin_direction=setenv.MOTOR_DIR_PIN,
|
|
pin_enable=setenv.MOTOR_EN_PIN,
|
|
baudrate=setenv.MOTOR_BAUDRATE,
|
|
serialport=setenv.MOTOR_NUMBER,
|
|
ms_resolution=setenv.MOTOR_MICROSTEP):
|
|
self.tmc_uart = TMC_UART(rx, tx, serialport, baudrate)
|
|
self.pin_step = pin_step
|
|
self.p_pin_step = Pin(pin_step, Pin.OUT)
|
|
self.p_pin_direction = Pin(pin_direction, Pin.OUT)
|
|
self.p_pin_enable = Pin(pin_enable, Pin.OUT)
|
|
#self.set_microstepping_resolution(setenv.MOTOR_MICROSTEP)
|
|
#self.clear_general_stat()
|
|
|
|
#-----------------------------------------------------------------------
|
|
# destructor
|
|
#-----------------------------------------------------------------------
|
|
def __del__(self):
|
|
if(self.debugmode == True):
|
|
print("TMC2209: Deinit")
|
|
|
|
self.set_pin_enable(False)
|
|
self.tmc_uart.deinit()
|
|
|
|
|
|
#-----------------------------------------------------------------------
|
|
# read the register Adress "DRVSTATUS" and prints all current setting
|
|
#-----------------------------------------------------------------------
|
|
def driver_status(self):
|
|
print("TMC2209: ---")
|
|
print("TMC2209: DRIVER STATUS:")
|
|
drvstatus = self.tmc_uart.read_int(DRVSTATUS)
|
|
|
|
if self.debugmode == True:
|
|
print("TMC2209:", bin(drvstatus))
|
|
|
|
if drvstatus & stst:
|
|
print("TMC2209: Info: motor is standing still")
|
|
else:
|
|
print("TMC2209: Info: motor is running")
|
|
|
|
if drvstatus & stealth:
|
|
print("TMC2209: Info: motor is running on StealthChop")
|
|
else:
|
|
print("TMC2209: Info: motor is running on SpreadCycle")
|
|
|
|
cs_act = drvstatus & cs_actual
|
|
cs_act = cs_act >> 16
|
|
print(f"TMC2209: CS actual: {str(cs_act)}")
|
|
|
|
if drvstatus & olb:
|
|
print("TMC2209: Warning: Open load detected on phase B")
|
|
|
|
if drvstatus & ola:
|
|
print("TMC2209: Warning: Open load detected on phase A")
|
|
|
|
if drvstatus & s2vsb:
|
|
print("TMC2209: Error: Short on low-side MOSFET detected on phase B. The driver becomes disabled")
|
|
|
|
if drvstatus & s2vsa:
|
|
print("TMC2209: Error: Short on low-side MOSFET detected on phase A. The driver becomes disabled")
|
|
|
|
if drvstatus & s2gb:
|
|
print("TMC2209: Error: Short to GND detected on phase B. The driver becomes disabled. ")
|
|
|
|
if drvstatus & s2ga:
|
|
print("TMC2209: Error: Short to GND detected on phase A. The driver becomes disabled. ")
|
|
|
|
if drvstatus & ot:
|
|
print("TMC2209: Error: Driver Overheating!")
|
|
|
|
if drvstatus & otpw:
|
|
print("TMC2209: Warning: Driver Overheating Prewarning!")
|
|
|
|
#-----------------------------------------------------------------------
|
|
# read the register Adress "GCONF" and prints all current setting
|
|
#-----------------------------------------------------------------------
|
|
def general_config(self):
|
|
print("TMC2209: ---")
|
|
print("TMC2209: GENERAL CONFIG")
|
|
gconf = self.tmc_uart.read_int(GCONF)
|
|
|
|
if self.debugmode == True:
|
|
print("TMC2209:", bin(gconf))
|
|
|
|
if gconf & i_scale_analog:
|
|
print("TMC2209: Driver is using voltage supplied to VREF as current reference")
|
|
else:
|
|
print("TMC2209: Driver is using internal reference derived from 5VOUT")
|
|
|
|
if gconf & internal_rsense:
|
|
print("TMC2209: Internal sense resistors. Use current supplied into VREF as reference.")
|
|
print("TMC2209: VREF pin internally is driven to GND in this mode.")
|
|
print("TMC2209: This will most likely destroy your driver!!!")
|
|
raise SystemExit
|
|
else:
|
|
print("TMC2209: Operation with external sense resistors")
|
|
|
|
if gconf & en_spreadcycle:
|
|
print("TMC2209: SpreadCycle mode enabled")
|
|
else:
|
|
print("TMC2209: StealthChop PWM mode enabled")
|
|
|
|
if gconf & shaft:
|
|
print("TMC2209: Inverse motor direction")
|
|
else:
|
|
print("TMC2209: normal motor direction")
|
|
|
|
if gconf & index_otpw:
|
|
print("TMC2209: INDEX pin outputs overtemperature prewarning flag")
|
|
else:
|
|
print("TMC2209: INDEX shows the first microstep position of sequencer")
|
|
|
|
if gconf & index_step:
|
|
print("TMC2209: INDEX output shows step pulses from internal pulse generator")
|
|
else:
|
|
print("TMC2209: INDEX output as selected by index_otpw")
|
|
|
|
if gconf & mstep_reg_select:
|
|
print("TMC2209: Microstep resolution selected by MSTEP register")
|
|
else:
|
|
print("TMC2209: Microstep resolution selected by pins MS1, MS2")
|
|
|
|
#-----------------------------------------------------------------------
|
|
# read the register Adress "GSTAT" and prints all current setting
|
|
#-----------------------------------------------------------------------
|
|
def general_stat(self):
|
|
print("TMC2209: ---")
|
|
print("TMC2209: GENERAL STAT")
|
|
gstat = self.tmc_uart.read_int(GSTAT)
|
|
|
|
if self.debugmode == True:
|
|
print("TMC2209:", bin(gstat))
|
|
|
|
if gstat & reset:
|
|
print("TMC2209: The Driver has been reset since the last read access to GSTAT")
|
|
|
|
if gstat & drv_err:
|
|
print("TMC2209: The driver has been shut down due to overtemperature or short circuit detection since the last read access")
|
|
|
|
if gstat & uv_cp:
|
|
print("TMC2209: Undervoltage on the charge pump. The driver is disabled in this case")
|
|
|
|
#-----------------------------------------------------------------------
|
|
# read the register Adress "GSTAT" and prints all current setting
|
|
#-----------------------------------------------------------------------
|
|
def clear_general_stat(self):
|
|
gstat = self.tmc_uart.read_int(GSTAT)
|
|
gstat = self.tmc_uart.set_bit(gstat, reset)
|
|
gstat = self.tmc_uart.set_bit(gstat, drv_err)
|
|
self.tmc_uart.write_reg_check(GSTAT, gstat)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# read the register Adress "IOIN" and prints all current setting
|
|
#-----------------------------------------------------------------------
|
|
def ioin(self):
|
|
print("TMC2209: ---")
|
|
print("TMC2209: INPUTS")
|
|
ioin = self.tmc_uart.read_int(IOIN)
|
|
|
|
if self.debugmode == True:
|
|
print("TMC2209:", bin(ioin))
|
|
|
|
if ioin & io_spread:
|
|
print("TMC2209: spread is high")
|
|
else:
|
|
print("TMC2209: spread is low")
|
|
|
|
if ioin & io_dir:
|
|
print("TMC2209: dir is high")
|
|
else:
|
|
print("TMC2209: dir is low")
|
|
|
|
if ioin & io_step:
|
|
print("TMC2209: step is high")
|
|
else:
|
|
print("TMC2209: step is low")
|
|
|
|
if ioin & io_enn:
|
|
print("TMC2209: en is high")
|
|
else:
|
|
print("TMC2209: en is low")
|
|
|
|
#-----------------------------------------------------------------------
|
|
# read the register Adress "CHOPCONF" and prints all current setting
|
|
#-----------------------------------------------------------------------
|
|
def chopper_control(self):
|
|
print("TMC2209: ---")
|
|
print("TMC2209: CHOPPER CONTROL")
|
|
chopconf = self.tmc_uart.read_int(CHOPCONF)
|
|
|
|
if self.debugmode == True:
|
|
print("TMC2209:", bin(chopconf))
|
|
|
|
print(f"TMC2209: native {str(self.get_microstepping_resolution())} microstep setting")
|
|
|
|
if chopconf & intpol:
|
|
print("TMC2209: interpolation to 256 microsteps")
|
|
|
|
if chopconf & vsense:
|
|
print("TMC2209: 1: High sensitivity, low sense resistor voltage")
|
|
else:
|
|
print("TMC2209: 0: Low sensitivity, high sense resistor voltage")
|
|
|
|
#-----------------------------------------------------------------------
|
|
# enables or disables the motor current output
|
|
#-----------------------------------------------------------------------
|
|
def set_pin_enable(self, enabled):
|
|
if enabled:
|
|
self.p_pin_enable.off()
|
|
# print(f"TMC2209: Motor output active = {enabled}")
|
|
else:
|
|
self.p_pin_enable.on()
|
|
# print("en",self.p_pin_enable.value(),enabled)
|
|
# print(f"TMC2209: Motor output active = {enabled}")
|
|
|
|
#-----------------------------------------------------------------------
|
|
# sets the motor shaft direction to the given value: 0 = CCW; 1 = CW
|
|
#-----------------------------------------------------------------------
|
|
def set_pin_direction(self, direction):
|
|
if (self.p_pin_direction.value() != direction):
|
|
# print("change",self.p_pin_direction.value() , direction)
|
|
|
|
if direction:
|
|
self.p_pin_direction.on()
|
|
else:
|
|
self.p_pin_direction.off()
|
|
# print("value",self.p_pin_direction.value() )
|
|
|
|
|
|
#-----------------------------------------------------------------------
|
|
# enables or disables the motor step output
|
|
#-----------------------------------------------------------------------
|
|
def set_pin_step(self, enabled):
|
|
if enabled:
|
|
self.p_pin_step.on()
|
|
else:
|
|
self.p_pin_step.off()
|
|
|
|
#-----------------------------------------------------------------------
|
|
# returns the motor shaft direction: 0 = CCW; 1 = CW
|
|
#-----------------------------------------------------------------------
|
|
def get_direction_shart(self):
|
|
|
|
return self.tmc_uart.read_int(GCONF) & shaft
|
|
|
|
|
|
#-----------------------------------------------------------------------
|
|
# sets the motor shaft direction to the given value: 0 = CCW; 1 = CW
|
|
#-----------------------------------------------------------------------
|
|
def set_direction_shart(self, direction):
|
|
gconf = self.tmc_uart.read_int(GCONF)
|
|
if (self.get_direction_shart()/8!=direction):
|
|
|
|
if direction:
|
|
if self.debugmode == True:
|
|
print("TMC2209: write inverse motor direction")
|
|
|
|
gconf = self.tmc_uart.set_bit(gconf, shaft)
|
|
else:
|
|
if self.debugmode == True:
|
|
print("TMC2209: write normal motor direction")
|
|
|
|
gconf = self.tmc_uart.clear_bit(gconf, shaft)
|
|
# print(direction)
|
|
self.tmc_uart.write_reg_check(GCONF, gconf)
|
|
time.sleep_us(100)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# return whether the tmc inbuilt interpolation is active
|
|
#-----------------------------------------------------------------------
|
|
def get_interpolation(self):
|
|
return self.tmc_uart.read_int(CHOPCONF) & intpol
|
|
|
|
#-----------------------------------------------------------------------
|
|
# enables the tmc inbuilt interpolation of the steps to 256 microsteps
|
|
#-----------------------------------------------------------------------
|
|
def set_interpolation(self, enabled):
|
|
chopconf = self.tmc_uart.read_int(CHOPCONF)
|
|
|
|
if enabled:
|
|
chopconf = self.tmc_uart.set_bit(chopconf, intpol)
|
|
else:
|
|
chopconf = self.tmc_uart.clear_bit(chopconf, intpol)
|
|
|
|
if self.debugmode == True:
|
|
print(f"TMC2209: writing microstep interpolation setting =", enabled)
|
|
|
|
self.tmc_uart.write_reg_check(CHOPCONF, chopconf)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# return whether spreadcycle (1) is active or stealthchop (0)
|
|
#-----------------------------------------------------------------------
|
|
def get_spread_cycle(self):
|
|
return self.tmc_uart.read_int(GCONF) & en_spreadcycle
|
|
|
|
#-----------------------------------------------------------------------
|
|
# enables spreadcycle (1) or stealthchop (0)
|
|
#-----------------------------------------------------------------------
|
|
def set_spread_cycle(self, enabled):
|
|
gconf = self.tmc_uart.read_int(GCONF)
|
|
|
|
if enabled:
|
|
if self.debugmode == True:
|
|
print("TMC2209: Spreadcycle = activated")
|
|
|
|
gconf = self.tmc_uart.set_bit(gconf, en_spreadcycle)
|
|
else:
|
|
if self.debugmode == True:
|
|
print("TMC2209: Stealthchop = deactivated")
|
|
|
|
gconf = self.tmc_uart.clear_bit(gconf, en_spreadcycle)
|
|
|
|
self.tmc_uart.write_reg_check(GCONF, gconf)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# returns the current native microstep resolution (1-256)
|
|
#-----------------------------------------------------------------------
|
|
def get_microstepping_resolution(self):
|
|
chopconf = self.tmc_uart.read_int(CHOPCONF)
|
|
print("chopconf : ", chopconf)
|
|
msresdezimal = chopconf & (msres0 | msres1 | msres2 | msres3)
|
|
msresdezimal = msresdezimal >> 24
|
|
msresdezimal = 8 - msresdezimal
|
|
self._micro_stepping_resolution = int(math.pow(2, msresdezimal))
|
|
|
|
return self._micro_stepping_resolution
|
|
|
|
#-----------------------------------------------------------------------
|
|
# sets the current native microstep resolution (1,2,4,8,16,32,64,128,256)
|
|
#-----------------------------------------------------------------------
|
|
def set_microstepping_resolution(self, msres):
|
|
chopconf = self.tmc_uart.read_int(CHOPCONF)
|
|
chopconf = chopconf & (~msres0 | ~msres1 | ~msres2 | ~msres3) #setting all bits to zero
|
|
print("chopconf 1 :",chopconf)
|
|
|
|
msresdezimal = int(math.log(msres, 2))
|
|
msresdezimal = 8 - msresdezimal
|
|
print("msresdezimal 2 :",msresdezimal)
|
|
|
|
chopconf = int(chopconf) & int(4043309055)
|
|
print("chopconf 2 :",chopconf)
|
|
|
|
chopconf = chopconf | msresdezimal << 24
|
|
print("chopconf 3 :",chopconf)
|
|
|
|
if self.debugmode == True:
|
|
print(f"TMC2209: writing {str(msres)} microstep setting")
|
|
|
|
self.tmc_uart.write_reg_check(CHOPCONF, chopconf)
|
|
self.set_microstep_resolution_regselect(True)
|
|
|
|
|
|
#-----------------------------------------------------------------------
|
|
# sets the register bit "mstep_reg_select" to 1 or 0 depending to the given value.
|
|
# this is needed to set the microstep resolution via UART
|
|
# this method is called by "setMicrosteppingResolution"
|
|
#-----------------------------------------------------------------------
|
|
def set_microstep_resolution_regselect(self, enabled):
|
|
gconf = self.tmc_uart.read_int(GCONF)
|
|
print("gconf1 :",gconf)
|
|
|
|
if enabled:
|
|
gconf = self.tmc_uart.set_bit(gconf, mstep_reg_select)
|
|
else:
|
|
gconf = self.tmc_uart.clear_bit(gconf, mstep_reg_select)
|
|
|
|
if self.debugmode == True:
|
|
print(f"TMC2209: writing MStep Reg Select = {enabled}")
|
|
print("gconf2 :",gconf)
|
|
|
|
self.tmc_uart.write_reg_check(GCONF, gconf)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# returns how many steps are needed for one revolution
|
|
#-----------------------------------------------------------------------
|
|
def get_steps_per_resolution(self):
|
|
return 200 * self.get_microstepping_resolution()
|
|
|
|
#-----------------------------------------------------------------------
|
|
# returns the current Microstep counter.
|
|
# Indicates actual position in the microstep table for CUR_A
|
|
#-----------------------------------------------------------------------
|
|
def get_microstep_counter(self, in_steps = False, offset = 0):
|
|
mscnt = self.tmc_uart.read_int(MSCNT)
|
|
|
|
if not in_steps:
|
|
return mscnt
|
|
|
|
return round((4 * self._micro_stepping_resolution) - ((mscnt - 64) * (self._micro_stepping_resolution * 4) / 1024) - 1) + offset
|
|
|
|
#-----------------------------------------------------------------------
|
|
# returns how many steps are needed for one rotation
|
|
#-----------------------------------------------------------------------
|
|
def get_steps_per_rotation(self):
|
|
return self.get_steps_per_resolution() * 21
|
|
|
|
#-----------------------------------------------------------------------
|
|
# return the current stallguard result
|
|
# its will be calculated with every fullstep
|
|
# higher values means a lower motor load
|
|
#-----------------------------------------------------------------------
|
|
def get_tstep(self):
|
|
return self.tmc_uart.read_int(TSTEP)
|
|
ehyfbzeyfeunz,ujefz
|
|
#-----------------------------------------------------------------------
|
|
# sets the register bit "VACTUAL" to to a given value
|
|
# VACTUAL allows moving the motor by UART control.
|
|
# It gives the motor velocity in +-(2^23)-1 [μsteps / t]
|
|
# 0: Normal operation. Driver reacts to STEP input
|
|
#-----------------------------------------------------------------------
|
|
def set_vactual(self, value):
|
|
self.tmc_uart.write_reg_check(VACTUAL, int(round(value)))
|
|
|
|
#-----------------------------------------------------------------------
|
|
# return the current stallguard result
|
|
# its will be calculated with every fullstep
|
|
# higher values means a lower motor load
|
|
#-----------------------------------------------------------------------
|
|
def get_stallguard(self):
|
|
return self.tmc_uart.read_int(SG_RESULT)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# sets the register bit "SGTHRS" to to a given value
|
|
# this is needed for the stallguard interrupt callback
|
|
# SG_RESULT becomes compared to the double of this threshold.
|
|
# SG_RESULT ≤ SGTHRS*2
|
|
#-----------------------------------------------------------------------
|
|
def set_stallguard_threshold(self, threshold):
|
|
if self.debugmode == True:
|
|
print("TMC2209: sgthrs =", bin(threshold))
|
|
|
|
self.tmc_uart.write_reg_check(SGTHRS, threshold)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# This is the lower threshold velocity for switching
|
|
# on smart energy CoolStep and StallGuard to DIAG output. (unsigned)
|
|
#-----------------------------------------------------------------------
|
|
def set_coolstep_threshold(self, threshold):
|
|
if self.debugmode == True:
|
|
print("TMC2209: tcoolthrs =", bin(threshold))
|
|
|
|
self.tmc_uart.write_reg_check(TCOOLTHRS, threshold)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# set a function to call back, when the driver detects a stall
|
|
# via stallguard
|
|
# high value on the diag pin can also mean a driver error
|
|
#-----------------------------------------------------------------------
|
|
def set_stallguard_callback(self, stallguard_pin, threshold, callback, min_speed = 2000):
|
|
self.set_stallguard_threshold(threshold)
|
|
self.set_coolstep_threshold(min_speed)
|
|
|
|
if self.debugmode == True:
|
|
print("TMC2209: setup stallguard callback")
|
|
|
|
p25 = Pin(stallguard_pin, Pin.IN, Pin.PULL_DOWN)
|
|
p25.irq(trigger=Pin.IRQ_RISING, handler=callback)
|
|
|
|
|
|
#-----------------------------------------------------------------------
|
|
# return whether Vref (1) or 5V (0) is used for current scale
|
|
#-----------------------------------------------------------------------
|
|
def get_iscale_analog(self):
|
|
return self.tmc_uart.read_int(GCONF) & i_scale_analog
|
|
|
|
#-----------------------------------------------------------------------
|
|
# sets Vref (1) or 5V (0) for current scale
|
|
#-----------------------------------------------------------------------
|
|
def set_iscale_analog(self, enabled):
|
|
gconf = self.tmc_uart.read_int(GCONF)
|
|
|
|
if enabled:
|
|
if self.debugmode == True:
|
|
print("TMC2209: activated Vref for current scale")
|
|
|
|
gconf = self.tmc_uart.set_bit(gconf, i_scale_analog)
|
|
else:
|
|
if self.debugmode == True:
|
|
print("TMC2209: activated 5V-out for current scale")
|
|
|
|
gconf = self.tmc_uart.clear_bit(gconf, i_scale_analog)
|
|
|
|
self.tmc_uart.write_reg_check(GCONF, gconf)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# returns which sense resistor voltage is used for current scaling
|
|
# 0: Low sensitivity, high sense resistor voltage
|
|
# 1: High sensitivity, low sense resistor voltage
|
|
#-----------------------------------------------------------------------
|
|
def get_voltage_sense(self):
|
|
return self.tmc_uart.read_int(CHOPCONF) & vsense
|
|
|
|
#-----------------------------------------------------------------------
|
|
# sets which sense resistor voltage is used for current scaling
|
|
# 0: Low sensitivity, high sense resistor voltage
|
|
# 1: High sensitivity, low sense resistor voltage
|
|
#-----------------------------------------------------------------------
|
|
def set_voltage_sense(self, enabled):
|
|
chopconf = self.tmc_uart.read_int(CHOPCONF)
|
|
|
|
if enabled:
|
|
if self.debugmode == True:
|
|
print("TMC2209: activated High sensitivity, low sense resistor voltage")
|
|
|
|
chopconf = self.tmc_uart.set_bit(chopconf, vsense)
|
|
else:
|
|
if self.debugmode == True:
|
|
print("TMC2209: activated Low sensitivity, high sense resistor voltage")
|
|
|
|
chopconf = self.tmc_uart.clear_bit(chopconf, vsense)
|
|
|
|
self.tmc_uart.write_reg_check(CHOPCONF, chopconf)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# returns which sense resistor voltage is used for current scaling
|
|
# 0: Low sensitivity, high sense resistor voltage
|
|
# 1: High sensitivity, low sense resistor voltage
|
|
#-----------------------------------------------------------------------
|
|
def get_internal_resistor_sense(self):
|
|
return self.tmc_uart.read_int(GCONF) & internal_rsense
|
|
|
|
#-----------------------------------------------------------------------
|
|
# sets which sense resistor voltage is used for current scaling
|
|
# 0: Low sensitivity, high sense resistor voltage
|
|
# 1: High sensitivity, low sense resistor voltage
|
|
#-----------------------------------------------------------------------
|
|
def set_internal_resistor_sense(self, enabled):
|
|
gconf = self.tmc_uart.read_int(GCONF)
|
|
|
|
if enabled:
|
|
if self.debugmode == True:
|
|
print("TMC2209: activated internal sense resistors.")
|
|
|
|
gconf = self.tmc_uart.set_bit(gconf, internal_rsense)
|
|
else:
|
|
if self.debugmode == True:
|
|
print("TMC2209: activated operation with external sense resistors")
|
|
|
|
gconf = self.tmc_uart.clear_bit(gconf, internal_rsense)
|
|
|
|
self.tmc_uart.write_reg_check(GCONF, gconf)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# sets the current scale (CS) for Running and Holding
|
|
# and the delay, when to be switched to Holding current
|
|
# IHold = 0-31; IRun = 0-31; IHoldDelay = 0-15
|
|
#-----------------------------------------------------------------------
|
|
def set_irun_ihold(self, IHold, IRun, IHoldDelay):
|
|
ihold_irun = 0
|
|
ihold_irun = ihold_irun | IHold << 0
|
|
ihold_irun = ihold_irun | IRun << 8
|
|
ihold_irun = ihold_irun | IHoldDelay << 16
|
|
|
|
if self.debugmode == True:
|
|
print("TMC2209: ihold_irun =", bin(ihold_irun))
|
|
|
|
self.tmc_uart.write_reg_check(IHOLD_IRUN, ihold_irun)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# sets the current flow for the motor
|
|
# run_current in mA
|
|
# check whether Vref is actually 1.2V
|
|
#-----------------------------------------------------------------------
|
|
def set_current_flow(self, run_current, hold_current_multiplier = 0.5, hold_current_delay = 10, Vref = setenv.MOTOR_VREF):
|
|
CS_IRun = 0
|
|
Rsense = 0.11
|
|
Vfs = 0
|
|
|
|
if self.get_voltage_sense():
|
|
if self.debugmode == True:
|
|
print("TMC2209: Vsense: 1")
|
|
|
|
Vfs = 0.180 * Vref / 2.5
|
|
else:
|
|
if self.debugmode == True:
|
|
print("TMC2209: Vsense: 0")
|
|
|
|
Vfs = 0.325 * Vref / 2.5
|
|
|
|
CS_IRun = 32.0 * 1.41421 * run_current / 1000.0 * (Rsense + 0.02) / Vfs - 1
|
|
CS_IRun = min(CS_IRun, 31)
|
|
CS_IRun = max(CS_IRun, 0)
|
|
CS_IHold = hold_current_multiplier * CS_IRun
|
|
CS_IRun = round(CS_IRun)
|
|
CS_IHold = round(CS_IHold)
|
|
hold_current_delay = round(hold_current_delay)
|
|
|
|
if self.debugmode == True:
|
|
print("TMC2209: CS_IRun =", CS_IRun)
|
|
print("TMC2209: CS_IHold =", CS_IHold)
|
|
print("TMC2209: Delay =", hold_current_delay)
|
|
|
|
self.set_irun_ihold(CS_IHold, CS_IRun, hold_current_delay)
|
|
|
|
#-----------------------------------------------------------------------
|
|
# tests the EN, DIR and STEP pin
|
|
# this sets the EN, DIR and STEP pin to HIGH, LOW and HIGH
|
|
# and checks the IOIN Register of the TMC meanwhile
|
|
#-----------------------------------------------------------------------
|
|
def test_pins(self):
|
|
pin_dir_ok = pin_step_ok = pin_en_ok = True
|
|
|
|
self.p_pin_step.on()
|
|
self.p_pin_direction.on()
|
|
self.p_pin_enable.on()
|
|
|
|
time.sleep(0.1)
|
|
ioin = self.tmc_uart.read_int(IOIN)
|
|
|
|
if not(ioin & io_dir):
|
|
pin_dir_ok = False
|
|
|
|
if not(ioin & io_step):
|
|
pin_step_ok = False
|
|
|
|
if not(ioin & io_enn):
|
|
pin_en_ok = False
|
|
|
|
self.p_pin_step.off()
|
|
self.p_pin_direction.off()
|
|
self.p_pin_enable.off()
|
|
|
|
time.sleep(0.1)
|
|
ioin = self.tmc_uart.read_int(IOIN)
|
|
|
|
if ioin & io_dir:
|
|
pin_dir_ok = False
|
|
|
|
if ioin & io_step:
|
|
pin_step_ok = False
|
|
|
|
if ioin & io_enn:
|
|
pin_en_ok = False
|
|
|
|
self.p_pin_step.on()
|
|
self.p_pin_direction.on()
|
|
self.p_pin_enable.on()
|
|
|
|
time.sleep(0.1)
|
|
ioin = self.tmc_uart.read_int(IOIN)
|
|
|
|
if not(ioin & io_dir):
|
|
pin_dir_ok = False
|
|
|
|
if not(ioin & io_step):
|
|
pin_step_ok = False
|
|
|
|
if not(ioin & io_enn):
|
|
pin_en_ok = False
|
|
|
|
self.set_pin_enable(False)
|
|
|
|
print("---")
|
|
print(f"Pin STEP: \t{pin_step_ok}")
|
|
print(f"Pin DIRECTION: \t{pin_dir_ok}")
|
|
print(f"Pin ENABLE: \t{pin_en_ok}")
|
|
print("---")
|