Initial commit
This commit is contained in:
325
BM/smart_motor.py
Normal file
325
BM/smart_motor.py
Normal file
@@ -0,0 +1,325 @@
|
||||
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
|
||||
Reference in New Issue
Block a user