326 lines
13 KiB
Python
326 lines
13 KiB
Python
import time
|
|
from as5600l import AS5600L
|
|
from tmc_2209 import TMC2209
|
|
import setenv
|
|
from machine import Pin
|
|
import gc
|
|
#-----------------------------------------------------------------------
|
|
# 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
|
|
#-----------------------------------------------------------------------
|
|
class SmartMotor(TMC2209):
|
|
status = False
|
|
|
|
position = 0
|
|
if setenv.MOTOR_SENSOR_END_PIN != -1 :
|
|
init_home_end =setenv.MOTOR_SENSOR_END_VALUE
|
|
pin_home_end = Pin(setenv.MOTOR_SENSOR_END_PIN, Pin.IN)
|
|
if setenv.MOTOR_SENSOR_BEGIN_PIN != -1 :
|
|
init_home_begin =setenv.MOTOR_SENSOR_BEGIN_VALUE
|
|
pin_home_begin = Pin(setenv.MOTOR_SENSOR_BEGIN_PIN, Pin.IN)
|
|
|
|
def get_encoder_position(self,encoder):
|
|
if self.encoder is not None:
|
|
self.actual = self.encoder.get_angle_degrees()
|
|
if self.actual != None and self.actual != self.last:
|
|
self.calc = self.actual-self.last
|
|
if self.actual < self.last :
|
|
if self.calc<-3800:
|
|
self.calc = abs((4096 - self.last + self.actual))
|
|
self.posreal+=self.calc
|
|
elif self.actual > self.last :
|
|
if self.calc>3800:
|
|
self.calc = abs((4096 - self.actual + self.last))
|
|
self.posreal+=self.calc
|
|
self.last = self.actual
|
|
return(self.posreal)
|
|
|
|
def init_encoder(self):
|
|
self.encoder = AS5600L()
|
|
self.actual = 0
|
|
self.last = 0
|
|
print(self.encoder.get_status())
|
|
self.posbegin = self.encoder.get_angle_degrees_fast()
|
|
print("posbegin",self.posbegin)
|
|
self.posreal = 0
|
|
self.last=self.posbegin
|
|
|
|
def reset_position(self):
|
|
self.position = 0
|
|
|
|
# def set_home_pins(self, start = -1, end = -1):
|
|
|
|
def set_encoder(self, encoder):
|
|
self.encoder = encoder
|
|
|
|
def init_vactual(self):
|
|
self.clear_general_stat()
|
|
time.sleep_us(10)
|
|
self.set_voltage_sense(False)#ok
|
|
self.set_current_flow(setenv.MOTOR_AMP)
|
|
self.set_iscale_analog(False)#ok
|
|
self.set_interpolation(True)
|
|
self.set_internal_resistor_sense(False)
|
|
self.set_spread_cycle(True)
|
|
self.set_microstepping_resolution(setenv.MOTOR_MICROSTEP)
|
|
self.set_microstep_resolution_regselect(True)
|
|
self.set_pin_enable(True)
|
|
return True
|
|
|
|
def rotate(self,
|
|
vactual,
|
|
degrees = 0,
|
|
acceleration = 0,
|
|
deceleration = 0,
|
|
KP = 1.0,
|
|
KI = 0,
|
|
KD = 0,
|
|
counter = False,
|
|
show_stallguard_result = False,
|
|
show_tstep = False,
|
|
absrel = True):
|
|
self.init_vactual()
|
|
liste_vitesse = [0]
|
|
liste_time = [0]
|
|
liste_distance = [0]
|
|
if(absrel):
|
|
if degrees - self.position < 0:
|
|
print(degrees - self.position)
|
|
self.set_direction_shart(False)
|
|
degrees = abs(degrees - self.position)
|
|
sense = False
|
|
else:
|
|
degrees = degrees - self.position
|
|
sense = True
|
|
self.set_direction_shart(True)
|
|
|
|
vactual = int(round((vactual/0.715)*self.get_steps_per_resolution())) # not tested!
|
|
self._stop = False
|
|
current_vactual = 0
|
|
last_error = 0
|
|
integral = 0
|
|
|
|
|
|
if self.debugmode == True:
|
|
if(degrees != 0):
|
|
print("vactual: "+str(vactual)+" for "+str(degrees)+" degrees")
|
|
|
|
|
|
if (degrees != 0):
|
|
start_time = time.ticks_ms()
|
|
end_time = start_time
|
|
current_degrees = 0
|
|
timespan = 0
|
|
error= 0
|
|
self.set_pin_enable(True)
|
|
while((current_degrees <= degrees)):
|
|
print(current_degrees)
|
|
error = degrees - current_degrees
|
|
integral = integral + error
|
|
derivative = error - last_error
|
|
speed_adjustment = KP * error + KI * integral + KD * derivative
|
|
last_error = error
|
|
|
|
start_time= time.ticks_ms()
|
|
gap_time = (start_time-end_time)/1000
|
|
rps = current_vactual/self.get_steps_per_resolution()*0.715
|
|
angle = rps*gap_time*360
|
|
current_degrees = current_degrees+angle
|
|
timespan +=gap_time
|
|
if(sense):
|
|
self.position += angle
|
|
else:
|
|
self.position -= angle
|
|
if ((current_vactual==0) or (error/current_vactual > current_vactual/deceleration)):
|
|
if(acceleration != 0 and abs(current_vactual)<abs(vactual)):
|
|
current_vactual += acceleration*gap_time
|
|
if current_vactual>vactual:
|
|
current_vactual=vactual
|
|
else:
|
|
current_vactual -= deceleration*gap_time
|
|
# liste_vitesse.append(current_vactual*setenv.MOTOR_MICROSTEP)
|
|
# liste_time.append(liste_time[len(liste_time)-1] + gap_time)
|
|
# liste_distance.append(current_degrees)
|
|
print(current_vactual*setenv.MOTOR_MICROSTEP)
|
|
self.set_vactual(current_vactual*setenv.MOTOR_MICROSTEP)
|
|
gc.collect()
|
|
end_time=start_time
|
|
self.set_pin_enable(False)
|
|
self.set_vactual(0)
|
|
# print("============vitesse==========")
|
|
# print(liste_vitesse)
|
|
# print("============temps============")
|
|
# print(liste_time)
|
|
# print("============distance============")
|
|
# print(liste_distance)
|
|
# print("=============================")
|
|
|
|
return ('fin de position a : ' + str(self.position) + ' degres')
|
|
|
|
def rotate_Closed_Loop(self,
|
|
vactual,
|
|
degrees = 0,
|
|
acceleration = 0,
|
|
deceleration = 0,
|
|
KP = 1.0,
|
|
KI = 0,
|
|
KD = 0,
|
|
counter = False,
|
|
show_stallguard_result = False,
|
|
show_tstep = False,
|
|
absrel = True):
|
|
self.init_vactual()
|
|
print(self.get_encoder_position(self.encoder))
|
|
if(absrel):
|
|
print(self.get_encoder_position(self.encoder))
|
|
if (degrees - self.get_encoder_position(self.encoder)) < 0:
|
|
self.set_direction_shart(False)
|
|
sense = False
|
|
else:
|
|
self.set_direction_shart(True)
|
|
sense = True
|
|
|
|
vactual = int(round((vactual/0.715)*self.get_steps_per_resolution())) # not tested!
|
|
self._stop = False
|
|
current_vactual = 0
|
|
last_error = 0
|
|
integral = 0
|
|
|
|
if (vactual<0):
|
|
acceleration = -acceleration
|
|
|
|
if self.debugmode == True:
|
|
if(degrees != 0):
|
|
print("vactual: "+str(vactual)+" for "+str(degrees)+" degrees")
|
|
|
|
|
|
if (degrees != 0):
|
|
current_degrees = self.get_encoder_position(0)
|
|
self.set_pin_enable(True)
|
|
start_time = time.ticks_ms()
|
|
end_time = start_time
|
|
current_degrees = 0
|
|
timespan = 0
|
|
|
|
if(degrees - current_degrees < 0):
|
|
self.set_set_direction_shart_shart(False)
|
|
while((not self._stop) and (current_degrees <= degrees)):
|
|
current_degrees = self.get_encoder_position(0)
|
|
|
|
error = degrees - current_degrees
|
|
integral = integral + error
|
|
derivative = error - last_error
|
|
speed_adjustment = KP * error + KI * integral + KD * derivative
|
|
last_error = error
|
|
|
|
start_time= time.ticks_ms()
|
|
gap_time = (start_time-end_time)/1000
|
|
rps = current_vactual/self.get_steps_per_resolution()*0.715
|
|
angle = rps*gap_time*360
|
|
current_degrees = current_degrees+angle
|
|
timespan +=gap_time
|
|
if(sense):
|
|
self.position += angle
|
|
else:
|
|
self.position -= angle
|
|
|
|
if ((current_vactual==0) or (error/current_vactual > current_vactual/deceleration)):
|
|
if(acceleration != 0 and abs(current_vactual)<abs(vactual)):
|
|
current_vactual += acceleration*gap_time
|
|
if current_vactual>vactual:
|
|
current_vactual=vactual
|
|
self.set_vactual(current_vactual)
|
|
else:
|
|
current_vactual -= deceleration*gap_time
|
|
self.set_vactual(current_vactual)
|
|
self.set_pin_enable(False)
|
|
self.set_vactual(0)
|
|
return ('fin de position a : ' + str(self.position) + ' degres')
|
|
|
|
def stop(self):
|
|
self.set_vactual(0)
|
|
time.sleep(1)
|
|
self.set_pin_enable(False)
|
|
self.p_pin_enable.off()
|
|
#-----------------------------------------------------------------------
|
|
# This function is for doing Home
|
|
# Setup "mode" to 'begin' for Home 0 and 'end' for go to max range
|
|
#For uses absolute position
|
|
#-----------------------------------------------------------------------
|
|
|
|
def go_home(self,mode=False):
|
|
self.run_vactual()
|
|
if(mode!=False):
|
|
if(mode == "begin"):
|
|
current_vactual = 0
|
|
if self.pin_home_begin != -1:
|
|
self.set_pin_enable(True)
|
|
self.set_direction_shart(False)
|
|
vactual = int(round((1/0.715)*self.get_steps_per_resolution()))
|
|
while self.pin_home_begin.value() == self.init_home_begin:
|
|
if(abs(current_vactual)<abs(vactual)):
|
|
current_vactual += 50*0.05
|
|
if current_vactual>vactual:
|
|
current_vactual=vactual
|
|
self.set_vactual(current_vactual)
|
|
self.set_vactual(0)
|
|
time.sleep(0.2)
|
|
self.set_direction_shart(True)
|
|
current_vactual = 0
|
|
while self.pin_home_begin.value() != self.init_home_begin:
|
|
if(abs(current_vactual)<abs(vactual)):
|
|
current_vactual += 50*0.05
|
|
if current_vactual>vactual:
|
|
current_vactual=vactual
|
|
self.set_vactual(current_vactual)
|
|
else :
|
|
print("Any begin sensor setup")
|
|
return False
|
|
if(mode == "end"):
|
|
if self.pin_home_end != -1:
|
|
self.set_pin_enable(True)
|
|
self.set_direction_shart(True)
|
|
vactual = int(round((1.5/0.715)*self.get_steps_per_resolution()))
|
|
while self.pin_home_end.value() == self.init_home_end:
|
|
if(abs(current_vactual)<abs(vactual)):
|
|
current_vactual += 100*0.05
|
|
if current_vactual>vactual:
|
|
current_vactual=vactual
|
|
self.set_vactual(current_vactual)
|
|
self.set_vactual(0)
|
|
time.sleep(0.2)
|
|
self.set_direction_shart(False)
|
|
current_vactual = 0
|
|
while self.pin_home_end.value() != self.init_home_end:
|
|
if(abs(current_vactual)<abs(vactual)):
|
|
current_vactual += 100*0.05
|
|
if current_vactual>vactual:
|
|
current_vactual=vactual
|
|
self.set_vactual(int(round(current_vactual)))
|
|
else :
|
|
print("Any end sensor setup")
|
|
return False
|
|
|
|
else:
|
|
self.set_voltage_sense(True)
|
|
self.set_current_flow(1800)
|
|
self.set_iscale_analog(True)
|
|
self.set_interpolation(True)
|
|
self.set_spread_cycle(True)
|
|
self.set_microstepping_resolution(setenv.MOTOR_MICROSTEP)
|
|
self.set_internal_resistor_sense(False)
|
|
self.set_microstep_resolution_regselect(True)
|
|
print(self.rotation(500,degrees=-self.position,acceleration=1000,deceleration=1000))
|
|
self.set_vactual(0)
|
|
self.position = 0
|
|
return True
|
|
|
|
#-----------------------------------------------------------------------
|
|
# return the current position result
|
|
#-----------------------------------------------------------------------
|
|
|
|
def get_position_degrees(self):
|
|
return self.position
|