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)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)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)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)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)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)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