| 
									
										
										
										
											2016-05-25 11:37:40 -04:00
										 |  |  | # Printer stepper support | 
					
						
							|  |  |  | # | 
					
						
							| 
									
										
										
										
											2017-08-21 11:25:26 -04:00
										 |  |  | # Copyright (C) 2016,2017  Kevin O'Connor <kevin@koconnor.net> | 
					
						
							| 
									
										
										
										
											2016-05-25 11:37:40 -04:00
										 |  |  | # | 
					
						
							|  |  |  | # This file may be distributed under the terms of the GNU GPLv3 license. | 
					
						
							|  |  |  | import math, logging | 
					
						
							| 
									
										
										
										
											2017-08-21 11:25:26 -04:00
										 |  |  | import homing, pins | 
					
						
							| 
									
										
										
										
											2016-05-25 11:37:40 -04:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2017-11-06 18:37:35 -05:00
										 |  |  | # Code storing the definitions for a stepper motor | 
					
						
							| 
									
										
										
										
											2016-05-25 11:37:40 -04:00
										 |  |  | class PrinterStepper: | 
					
						
							| 
									
										
										
										
											2017-10-29 10:58:04 -04:00
										 |  |  |     def __init__(self, printer, config): | 
					
						
							|  |  |  |         self.name = config.section | 
					
						
							|  |  |  |         if self.name.startswith('stepper_'): | 
					
						
							|  |  |  |             self.name = self.name[8:] | 
					
						
							| 
									
										
										
										
											2016-05-25 11:37:40 -04:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2017-04-11 11:37:09 -04:00
										 |  |  |         self.step_dist = config.getfloat('step_distance', above=0.) | 
					
						
							| 
									
										
										
										
											2017-08-21 11:25:26 -04:00
										 |  |  |         self.mcu_stepper = pins.setup_pin( | 
					
						
							|  |  |  |             printer, 'stepper', config.get('step_pin')) | 
					
						
							|  |  |  |         dir_pin_params = pins.get_printer_pins(printer).parse_pin_desc( | 
					
						
							|  |  |  |             config.get('dir_pin'), can_invert=True) | 
					
						
							|  |  |  |         self.mcu_stepper.setup_dir_pin(dir_pin_params) | 
					
						
							|  |  |  |         self.mcu_stepper.setup_step_distance(self.step_dist) | 
					
						
							| 
									
										
										
										
											2017-07-24 13:54:46 -04:00
										 |  |  | 
 | 
					
						
							|  |  |  |         enable_pin = config.get('enable_pin', None) | 
					
						
							| 
									
										
										
										
											2017-09-19 15:33:34 -04:00
										 |  |  |         self.mcu_enable = None | 
					
						
							| 
									
										
										
										
											2017-07-24 13:54:46 -04:00
										 |  |  |         if enable_pin is not None: | 
					
						
							| 
									
										
										
										
											2017-08-21 11:25:26 -04:00
										 |  |  |             self.mcu_enable = pins.setup_pin(printer, 'digital_out', enable_pin) | 
					
						
							|  |  |  |             self.mcu_enable.setup_max_duration(0.) | 
					
						
							| 
									
										
										
										
											2017-07-24 13:54:46 -04:00
										 |  |  |         self.need_motor_enable = True | 
					
						
							|  |  |  |     def _dist_to_time(self, dist, start_velocity, accel): | 
					
						
							|  |  |  |         # Calculate the time it takes to travel a distance with constant accel | 
					
						
							|  |  |  |         time_offset = start_velocity / accel | 
					
						
							|  |  |  |         return math.sqrt(2. * dist / accel + time_offset**2) - time_offset | 
					
						
							|  |  |  |     def set_max_jerk(self, max_halt_velocity, max_accel): | 
					
						
							|  |  |  |         # Calculate the firmware's maximum halt interval time | 
					
						
							|  |  |  |         last_step_time = self._dist_to_time( | 
					
						
							|  |  |  |             self.step_dist, max_halt_velocity, max_accel) | 
					
						
							|  |  |  |         second_last_step_time = self._dist_to_time( | 
					
						
							|  |  |  |             2. * self.step_dist, max_halt_velocity, max_accel) | 
					
						
							|  |  |  |         min_stop_interval = second_last_step_time - last_step_time | 
					
						
							| 
									
										
										
										
											2017-08-21 11:25:26 -04:00
										 |  |  |         self.mcu_stepper.setup_min_stop_interval(min_stop_interval) | 
					
						
							| 
									
										
										
										
											2017-09-12 12:47:40 -04:00
										 |  |  |     def motor_enable(self, print_time, enable=0): | 
					
						
							| 
									
										
										
										
											2017-07-24 13:54:46 -04:00
										 |  |  |         if enable and self.need_motor_enable: | 
					
						
							| 
									
										
										
										
											2017-09-12 12:47:40 -04:00
										 |  |  |             self.mcu_stepper.reset_step_clock(print_time) | 
					
						
							| 
									
										
										
										
											2017-07-24 13:54:46 -04:00
										 |  |  |         if (self.mcu_enable is not None | 
					
						
							|  |  |  |             and self.mcu_enable.get_last_setting() != enable): | 
					
						
							| 
									
										
										
										
											2017-09-12 12:47:40 -04:00
										 |  |  |             self.mcu_enable.set_digital(print_time, enable) | 
					
						
							| 
									
										
										
										
											2017-07-24 13:54:46 -04:00
										 |  |  |         self.need_motor_enable = not enable | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2017-11-06 18:37:35 -05:00
										 |  |  | # Support for stepper controlled linear axis with an endstop | 
					
						
							| 
									
										
										
										
											2017-07-24 13:54:46 -04:00
										 |  |  | class PrinterHomingStepper(PrinterStepper): | 
					
						
							| 
									
										
										
										
											2017-10-29 10:58:04 -04:00
										 |  |  |     def __init__(self, printer, config): | 
					
						
							|  |  |  |         PrinterStepper.__init__(self, printer, config) | 
					
						
							| 
									
										
										
										
											2017-11-06 18:37:35 -05:00
										 |  |  |         # Endstop and its position | 
					
						
							| 
									
										
										
										
											2017-08-21 11:25:26 -04:00
										 |  |  |         self.mcu_endstop = pins.setup_pin( | 
					
						
							|  |  |  |             printer, 'endstop', config.get('endstop_pin')) | 
					
						
							| 
									
										
										
										
											2017-07-24 13:54:46 -04:00
										 |  |  |         self.mcu_endstop.add_stepper(self.mcu_stepper) | 
					
						
							| 
									
										
										
										
											2017-11-06 18:37:35 -05:00
										 |  |  |         self.position_endstop = config.getfloat('position_endstop') | 
					
						
							|  |  |  |         # Axis range | 
					
						
							| 
									
										
										
										
											2017-07-24 13:54:46 -04:00
										 |  |  |         self.position_min = config.getfloat('position_min', 0.) | 
					
						
							|  |  |  |         self.position_max = config.getfloat( | 
					
						
							|  |  |  |             'position_max', 0., above=self.position_min) | 
					
						
							| 
									
										
										
										
											2017-11-06 18:37:35 -05:00
										 |  |  |         # Homing mechanics | 
					
						
							| 
									
										
										
										
											2017-04-11 11:37:09 -04:00
										 |  |  |         self.homing_speed = config.getfloat('homing_speed', 5.0, above=0.) | 
					
						
							| 
									
										
										
										
											2017-11-06 18:37:35 -05:00
										 |  |  |         self.homing_retract_dist = config.getfloat( | 
					
						
							|  |  |  |             'homing_retract_dist', 5., above=0.) | 
					
						
							| 
									
										
										
										
											2017-07-24 14:12:17 -04:00
										 |  |  |         self.homing_positive_dir = config.getboolean('homing_positive_dir', None) | 
					
						
							|  |  |  |         if self.homing_positive_dir is None: | 
					
						
							|  |  |  |             axis_len = self.position_max - self.position_min | 
					
						
							|  |  |  |             if self.position_endstop <= self.position_min + axis_len / 4.: | 
					
						
							|  |  |  |                 self.homing_positive_dir = False | 
					
						
							|  |  |  |             elif self.position_endstop >= self.position_max - axis_len / 4.: | 
					
						
							|  |  |  |                 self.homing_positive_dir = True | 
					
						
							|  |  |  |             else: | 
					
						
							|  |  |  |                 raise config.error( | 
					
						
							|  |  |  |                     "Unable to infer homing_positive_dir in section '%s'" % ( | 
					
						
							|  |  |  |                         config.section,)) | 
					
						
							| 
									
										
										
										
											2017-11-06 18:37:35 -05:00
										 |  |  |         # Endstop stepper phase position tracking | 
					
						
							| 
									
										
										
										
											2017-04-11 11:37:09 -04:00
										 |  |  |         self.homing_stepper_phases = config.getint( | 
					
						
							|  |  |  |             'homing_stepper_phases', None, minval=0) | 
					
						
							|  |  |  |         endstop_accuracy = config.getfloat( | 
					
						
							|  |  |  |             'homing_endstop_accuracy', None, above=0.) | 
					
						
							|  |  |  |         self.homing_endstop_accuracy = self.homing_endstop_phase = None | 
					
						
							| 
									
										
										
										
											2016-07-26 22:06:14 -04:00
										 |  |  |         if self.homing_stepper_phases: | 
					
						
							| 
									
										
										
										
											2017-04-11 11:37:09 -04:00
										 |  |  |             self.homing_endstop_phase = config.getint( | 
					
						
							|  |  |  |                 'homing_endstop_phase', None, minval=0 | 
					
						
							|  |  |  |                 , maxval=self.homing_stepper_phases-1) | 
					
						
							| 
									
										
										
										
											2017-10-03 19:32:08 -04:00
										 |  |  |             if self.homing_endstop_phase is not None: | 
					
						
							|  |  |  |                 # Adjust the endstop position so 0.0 is always at a full step | 
					
						
							|  |  |  |                 micro_steps = self.homing_stepper_phases // 4 | 
					
						
							|  |  |  |                 phase_offset = ( | 
					
						
							|  |  |  |                     ((self.homing_endstop_phase + micro_steps // 2) % micro_steps) | 
					
						
							|  |  |  |                     - micro_steps // 2) * self.step_dist | 
					
						
							|  |  |  |                 full_step = micro_steps * self.step_dist | 
					
						
							|  |  |  |                 es_pos = (int(self.position_endstop / full_step + .5) * full_step | 
					
						
							|  |  |  |                           + phase_offset) | 
					
						
							|  |  |  |                 if es_pos != self.position_endstop: | 
					
						
							|  |  |  |                     logging.info("Changing %s endstop position to %.3f" | 
					
						
							|  |  |  |                                  " (from %.3f)", self.name, es_pos, | 
					
						
							|  |  |  |                                  self.position_endstop) | 
					
						
							|  |  |  |                     self.position_endstop = es_pos | 
					
						
							| 
									
										
										
										
											2016-07-26 22:06:14 -04:00
										 |  |  |             if endstop_accuracy is None: | 
					
						
							|  |  |  |                 self.homing_endstop_accuracy = self.homing_stepper_phases//2 - 1 | 
					
						
							|  |  |  |             elif self.homing_endstop_phase is not None: | 
					
						
							|  |  |  |                 self.homing_endstop_accuracy = int(math.ceil( | 
					
						
							| 
									
										
										
										
											2017-11-07 11:51:27 -05:00
										 |  |  |                     endstop_accuracy * .5 / self.step_dist)) | 
					
						
							| 
									
										
										
										
											2016-07-26 22:06:14 -04:00
										 |  |  |             else: | 
					
						
							|  |  |  |                 self.homing_endstop_accuracy = int(math.ceil( | 
					
						
							| 
									
										
										
										
											2017-11-07 11:51:27 -05:00
										 |  |  |                     endstop_accuracy / self.step_dist)) | 
					
						
							| 
									
										
										
										
											2017-06-09 23:39:46 -04:00
										 |  |  |             if self.homing_endstop_accuracy >= self.homing_stepper_phases // 2: | 
					
						
							| 
									
										
										
										
											2016-07-26 22:06:14 -04:00
										 |  |  |                 logging.info("Endstop for %s is not accurate enough for stepper" | 
					
						
							| 
									
										
										
										
											2017-09-27 11:43:14 -04:00
										 |  |  |                              " phase adjustment", name) | 
					
						
							| 
									
										
										
										
											2016-07-26 22:06:14 -04:00
										 |  |  |                 self.homing_stepper_phases = None | 
					
						
							| 
									
										
										
										
											2017-09-13 12:06:08 -04:00
										 |  |  |             if self.mcu_endstop.get_mcu().is_fileoutput(): | 
					
						
							| 
									
										
										
										
											2017-03-31 14:38:09 -04:00
										 |  |  |                 self.homing_endstop_accuracy = self.homing_stepper_phases | 
					
						
							| 
									
										
										
										
											2017-08-29 17:37:14 -04:00
										 |  |  |     def get_homing_speed(self): | 
					
						
							|  |  |  |         # Round the configured homing speed so that it is an even | 
					
						
							|  |  |  |         # number of ticks per step. | 
					
						
							| 
									
										
										
										
											2017-09-15 12:28:55 -04:00
										 |  |  |         adjusted_freq = self.mcu_stepper.get_mcu().get_adjusted_freq() | 
					
						
							|  |  |  |         dist_ticks = adjusted_freq * self.step_dist | 
					
						
							| 
									
										
										
										
											2017-08-29 17:37:14 -04:00
										 |  |  |         ticks_per_step = round(dist_ticks / self.homing_speed) | 
					
						
							|  |  |  |         return dist_ticks / ticks_per_step | 
					
						
							| 
									
										
										
										
											2016-10-13 10:04:30 -04:00
										 |  |  |     def get_homed_offset(self): | 
					
						
							| 
									
										
										
										
											2016-11-14 13:40:35 -05:00
										 |  |  |         if not self.homing_stepper_phases or self.need_motor_enable: | 
					
						
							| 
									
										
										
										
											2016-10-13 10:04:30 -04:00
										 |  |  |             return 0 | 
					
						
							| 
									
										
										
										
											2016-12-08 18:12:20 -05:00
										 |  |  |         pos = self.mcu_stepper.get_mcu_position() | 
					
						
							| 
									
										
										
										
											2016-07-26 22:06:14 -04:00
										 |  |  |         pos %= self.homing_stepper_phases | 
					
						
							|  |  |  |         if self.homing_endstop_phase is None: | 
					
						
							| 
									
										
										
										
											2017-09-27 11:43:14 -04:00
										 |  |  |             logging.info("Setting %s endstop phase to %d", self.name, pos) | 
					
						
							| 
									
										
										
										
											2016-07-26 22:06:14 -04:00
										 |  |  |             self.homing_endstop_phase = pos | 
					
						
							| 
									
										
										
										
											2016-10-13 10:04:30 -04:00
										 |  |  |             return 0 | 
					
						
							| 
									
										
										
										
											2016-07-26 22:06:14 -04:00
										 |  |  |         delta = (pos - self.homing_endstop_phase) % self.homing_stepper_phases | 
					
						
							|  |  |  |         if delta >= self.homing_stepper_phases - self.homing_endstop_accuracy: | 
					
						
							|  |  |  |             delta -= self.homing_stepper_phases | 
					
						
							|  |  |  |         elif delta > self.homing_endstop_accuracy: | 
					
						
							| 
									
										
										
										
											2016-11-18 14:35:31 -05:00
										 |  |  |             raise homing.EndstopError( | 
					
						
							|  |  |  |                 "Endstop %s incorrect phase (got %d vs %d)" % ( | 
					
						
							|  |  |  |                     self.name, pos, self.homing_endstop_phase)) | 
					
						
							| 
									
										
										
										
											2017-04-04 19:20:54 -04:00
										 |  |  |         return delta * self.step_dist |