| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  | # adds support fro ARC commands via G2/G3 | 
					
						
							|  |  |  | # | 
					
						
							|  |  |  | # Copyright (C) 2019  Aleksej Vasiljkovic <achmed21@gmail.com> | 
					
						
							|  |  |  | # | 
					
						
							|  |  |  | # function planArc() originates from https://github.com/MarlinFirmware/Marlin | 
					
						
							|  |  |  | # Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm | 
					
						
							|  |  |  | # | 
					
						
							|  |  |  | # This file may be distributed under the terms of the GNU GPLv3 license. | 
					
						
							| 
									
										
										
										
											2020-04-22 14:30:52 -04:00
										 |  |  | import math | 
					
						
							| 
									
										
										
										
											2022-12-11 20:59:02 -05:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2020-04-22 14:30:52 -04:00
										 |  |  | # Coordinates created by this are converted into G1 commands. | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  | # | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  | # supports XY, XZ & YZ planes with remaining axis as helical | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | # Enum | 
					
						
							|  |  |  | ARC_PLANE_X_Y = 0 | 
					
						
							|  |  |  | ARC_PLANE_X_Z = 1 | 
					
						
							|  |  |  | ARC_PLANE_Y_Z = 2 | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | # Enum | 
					
						
							|  |  |  | X_AXIS = 0 | 
					
						
							|  |  |  | Y_AXIS = 1 | 
					
						
							|  |  |  | Z_AXIS = 2 | 
					
						
							|  |  |  | E_AXIS = 3 | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  | 
 | 
					
						
							|  |  |  | class ArcSupport: | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  |     def __init__(self, config): | 
					
						
							|  |  |  |         self.printer = config.get_printer() | 
					
						
							| 
									
										
										
										
											2020-04-22 14:30:52 -04:00
										 |  |  |         self.mm_per_arc_segment = config.getfloat('resolution', 1., above=0.0) | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2020-08-17 00:05:41 -04:00
										 |  |  |         self.gcode_move = self.printer.load_object(config, 'gcode_move') | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  |         self.gcode = self.printer.lookup_object('gcode') | 
					
						
							| 
									
										
										
										
											2020-04-22 14:30:52 -04:00
										 |  |  |         self.gcode.register_command("G2", self.cmd_G2) | 
					
						
							| 
									
										
										
										
											2022-01-23 02:47:51 +01:00
										 |  |  |         self.gcode.register_command("G3", self.cmd_G3) | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  |         self.gcode.register_command("G17", self.cmd_G17) | 
					
						
							|  |  |  |         self.gcode.register_command("G18", self.cmd_G18) | 
					
						
							|  |  |  |         self.gcode.register_command("G19", self.cmd_G19) | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2022-12-11 20:59:02 -05:00
										 |  |  |         self.Coord = self.gcode.Coord | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  |         # backwards compatibility, prior implementation only supported XY | 
					
						
							|  |  |  |         self.plane = ARC_PLANE_X_Y | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2020-04-24 13:52:13 -04:00
										 |  |  |     def cmd_G2(self, gcmd): | 
					
						
							| 
									
										
										
										
											2022-01-23 02:47:51 +01:00
										 |  |  |         self._cmd_inner(gcmd, True) | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  |     def cmd_G3(self, gcmd): | 
					
						
							|  |  |  |         self._cmd_inner(gcmd, False) | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  |     def cmd_G17(self, gcmd): | 
					
						
							|  |  |  |         self.plane = ARC_PLANE_X_Y | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  |     def cmd_G18(self, gcmd): | 
					
						
							|  |  |  |         self.plane = ARC_PLANE_X_Z | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  |     def cmd_G19(self, gcmd): | 
					
						
							|  |  |  |         self.plane = ARC_PLANE_Y_Z | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2022-01-23 02:47:51 +01:00
										 |  |  |     def _cmd_inner(self, gcmd, clockwise): | 
					
						
							| 
									
										
										
										
											2020-08-05 11:43:45 -04:00
										 |  |  |         gcodestatus = self.gcode_move.get_status() | 
					
						
							| 
									
										
										
										
											2020-04-22 15:20:59 -04:00
										 |  |  |         if not gcodestatus['absolute_coordinates']: | 
					
						
							| 
									
										
										
										
											2020-08-05 11:43:45 -04:00
										 |  |  |             raise gcmd.error("G2/G3 does not support relative move mode") | 
					
						
							| 
									
										
										
										
											2020-04-22 15:20:59 -04:00
										 |  |  |         currentPos = gcodestatus['gcode_position'] | 
					
						
							| 
									
										
										
										
											2020-04-22 14:30:52 -04:00
										 |  |  | 
 | 
					
						
							|  |  |  |         # Parse parameters | 
					
						
							| 
									
										
										
										
											2022-12-11 20:59:02 -05:00
										 |  |  |         asTarget = self.Coord(x=gcmd.get_float("X", currentPos[0]), | 
					
						
							|  |  |  |                               y=gcmd.get_float("Y", currentPos[1]), | 
					
						
							|  |  |  |                               z=gcmd.get_float("Z", currentPos[2]), | 
					
						
							|  |  |  |                               e=None) | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2020-04-24 13:52:13 -04:00
										 |  |  |         if gcmd.get_float("R", None) is not None: | 
					
						
							|  |  |  |             raise gcmd.error("G2/G3 does not support R moves") | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  | 
 | 
					
						
							|  |  |  |         # determine the plane coordinates and the helical axis | 
					
						
							|  |  |  |         asPlanar = [ gcmd.get_float(a, 0.) for i,a in enumerate('IJ') ] | 
					
						
							|  |  |  |         axes = (X_AXIS, Y_AXIS, Z_AXIS) | 
					
						
							|  |  |  |         if self.plane == ARC_PLANE_X_Z: | 
					
						
							|  |  |  |             asPlanar = [ gcmd.get_float(a, 0.) for i,a in enumerate('IK') ] | 
					
						
							|  |  |  |             axes = (X_AXIS, Z_AXIS, Y_AXIS) | 
					
						
							|  |  |  |         elif self.plane == ARC_PLANE_Y_Z: | 
					
						
							|  |  |  |             asPlanar = [ gcmd.get_float(a, 0.) for i,a in enumerate('JK') ] | 
					
						
							|  |  |  |             axes = (Y_AXIS, Z_AXIS, X_AXIS) | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  |         if not asPlanar[0] or not asPlanar[1]: | 
					
						
							|  |  |  |             raise gcmd.error("G2/G3 requires IJ, IK or JK parameters") | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2020-04-24 13:52:13 -04:00
										 |  |  |         asE = gcmd.get_float("E", None) | 
					
						
							|  |  |  |         asF = gcmd.get_float("F", None) | 
					
						
							| 
									
										
										
										
											2020-04-22 14:30:52 -04:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  |         # Build list of linear coordinates to move | 
					
						
							|  |  |  |         coords = self.planArc(currentPos, asTarget, asPlanar, | 
					
						
							|  |  |  |                               clockwise, *axes) | 
					
						
							| 
									
										
										
										
											2020-06-04 15:19:08 -04:00
										 |  |  |         e_per_move = e_base = 0. | 
					
						
							|  |  |  |         if asE is not None: | 
					
						
							|  |  |  |             if gcodestatus['absolute_extrude']: | 
					
						
							|  |  |  |                 e_base = currentPos[3] | 
					
						
							|  |  |  |             e_per_move = (asE - e_base) / len(coords) | 
					
						
							| 
									
										
										
										
											2020-04-22 14:30:52 -04:00
										 |  |  | 
 | 
					
						
							|  |  |  |         # Convert coords into G1 commands | 
					
						
							|  |  |  |         for coord in coords: | 
					
						
							| 
									
										
										
										
											2020-04-22 15:20:59 -04:00
										 |  |  |             g1_params = {'X': coord[0], 'Y': coord[1], 'Z': coord[2]} | 
					
						
							| 
									
										
										
										
											2020-06-04 15:19:08 -04:00
										 |  |  |             if e_per_move: | 
					
						
							|  |  |  |                 g1_params['E'] = e_base + e_per_move | 
					
						
							| 
									
										
										
										
											2020-08-08 02:15:03 +10:00
										 |  |  |                 if gcodestatus['absolute_extrude']: | 
					
						
							|  |  |  |                     e_base += e_per_move | 
					
						
							| 
									
										
										
										
											2020-04-22 14:30:52 -04:00
										 |  |  |             if asF is not None: | 
					
						
							|  |  |  |                 g1_params['F'] = asF | 
					
						
							| 
									
										
										
										
											2020-04-22 12:40:32 -04:00
										 |  |  |             g1_gcmd = self.gcode.create_gcode_command("G1", "G1", g1_params) | 
					
						
							| 
									
										
										
										
											2020-08-05 11:43:45 -04:00
										 |  |  |             self.gcode_move.cmd_G1(g1_gcmd) | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  | 
 | 
					
						
							|  |  |  |     # function planArc() originates from marlin plan_arc() | 
					
						
							|  |  |  |     # https://github.com/MarlinFirmware/Marlin | 
					
						
							|  |  |  |     # | 
					
						
							|  |  |  |     # The arc is approximated by generating many small linear segments. | 
					
						
							|  |  |  |     # The length of each segment is configured in MM_PER_ARC_SEGMENT | 
					
						
							|  |  |  |     # Arcs smaller then this value, will be a Line only | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  |     # | 
					
						
							|  |  |  |     # alpha and beta axes are the current plane, helical axis is linear travel | 
					
						
							|  |  |  |     def planArc(self, currentPos, targetPos, offset, clockwise, | 
					
						
							|  |  |  |                 alpha_axis, beta_axis, helical_axis): | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  |         # todo: sometimes produces full circles | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  |         # Radius vector from center to current location | 
					
						
							| 
									
										
										
										
											2020-04-22 17:36:58 -04:00
										 |  |  |         r_P = -offset[0] | 
					
						
							|  |  |  |         r_Q = -offset[1] | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2020-04-22 17:36:58 -04:00
										 |  |  |         # Determine angular travel | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  |         center_P = currentPos[alpha_axis] - r_P | 
					
						
							|  |  |  |         center_Q = currentPos[beta_axis] - r_Q | 
					
						
							|  |  |  |         rt_Alpha = targetPos[alpha_axis] - center_P | 
					
						
							|  |  |  |         rt_Beta = targetPos[beta_axis] - center_Q | 
					
						
							|  |  |  |         angular_travel = math.atan2(r_P * rt_Beta - r_Q * rt_Alpha, | 
					
						
							|  |  |  |                                     r_P * rt_Alpha + r_Q * rt_Beta) | 
					
						
							| 
									
										
										
										
											2020-04-22 17:36:58 -04:00
										 |  |  |         if angular_travel < 0.: | 
					
						
							|  |  |  |             angular_travel += 2. * math.pi | 
					
						
							|  |  |  |         if clockwise: | 
					
						
							|  |  |  |             angular_travel -= 2. * math.pi | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2020-04-22 17:36:58 -04:00
										 |  |  |         if (angular_travel == 0. | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  |             and currentPos[alpha_axis] == targetPos[alpha_axis] | 
					
						
							|  |  |  |             and currentPos[beta_axis] == targetPos[beta_axis]): | 
					
						
							| 
									
										
										
										
											2020-04-22 17:36:58 -04:00
										 |  |  |             # Make a circle if the angular rotation is 0 and the | 
					
						
							|  |  |  |             # target is current position | 
					
						
							|  |  |  |             angular_travel = 2. * math.pi | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2020-04-22 17:36:58 -04:00
										 |  |  |         # Determine number of segments | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  |         linear_travel = targetPos[helical_axis] - currentPos[helical_axis] | 
					
						
							| 
									
										
										
										
											2020-04-22 17:36:58 -04:00
										 |  |  |         radius = math.hypot(r_P, r_Q) | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  |         flat_mm = radius * angular_travel | 
					
						
							| 
									
										
										
										
											2020-04-22 17:36:58 -04:00
										 |  |  |         if linear_travel: | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  |             mm_of_travel = math.hypot(flat_mm, linear_travel) | 
					
						
							|  |  |  |         else: | 
					
						
							| 
									
										
										
										
											2020-04-22 17:36:58 -04:00
										 |  |  |             mm_of_travel = math.fabs(flat_mm) | 
					
						
							|  |  |  |         segments = max(1., math.floor(mm_of_travel / self.mm_per_arc_segment)) | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2020-04-22 17:36:58 -04:00
										 |  |  |         # Generate coordinates | 
					
						
							|  |  |  |         theta_per_segment = angular_travel / segments | 
					
						
							|  |  |  |         linear_per_segment = linear_travel / segments | 
					
						
							|  |  |  |         coords = [] | 
					
						
							|  |  |  |         for i in range(1, int(segments)): | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  |             dist_Helical = i * linear_per_segment | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  |             cos_Ti = math.cos(i * theta_per_segment) | 
					
						
							|  |  |  |             sin_Ti = math.sin(i * theta_per_segment) | 
					
						
							|  |  |  |             r_P = -offset[0] * cos_Ti + offset[1] * sin_Ti | 
					
						
							|  |  |  |             r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti | 
					
						
							|  |  |  | 
 | 
					
						
							| 
									
										
										
										
											2022-11-22 23:22:46 -05:00
										 |  |  |             # Coord doesn't support index assignment, create list | 
					
						
							|  |  |  |             c = [None, None, None, None] | 
					
						
							|  |  |  |             c[alpha_axis] = center_P + r_P | 
					
						
							|  |  |  |             c[beta_axis] = center_Q + r_Q | 
					
						
							|  |  |  |             c[helical_axis] = currentPos[helical_axis] + dist_Helical | 
					
						
							| 
									
										
										
										
											2022-12-11 20:59:02 -05:00
										 |  |  |             coords.append(self.Coord(*c)) | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  | 
 | 
					
						
							| 
									
										
										
										
											2020-04-22 17:36:58 -04:00
										 |  |  |         coords.append(targetPos) | 
					
						
							| 
									
										
										
										
											2019-08-21 21:44:45 +02:00
										 |  |  |         return coords | 
					
						
							|  |  |  | 
 | 
					
						
							|  |  |  | def load_config(config): | 
					
						
							|  |  |  |     return ArcSupport(config) |