1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
|
# 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.
import math
# Coordinates created by this are converted into G1 commands.
#
# 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
class ArcSupport:
def __init__(self, config):
self.printer = config.get_printer()
self.mm_per_arc_segment = config.getfloat('resolution', 1., above=0.0)
self.gcode_move = self.printer.load_object(config, 'gcode_move')
self.gcode = self.printer.lookup_object('gcode')
self.gcode.register_command("G2", self.cmd_G2)
self.gcode.register_command("G3", self.cmd_G3)
self.gcode.register_command("G17", self.cmd_G17)
self.gcode.register_command("G18", self.cmd_G18)
self.gcode.register_command("G19", self.cmd_G19)
# backwards compatibility, prior implementation only supported XY
self.plane = ARC_PLANE_X_Y
def cmd_G2(self, gcmd):
self._cmd_inner(gcmd, True)
def cmd_G3(self, gcmd):
self._cmd_inner(gcmd, False)
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
def _cmd_inner(self, gcmd, clockwise):
gcodestatus = self.gcode_move.get_status()
if not gcodestatus['absolute_coordinates']:
raise gcmd.error("G2/G3 does not support relative move mode")
currentPos = gcodestatus['gcode_position']
absolut_extrude = gcodestatus['absolute_extrude']
# Parse parameters
asTarget = [gcmd.get_float("X", currentPos[0]),
gcmd.get_float("Y", currentPos[1]),
gcmd.get_float("Z", currentPos[2])]
if gcmd.get_float("R", None) is not None:
raise gcmd.error("G2/G3 does not support R moves")
# determine the plane coordinates and the helical axis
I = gcmd.get_float('I', 0.)
J = gcmd.get_float('J', 0.)
asPlanar = (I, J)
axes = (X_AXIS, Y_AXIS, Z_AXIS)
if self.plane == ARC_PLANE_X_Z:
K = gcmd.get_float('K', 0.)
asPlanar = (I, K)
axes = (X_AXIS, Z_AXIS, Y_AXIS)
elif self.plane == ARC_PLANE_Y_Z:
K = gcmd.get_float('K', 0.)
asPlanar = (J, K)
axes = (Y_AXIS, Z_AXIS, X_AXIS)
if not (asPlanar[0] or asPlanar[1]):
raise gcmd.error("G2/G3 requires IJ, IK or JK parameters")
# Build linear coordinates to move
self.planArc(currentPos, asTarget, asPlanar, clockwise,
gcmd, absolut_extrude, *axes)
# 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
#
# alpha and beta axes are the current plane, helical axis is linear travel
def planArc(self, currentPos, targetPos, offset, clockwise,
gcmd, absolut_extrude,
alpha_axis, beta_axis, helical_axis):
# todo: sometimes produces full circles
# Radius vector from center to current location
r_P = -offset[0]
r_Q = -offset[1]
# Determine angular travel
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)
if angular_travel < 0.:
angular_travel += 2. * math.pi
if clockwise:
angular_travel -= 2. * math.pi
if (angular_travel == 0.
and currentPos[alpha_axis] == targetPos[alpha_axis]
and currentPos[beta_axis] == targetPos[beta_axis]):
# Make a circle if the angular rotation is 0 and the
# target is current position
angular_travel = 2. * math.pi
# Determine number of segments
linear_travel = targetPos[helical_axis] - currentPos[helical_axis]
radius = math.hypot(r_P, r_Q)
flat_mm = radius * angular_travel
if linear_travel:
mm_of_travel = math.hypot(flat_mm, linear_travel)
else:
mm_of_travel = math.fabs(flat_mm)
segments = max(1., math.floor(mm_of_travel / self.mm_per_arc_segment))
# Generate coordinates
theta_per_segment = angular_travel / segments
linear_per_segment = linear_travel / segments
asE = gcmd.get_float("E", None)
asF = gcmd.get_float("F", None)
e_per_move = e_base = 0.
if asE is not None:
if absolut_extrude:
e_base = currentPos[3]
e_per_move = (asE - e_base) / segments
for i in range(1, int(segments) + 1):
dist_Helical = i * linear_per_segment
c_theta = i * theta_per_segment
cos_Ti = math.cos(c_theta)
sin_Ti = math.sin(c_theta)
r_P = -offset[0] * cos_Ti + offset[1] * sin_Ti
r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti
c = [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
if i == segments:
c = targetPos
# Convert coords into G1 commands
g1_params = {'X': c[0], 'Y': c[1], 'Z': c[2]}
if e_per_move:
g1_params['E'] = e_base + e_per_move
if absolut_extrude:
e_base += e_per_move
if asF is not None:
g1_params['F'] = asF
g1_gcmd = self.gcode.create_gcode_command("G1", "G1", g1_params)
self.gcode_move.cmd_G1(g1_gcmd)
def load_config(config):
return ArcSupport(config)
|
