| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  | # Support for a manual controlled stepper | 
					
						
							|  |  |  | # | 
					
						
							|  |  |  | # Copyright (C) 2019  Kevin O'Connor <kevin@koconnor.net> | 
					
						
							|  |  |  | # | 
					
						
							|  |  |  | # This file may be distributed under the terms of the GNU GPLv3 license. | 
					
						
							| 
									
										
										
										
											2019-03-03 13:23:45 -05:00
										 |  |  | import stepper, homing, force_move, chelper | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  | 
 | 
					
						
							|  |  |  | ENDSTOP_SAMPLE_TIME = .000015 | 
					
						
							|  |  |  | ENDSTOP_SAMPLE_COUNT = 4 | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | class ManualStepper: | 
					
						
							|  |  |  |     def __init__(self, config): | 
					
						
							|  |  |  |         self.printer = config.get_printer() | 
					
						
							|  |  |  |         if config.get('endstop_pin', None) is not None: | 
					
						
							|  |  |  |             self.can_home = True | 
					
						
							| 
									
										
										
										
											2019-11-12 12:49:21 -05:00
										 |  |  |             self.rail = stepper.PrinterRail( | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |                 config, need_position_minmax=False, default_position_endstop=0.) | 
					
						
							| 
									
										
										
										
											2019-11-12 12:49:21 -05:00
										 |  |  |             self.steppers = self.rail.get_steppers() | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |         else: | 
					
						
							|  |  |  |             self.can_home = False | 
					
						
							| 
									
										
										
										
											2019-11-12 12:49:21 -05:00
										 |  |  |             self.rail = stepper.PrinterStepper(config) | 
					
						
							|  |  |  |             self.steppers = [self.rail] | 
					
						
							| 
									
										
										
										
											2019-03-03 13:23:45 -05:00
										 |  |  |         self.velocity = config.getfloat('velocity', 5., above=0.) | 
					
						
							|  |  |  |         self.accel = config.getfloat('accel', 0., minval=0.) | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |         self.next_cmd_time = 0. | 
					
						
							|  |  |  |         # Setup iterative solver | 
					
						
							|  |  |  |         ffi_main, ffi_lib = chelper.get_ffi() | 
					
						
							| 
									
										
										
										
											2019-10-29 11:34:52 -04:00
										 |  |  |         self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free) | 
					
						
							| 
									
										
										
										
											2019-10-29 12:44:39 -04:00
										 |  |  |         self.trapq_append = ffi_lib.trapq_append | 
					
						
							| 
									
										
										
										
											2019-10-29 11:34:52 -04:00
										 |  |  |         self.trapq_free_moves = ffi_lib.trapq_free_moves | 
					
						
							| 
									
										
										
										
											2019-11-12 12:49:21 -05:00
										 |  |  |         self.rail.setup_itersolve('cartesian_stepper_alloc', 'x') | 
					
						
							|  |  |  |         self.rail.set_trapq(self.trapq) | 
					
						
							|  |  |  |         self.rail.set_max_jerk(9999999.9, 9999999.9) | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |         # Register commands | 
					
						
							|  |  |  |         stepper_name = config.get_name().split()[1] | 
					
						
							|  |  |  |         self.gcode = self.printer.lookup_object('gcode') | 
					
						
							|  |  |  |         self.gcode.register_mux_command('MANUAL_STEPPER', "STEPPER", | 
					
						
							|  |  |  |                                         stepper_name, self.cmd_MANUAL_STEPPER, | 
					
						
							|  |  |  |                                         desc=self.cmd_MANUAL_STEPPER_help) | 
					
						
							|  |  |  |     def sync_print_time(self): | 
					
						
							|  |  |  |         toolhead = self.printer.lookup_object('toolhead') | 
					
						
							|  |  |  |         print_time = toolhead.get_last_move_time() | 
					
						
							|  |  |  |         if self.next_cmd_time > print_time: | 
					
						
							|  |  |  |             toolhead.dwell(self.next_cmd_time - print_time) | 
					
						
							|  |  |  |         else: | 
					
						
							|  |  |  |             self.next_cmd_time = print_time | 
					
						
							|  |  |  |     def do_enable(self, enable): | 
					
						
							|  |  |  |         self.sync_print_time() | 
					
						
							| 
									
										
										
										
											2019-11-12 12:49:21 -05:00
										 |  |  |         stepper_enable = self.printer.lookup_object('stepper_enable') | 
					
						
							|  |  |  |         if enable: | 
					
						
							|  |  |  |             for s in self.steppers: | 
					
						
							| 
									
										
										
										
											2019-11-12 13:55:50 -05:00
										 |  |  |                 se = stepper_enable.lookup_enable(s.get_name()) | 
					
						
							|  |  |  |                 se.motor_enable(self.next_cmd_time) | 
					
						
							| 
									
										
										
										
											2019-11-12 12:49:21 -05:00
										 |  |  |         else: | 
					
						
							|  |  |  |             for s in self.steppers: | 
					
						
							| 
									
										
										
										
											2019-11-12 13:55:50 -05:00
										 |  |  |                 se = stepper_enable.lookup_enable(s.get_name()) | 
					
						
							|  |  |  |                 se.motor_disable(self.next_cmd_time) | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |         self.sync_print_time() | 
					
						
							|  |  |  |     def do_set_position(self, setpos): | 
					
						
							| 
									
										
										
										
											2019-11-12 12:49:21 -05:00
										 |  |  |         self.rail.set_position([setpos, 0., 0.]) | 
					
						
							| 
									
										
										
										
											2019-03-03 13:23:45 -05:00
										 |  |  |     def do_move(self, movepos, speed, accel): | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |         self.sync_print_time() | 
					
						
							| 
									
										
										
										
											2019-11-12 12:49:21 -05:00
										 |  |  |         cp = self.rail.get_commanded_position() | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |         dist = movepos - cp | 
					
						
							| 
									
										
										
										
											2019-11-06 08:41:50 -05:00
										 |  |  |         axis_r, accel_t, cruise_t, cruise_v = force_move.calc_move_time( | 
					
						
							| 
									
										
										
										
											2019-03-03 13:23:45 -05:00
										 |  |  |             dist, speed, accel) | 
					
						
							| 
									
										
										
										
											2019-10-29 12:44:39 -04:00
										 |  |  |         self.trapq_append(self.trapq, self.next_cmd_time, | 
					
						
							|  |  |  |                           accel_t, cruise_t, accel_t, | 
					
						
							| 
									
										
										
										
											2019-11-06 08:41:50 -05:00
										 |  |  |                           cp, 0., 0., axis_r, 0., 0., | 
					
						
							| 
									
										
										
										
											2019-10-29 12:44:39 -04:00
										 |  |  |                           0., cruise_v, accel) | 
					
						
							| 
									
										
										
										
											2019-11-24 10:59:13 -05:00
										 |  |  |         self.next_cmd_time = self.next_cmd_time + accel_t + cruise_t + accel_t | 
					
						
							| 
									
										
										
										
											2019-11-12 12:49:21 -05:00
										 |  |  |         self.rail.generate_steps(self.next_cmd_time) | 
					
						
							| 
									
										
										
										
											2019-11-24 10:59:13 -05:00
										 |  |  |         self.trapq_free_moves(self.trapq, self.next_cmd_time + 99999.9) | 
					
						
							|  |  |  |         toolhead = self.printer.lookup_object('toolhead') | 
					
						
							|  |  |  |         toolhead.note_kinematic_activity(self.next_cmd_time) | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |         self.sync_print_time() | 
					
						
							| 
									
										
										
										
											2019-03-03 13:23:45 -05:00
										 |  |  |     def do_homing_move(self, movepos, speed, accel, triggered): | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |         if not self.can_home: | 
					
						
							|  |  |  |             raise self.gcode.error("No endstop for this manual stepper") | 
					
						
							|  |  |  |         # Start endstop checking | 
					
						
							|  |  |  |         self.sync_print_time() | 
					
						
							| 
									
										
										
										
											2020-02-12 13:03:42 -05:00
										 |  |  |         endstops = self.rail.get_endstops() | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |         for mcu_endstop, name in endstops: | 
					
						
							|  |  |  |             min_step_dist = min([s.get_step_dist() | 
					
						
							|  |  |  |                                  for s in mcu_endstop.get_steppers()]) | 
					
						
							|  |  |  |             mcu_endstop.home_start( | 
					
						
							|  |  |  |                 self.next_cmd_time, ENDSTOP_SAMPLE_TIME, ENDSTOP_SAMPLE_COUNT, | 
					
						
							|  |  |  |                 min_step_dist / speed, triggered=triggered) | 
					
						
							|  |  |  |         # Issue move | 
					
						
							| 
									
										
										
										
											2019-03-03 13:23:45 -05:00
										 |  |  |         self.do_move(movepos, speed, accel) | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |         # Wait for endstops to trigger | 
					
						
							|  |  |  |         error = None | 
					
						
							|  |  |  |         for mcu_endstop, name in endstops: | 
					
						
							|  |  |  |             try: | 
					
						
							|  |  |  |                 mcu_endstop.home_wait(self.next_cmd_time) | 
					
						
							|  |  |  |             except mcu_endstop.TimeoutError as e: | 
					
						
							|  |  |  |                 if error is None: | 
					
						
							|  |  |  |                     error = "Failed to home %s: %s" % (name, str(e)) | 
					
						
							|  |  |  |         self.sync_print_time() | 
					
						
							|  |  |  |         if error is not None: | 
					
						
							| 
									
										
										
										
											2019-06-03 13:01:57 -04:00
										 |  |  |             raise homing.CommandError(error) | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |     cmd_MANUAL_STEPPER_help = "Command a manually configured stepper" | 
					
						
							|  |  |  |     def cmd_MANUAL_STEPPER(self, params): | 
					
						
							|  |  |  |         if 'ENABLE' in params: | 
					
						
							|  |  |  |             self.do_enable(self.gcode.get_int('ENABLE', params)) | 
					
						
							|  |  |  |         if 'SET_POSITION' in params: | 
					
						
							|  |  |  |             setpos = self.gcode.get_float('SET_POSITION', params) | 
					
						
							|  |  |  |             self.do_set_position(setpos) | 
					
						
							|  |  |  |         homing_move = self.gcode.get_int('STOP_ON_ENDSTOP', params, 0) | 
					
						
							| 
									
										
										
										
											2019-03-03 13:23:45 -05:00
										 |  |  |         speed = self.gcode.get_float('SPEED', params, self.velocity, above=0.) | 
					
						
							|  |  |  |         accel = self.gcode.get_float('ACCEL', params, self.accel, minval=0.) | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |         if homing_move: | 
					
						
							|  |  |  |             movepos = self.gcode.get_float('MOVE', params) | 
					
						
							| 
									
										
										
										
											2019-03-03 13:23:45 -05:00
										 |  |  |             self.do_homing_move(movepos, speed, accel, homing_move > 0) | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  |         elif 'MOVE' in params: | 
					
						
							|  |  |  |             movepos = self.gcode.get_float('MOVE', params) | 
					
						
							| 
									
										
										
										
											2019-03-03 13:23:45 -05:00
										 |  |  |             self.do_move(movepos, speed, accel) | 
					
						
							| 
									
										
										
										
											2019-02-08 20:11:05 -05:00
										 |  |  | 
 | 
					
						
							|  |  |  | def load_config_prefix(config): | 
					
						
							|  |  |  |     return ManualStepper(config) |