puda-drivers 0.0.2__py3-none-any.whl → 0.0.4__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- puda_drivers/core/serialcontroller.py +59 -44
- puda_drivers/move/gcode.py +410 -145
- puda_drivers/transfer/liquid/sartorius/sartorius.py +289 -149
- {puda_drivers-0.0.2.dist-info → puda_drivers-0.0.4.dist-info}/METADATA +1 -3
- {puda_drivers-0.0.2.dist-info → puda_drivers-0.0.4.dist-info}/RECORD +7 -7
- {puda_drivers-0.0.2.dist-info → puda_drivers-0.0.4.dist-info}/WHEEL +0 -0
- {puda_drivers-0.0.2.dist-info → puda_drivers-0.0.4.dist-info}/licenses/LICENSE +0 -0
puda_drivers/move/gcode.py
CHANGED
|
@@ -1,22 +1,64 @@
|
|
|
1
1
|
"""
|
|
2
|
-
|
|
3
|
-
via serial communication.
|
|
2
|
+
G-code controller for motion systems.
|
|
4
3
|
|
|
5
|
-
|
|
4
|
+
This module provides a Python interface for controlling G-code compatible motion
|
|
5
|
+
systems (e.g., QuBot) via serial communication. All movements are executed in
|
|
6
|
+
absolute coordinates, with relative moves converted to absolute internally.
|
|
7
|
+
Supports homing and position synchronization.
|
|
6
8
|
"""
|
|
7
9
|
|
|
8
10
|
import re
|
|
9
|
-
from typing import Optional, Dict, Tuple
|
|
10
11
|
import logging
|
|
12
|
+
from dataclasses import dataclass
|
|
13
|
+
from typing import Optional, Dict, Tuple, Union
|
|
14
|
+
|
|
11
15
|
from puda_drivers.core.serialcontroller import SerialController
|
|
12
16
|
|
|
13
17
|
|
|
18
|
+
@dataclass
|
|
19
|
+
class AxisLimits:
|
|
20
|
+
"""Holds min/max limits for an axis."""
|
|
21
|
+
|
|
22
|
+
min: float
|
|
23
|
+
max: float
|
|
24
|
+
|
|
25
|
+
def validate(self, value: float) -> None:
|
|
26
|
+
"""
|
|
27
|
+
Validate that a value is within the axis limits.
|
|
28
|
+
|
|
29
|
+
Args:
|
|
30
|
+
value: Value to validate
|
|
31
|
+
|
|
32
|
+
Raises:
|
|
33
|
+
ValueError: If value is outside the limits
|
|
34
|
+
"""
|
|
35
|
+
if not (self.min <= value <= self.max):
|
|
36
|
+
raise ValueError(
|
|
37
|
+
f"Value {value} outside axis limits [{self.min}, {self.max}]"
|
|
38
|
+
)
|
|
39
|
+
|
|
40
|
+
|
|
14
41
|
class GCodeController(SerialController):
|
|
42
|
+
"""
|
|
43
|
+
Controller for G-code compatible motion systems.
|
|
44
|
+
|
|
45
|
+
This class provides methods for controlling multi-axis motion systems that
|
|
46
|
+
understand G-code commands. All movements are executed in absolute coordinates,
|
|
47
|
+
with relative moves converted to absolute internally. Supports homing and
|
|
48
|
+
position synchronization.
|
|
49
|
+
|
|
50
|
+
Attributes:
|
|
51
|
+
DEFAULT_FEEDRATE: Default feed rate in mm/min (3000)
|
|
52
|
+
MAX_FEEDRATE: Maximum allowed feed rate in mm/min (3000)
|
|
53
|
+
TOLERANCE: Position synchronization tolerance in mm (0.01)
|
|
54
|
+
"""
|
|
55
|
+
|
|
15
56
|
DEFAULT_FEEDRATE = 3000 # mm/min
|
|
16
57
|
MAX_FEEDRATE = 3000 # mm/min
|
|
17
58
|
TOLERANCE = 0.01 # tolerance for position sync in mm
|
|
18
59
|
|
|
19
60
|
PROTOCOL_TERMINATOR = "\r"
|
|
61
|
+
VALID_AXES = "XYZA"
|
|
20
62
|
|
|
21
63
|
def __init__(
|
|
22
64
|
self,
|
|
@@ -26,19 +68,22 @@ class GCodeController(SerialController):
|
|
|
26
68
|
feed: int = DEFAULT_FEEDRATE,
|
|
27
69
|
):
|
|
28
70
|
"""
|
|
29
|
-
|
|
71
|
+
Initialize the G-code controller.
|
|
30
72
|
|
|
31
73
|
Args:
|
|
32
|
-
|
|
33
|
-
baudrate
|
|
34
|
-
timeout
|
|
74
|
+
port_name: Serial port name (e.g., '/dev/ttyACM0' or 'COM3')
|
|
75
|
+
baudrate: Baud rate for serial communication. Defaults to 9600.
|
|
76
|
+
timeout: Timeout in seconds for operations. Defaults to 20.
|
|
77
|
+
feed: Initial feed rate in mm/min. Defaults to 3000.
|
|
35
78
|
"""
|
|
36
79
|
super().__init__(port_name, baudrate, timeout)
|
|
37
80
|
|
|
38
|
-
# Initialize instance-specific logger
|
|
39
81
|
self._logger = logging.getLogger(__name__)
|
|
40
82
|
self._logger.info(
|
|
41
|
-
|
|
83
|
+
"GCodeController initialized with port='%s', baudrate=%s, timeout=%s",
|
|
84
|
+
port_name,
|
|
85
|
+
baudrate,
|
|
86
|
+
timeout,
|
|
42
87
|
)
|
|
43
88
|
|
|
44
89
|
# Tracks internal position state
|
|
@@ -49,81 +94,208 @@ class GCodeController(SerialController):
|
|
|
49
94
|
"A": 0.0,
|
|
50
95
|
}
|
|
51
96
|
self._feed: int = feed
|
|
52
|
-
|
|
97
|
+
|
|
98
|
+
# Initialize axis limits with default values
|
|
99
|
+
self._axis_limits: Dict[str, AxisLimits] = {
|
|
100
|
+
"X": AxisLimits(0, 0),
|
|
101
|
+
"Y": AxisLimits(0, 0),
|
|
102
|
+
"Z": AxisLimits(0, 0),
|
|
103
|
+
"A": AxisLimits(0, 0),
|
|
104
|
+
}
|
|
53
105
|
|
|
54
106
|
@property
|
|
55
|
-
def feed(self):
|
|
56
|
-
"""
|
|
107
|
+
def feed(self) -> int:
|
|
108
|
+
"""Get the current feed rate in mm/min."""
|
|
57
109
|
return self._feed
|
|
58
110
|
|
|
59
111
|
@feed.setter
|
|
60
|
-
def feed(self, new_feed: int):
|
|
61
|
-
"""
|
|
112
|
+
def feed(self, new_feed: int) -> None:
|
|
113
|
+
"""
|
|
114
|
+
Set the movement feed rate, enforcing the maximum limit.
|
|
115
|
+
|
|
116
|
+
Args:
|
|
117
|
+
new_feed: New feed rate in mm/min (must be > 0)
|
|
62
118
|
|
|
63
|
-
|
|
119
|
+
Raises:
|
|
120
|
+
ValueError: If feed rate is not positive
|
|
121
|
+
"""
|
|
64
122
|
if new_feed <= 0:
|
|
65
|
-
error_msg =
|
|
123
|
+
error_msg = (
|
|
124
|
+
f"Attempted to set invalid feed rate: {new_feed}. Must be > 0."
|
|
125
|
+
)
|
|
66
126
|
self._logger.error(error_msg)
|
|
67
127
|
raise ValueError(error_msg)
|
|
68
128
|
|
|
69
|
-
# 2. Check and enforce the maximum rate
|
|
70
129
|
if new_feed > self.MAX_FEEDRATE:
|
|
71
|
-
# Log the change and cap the value
|
|
72
130
|
self._logger.warning(
|
|
73
|
-
|
|
74
|
-
|
|
131
|
+
"Requested feed rate (%s) exceeds maximum (%s). "
|
|
132
|
+
"Setting feed rate to maximum: %s.",
|
|
133
|
+
new_feed,
|
|
134
|
+
self.MAX_FEEDRATE,
|
|
135
|
+
self.MAX_FEEDRATE,
|
|
75
136
|
)
|
|
76
137
|
self._feed = self.MAX_FEEDRATE
|
|
77
138
|
else:
|
|
78
|
-
# Set the value normally
|
|
79
139
|
self._feed = new_feed
|
|
80
|
-
self._logger.debug(
|
|
140
|
+
self._logger.debug("Feed rate set to: %s mm/min.", self._feed)
|
|
81
141
|
|
|
82
142
|
def _build_command(self, command: str) -> str:
|
|
83
|
-
"""
|
|
143
|
+
"""
|
|
144
|
+
Build a G-code command with terminator.
|
|
145
|
+
|
|
146
|
+
Args:
|
|
147
|
+
command: G-code command string (without terminator)
|
|
148
|
+
|
|
149
|
+
Returns:
|
|
150
|
+
Complete command string with terminator
|
|
151
|
+
"""
|
|
84
152
|
return f"{command}{self.PROTOCOL_TERMINATOR}"
|
|
85
153
|
|
|
86
|
-
def
|
|
154
|
+
def _validate_axis(self, axis: str) -> str:
|
|
87
155
|
"""
|
|
88
|
-
|
|
156
|
+
Validate and normalize an axis name.
|
|
89
157
|
|
|
90
158
|
Args:
|
|
91
|
-
axis
|
|
92
|
-
|
|
159
|
+
axis: Axis name to validate
|
|
160
|
+
|
|
161
|
+
Returns:
|
|
162
|
+
Uppercase axis name
|
|
93
163
|
|
|
94
164
|
Raises:
|
|
95
|
-
ValueError: If
|
|
165
|
+
ValueError: If axis is not valid
|
|
96
166
|
"""
|
|
97
|
-
|
|
167
|
+
axis_upper = axis.upper()
|
|
168
|
+
if axis_upper not in self.VALID_AXES:
|
|
169
|
+
self._logger.error(
|
|
170
|
+
"Invalid axis '%s' provided. Must be one of: %s.",
|
|
171
|
+
axis_upper,
|
|
172
|
+
", ".join(self.VALID_AXES),
|
|
173
|
+
)
|
|
174
|
+
raise ValueError(
|
|
175
|
+
f"Invalid axis. Must be one of: {', '.join(self.VALID_AXES)}."
|
|
176
|
+
)
|
|
177
|
+
return axis_upper
|
|
98
178
|
|
|
99
|
-
|
|
100
|
-
|
|
101
|
-
|
|
102
|
-
|
|
103
|
-
|
|
104
|
-
|
|
105
|
-
|
|
106
|
-
|
|
107
|
-
|
|
108
|
-
|
|
179
|
+
def _validate_move_positions(
|
|
180
|
+
self,
|
|
181
|
+
x: Optional[float] = None,
|
|
182
|
+
y: Optional[float] = None,
|
|
183
|
+
z: Optional[float] = None,
|
|
184
|
+
a: Optional[float] = None,
|
|
185
|
+
) -> None:
|
|
186
|
+
"""
|
|
187
|
+
Validate that move positions are within axis limits.
|
|
188
|
+
|
|
189
|
+
Only validates axes that are being moved (not None). Raises ValueError
|
|
190
|
+
if any position is outside the configured limits.
|
|
191
|
+
|
|
192
|
+
Args:
|
|
193
|
+
x: Target X position (optional)
|
|
194
|
+
y: Target Y position (optional)
|
|
195
|
+
z: Target Z position (optional)
|
|
196
|
+
a: Target A position (optional)
|
|
197
|
+
|
|
198
|
+
Raises:
|
|
199
|
+
ValueError: If any position is outside the axis limits
|
|
200
|
+
"""
|
|
201
|
+
if x is not None:
|
|
202
|
+
if "X" in self._axis_limits:
|
|
203
|
+
try:
|
|
204
|
+
self._axis_limits["X"].validate(x)
|
|
205
|
+
except ValueError as e:
|
|
206
|
+
self._logger.error("Move validation failed for X axis: %s", e)
|
|
207
|
+
raise
|
|
208
|
+
if y is not None:
|
|
209
|
+
if "Y" in self._axis_limits:
|
|
210
|
+
try:
|
|
211
|
+
self._axis_limits["Y"].validate(y)
|
|
212
|
+
except ValueError as e:
|
|
213
|
+
self._logger.error("Move validation failed for Y axis: %s", e)
|
|
214
|
+
raise
|
|
215
|
+
if z is not None:
|
|
216
|
+
if "Z" in self._axis_limits:
|
|
217
|
+
try:
|
|
218
|
+
self._axis_limits["Z"].validate(z)
|
|
219
|
+
except ValueError as e:
|
|
220
|
+
self._logger.error("Move validation failed for Z axis: %s", e)
|
|
221
|
+
raise
|
|
222
|
+
if a is not None:
|
|
223
|
+
if "A" in self._axis_limits:
|
|
224
|
+
try:
|
|
225
|
+
self._axis_limits["A"].validate(a)
|
|
226
|
+
except ValueError as e:
|
|
227
|
+
self._logger.error("Move validation failed for A axis: %s", e)
|
|
228
|
+
raise
|
|
229
|
+
|
|
230
|
+
def set_axis_limits(
|
|
231
|
+
self, axis: str, min_val: float, max_val: float
|
|
232
|
+
) -> None:
|
|
233
|
+
"""
|
|
234
|
+
Set the min/max limits for an axis.
|
|
235
|
+
|
|
236
|
+
Args:
|
|
237
|
+
axis: Axis name ('X', 'Y', 'Z', 'A')
|
|
238
|
+
min_val: Minimum allowed value
|
|
239
|
+
max_val: Maximum allowed value
|
|
240
|
+
|
|
241
|
+
Raises:
|
|
242
|
+
ValueError: If axis is unknown or min >= max
|
|
243
|
+
"""
|
|
244
|
+
axis = self._validate_axis(axis)
|
|
245
|
+
|
|
246
|
+
if min_val >= max_val:
|
|
247
|
+
raise ValueError("min must be < max")
|
|
248
|
+
|
|
249
|
+
self._axis_limits[axis] = AxisLimits(min_val, max_val)
|
|
250
|
+
self._logger.info(
|
|
251
|
+
"Set limits for axis %s: [%s, %s]", axis, min_val, max_val
|
|
252
|
+
)
|
|
253
|
+
|
|
254
|
+
def get_axis_limits(
|
|
255
|
+
self, axis: Optional[str] = None
|
|
256
|
+
) -> Union[AxisLimits, Dict[str, AxisLimits]]:
|
|
257
|
+
"""
|
|
258
|
+
Get the current limits for an axis or all axes.
|
|
259
|
+
|
|
260
|
+
Args:
|
|
261
|
+
axis: Optional axis name ('X', 'Y', 'Z', 'A'). If None, returns all limits.
|
|
262
|
+
|
|
263
|
+
Returns:
|
|
264
|
+
If axis is specified: AxisLimits object with min and max values.
|
|
265
|
+
If axis is None: Dictionary of all axis limits.
|
|
266
|
+
|
|
267
|
+
Raises:
|
|
268
|
+
ValueError: If axis is unknown (only when axis is provided)
|
|
269
|
+
"""
|
|
270
|
+
if axis is None:
|
|
271
|
+
return self._axis_limits.copy()
|
|
272
|
+
axis = self._validate_axis(axis)
|
|
273
|
+
return self._axis_limits[axis]
|
|
274
|
+
|
|
275
|
+
def home(self, axis: Optional[str] = None) -> None:
|
|
276
|
+
"""
|
|
277
|
+
Home one or all axes (G28 command).
|
|
278
|
+
|
|
279
|
+
Args:
|
|
280
|
+
axis: Optional axis to home ('X', 'Y', 'Z', 'A').
|
|
281
|
+
If None, homes all axes.
|
|
109
282
|
|
|
283
|
+
Raises:
|
|
284
|
+
ValueError: If an invalid axis is provided
|
|
285
|
+
"""
|
|
286
|
+
if axis:
|
|
287
|
+
axis = self._validate_axis(axis)
|
|
110
288
|
cmd = f"G28 {axis}"
|
|
111
289
|
home_target = axis
|
|
112
290
|
else:
|
|
113
291
|
cmd = "G28"
|
|
114
292
|
home_target = "All"
|
|
115
293
|
|
|
116
|
-
|
|
117
|
-
self.
|
|
118
|
-
|
|
119
|
-
self._send_command("G28\r")
|
|
120
|
-
self._logger.info(f"Homing of {home_target} completed.")
|
|
294
|
+
self._logger.info("[%s] homing axis/axes: %s **", cmd, home_target)
|
|
295
|
+
self._send_command(self._build_command(cmd))
|
|
296
|
+
self._logger.info("Homing of %s completed.", home_target)
|
|
121
297
|
|
|
122
|
-
#
|
|
123
|
-
# self.sync_position()
|
|
124
|
-
|
|
125
|
-
# 3. Internal Position Update (Optimistic Zeroing)
|
|
126
|
-
# If the homing was for a specific axis, zero only that axis.
|
|
298
|
+
# Update internal position (optimistic zeroing)
|
|
127
299
|
if axis:
|
|
128
300
|
self.current_position[axis] = 0.0
|
|
129
301
|
else:
|
|
@@ -131,7 +303,8 @@ class GCodeController(SerialController):
|
|
|
131
303
|
self.current_position[key] = 0.0
|
|
132
304
|
|
|
133
305
|
self._logger.debug(
|
|
134
|
-
|
|
306
|
+
"Internal position updated (optimistically zeroed) to %s",
|
|
307
|
+
self.current_position,
|
|
135
308
|
)
|
|
136
309
|
|
|
137
310
|
def move_absolute(
|
|
@@ -143,18 +316,31 @@ class GCodeController(SerialController):
|
|
|
143
316
|
feed: Optional[int] = None,
|
|
144
317
|
) -> None:
|
|
145
318
|
"""
|
|
146
|
-
|
|
319
|
+
Move to an absolute position (G90 + G1 command).
|
|
320
|
+
|
|
321
|
+
Args:
|
|
322
|
+
x: Target X position (optional)
|
|
323
|
+
y: Target Y position (optional)
|
|
324
|
+
z: Target Z position (optional)
|
|
325
|
+
a: Target A position (optional)
|
|
326
|
+
feed: Feed rate for this move (optional, uses current feed if not specified)
|
|
327
|
+
|
|
328
|
+
Raises:
|
|
329
|
+
ValueError: If any position is outside the axis limits
|
|
147
330
|
"""
|
|
331
|
+
# Validate positions before executing move
|
|
332
|
+
self._validate_move_positions(x=x, y=y, z=z, a=a)
|
|
333
|
+
|
|
334
|
+
feed_rate = feed if feed is not None else self._feed
|
|
148
335
|
self._logger.info(
|
|
149
|
-
|
|
336
|
+
"Preparing absolute move to X:%s, Y:%s, Z:%s, A:%s at F:%s",
|
|
337
|
+
x,
|
|
338
|
+
y,
|
|
339
|
+
z,
|
|
340
|
+
a,
|
|
341
|
+
feed_rate,
|
|
150
342
|
)
|
|
151
343
|
|
|
152
|
-
# Ensure absolute mode is active
|
|
153
|
-
if not self._is_absolute_mode:
|
|
154
|
-
self._logger.debug("Switching to absolute positioning mode (G90).")
|
|
155
|
-
self._send_command(self._build_command("G90"))
|
|
156
|
-
self._is_absolute_mode = True
|
|
157
|
-
|
|
158
344
|
self._execute_move(x=x, y=y, z=z, a=a, feed=feed)
|
|
159
345
|
|
|
160
346
|
def move_relative(
|
|
@@ -166,19 +352,38 @@ class GCodeController(SerialController):
|
|
|
166
352
|
feed: Optional[int] = None,
|
|
167
353
|
) -> None:
|
|
168
354
|
"""
|
|
169
|
-
|
|
355
|
+
Move relative to the current position (converted to absolute move internally).
|
|
356
|
+
|
|
357
|
+
Args:
|
|
358
|
+
x: Relative X movement (optional)
|
|
359
|
+
y: Relative Y movement (optional)
|
|
360
|
+
z: Relative Z movement (optional)
|
|
361
|
+
a: Relative A movement (optional)
|
|
362
|
+
feed: Feed rate for this move (optional, uses current feed if not specified)
|
|
363
|
+
|
|
364
|
+
Raises:
|
|
365
|
+
ValueError: If any resulting absolute position is outside the axis limits
|
|
170
366
|
"""
|
|
367
|
+
feed_rate = feed if feed is not None else self._feed
|
|
171
368
|
self._logger.info(
|
|
172
|
-
|
|
369
|
+
"Preparing relative move by dX:%s, dY:%s, dZ:%s, dA:%s at F:%s",
|
|
370
|
+
x,
|
|
371
|
+
y,
|
|
372
|
+
z,
|
|
373
|
+
a,
|
|
374
|
+
feed_rate,
|
|
173
375
|
)
|
|
174
376
|
|
|
175
|
-
#
|
|
176
|
-
|
|
177
|
-
|
|
178
|
-
|
|
179
|
-
|
|
377
|
+
# Convert relative movements to absolute positions
|
|
378
|
+
abs_x = (self.current_position["X"] + x) if x is not None else None
|
|
379
|
+
abs_y = (self.current_position["Y"] + y) if y is not None else None
|
|
380
|
+
abs_z = (self.current_position["Z"] + z) if z is not None else None
|
|
381
|
+
abs_a = (self.current_position["A"] + a) if a is not None else None
|
|
180
382
|
|
|
181
|
-
|
|
383
|
+
# Validate absolute positions before executing move
|
|
384
|
+
self._validate_move_positions(x=abs_x, y=abs_y, z=abs_z, a=abs_a)
|
|
385
|
+
|
|
386
|
+
self._execute_move(x=abs_x, y=abs_y, z=abs_z, a=abs_a, feed=feed)
|
|
182
387
|
|
|
183
388
|
def _execute_move(
|
|
184
389
|
self,
|
|
@@ -188,70 +393,130 @@ class GCodeController(SerialController):
|
|
|
188
393
|
a: Optional[float] = None,
|
|
189
394
|
feed: Optional[int] = None,
|
|
190
395
|
) -> None:
|
|
191
|
-
"""
|
|
396
|
+
"""
|
|
397
|
+
Internal helper for executing G1 move commands with safe movement pattern.
|
|
398
|
+
All coordinates are treated as absolute positions.
|
|
192
399
|
|
|
193
|
-
|
|
194
|
-
|
|
400
|
+
Safe move pattern:
|
|
401
|
+
1. If X or Y movement is needed, first move Z to 0 (safe height)
|
|
402
|
+
2. Then move X, Y (and optionally A) to target
|
|
403
|
+
3. Finally move Z to target position (if specified)
|
|
195
404
|
|
|
196
|
-
|
|
405
|
+
Args:
|
|
406
|
+
x: Absolute X position (optional)
|
|
407
|
+
y: Absolute Y position (optional)
|
|
408
|
+
z: Absolute Z position (optional)
|
|
409
|
+
a: Absolute A position (optional)
|
|
410
|
+
feed: Feed rate (optional)
|
|
411
|
+
"""
|
|
412
|
+
# Calculate target positions (all absolute)
|
|
197
413
|
target_pos = self.current_position.copy()
|
|
198
|
-
|
|
414
|
+
has_x = x is not None
|
|
415
|
+
has_y = y is not None
|
|
416
|
+
has_z = z is not None
|
|
417
|
+
has_a = a is not None
|
|
199
418
|
|
|
200
|
-
|
|
201
|
-
|
|
202
|
-
|
|
203
|
-
|
|
204
|
-
|
|
205
|
-
# Calculate the target position
|
|
206
|
-
if self._is_absolute_mode:
|
|
207
|
-
target_pos[axis] = value
|
|
208
|
-
else:
|
|
209
|
-
target_pos[axis] = self.current_position[axis] + value
|
|
419
|
+
if not (has_x or has_y or has_z or has_a):
|
|
420
|
+
self._logger.warning(
|
|
421
|
+
"Move command issued without any axis movement. Skipping transmission."
|
|
422
|
+
)
|
|
423
|
+
return
|
|
210
424
|
|
|
211
|
-
|
|
425
|
+
# Set target positions (all absolute)
|
|
426
|
+
if has_x:
|
|
427
|
+
target_pos["X"] = x
|
|
428
|
+
if has_y:
|
|
429
|
+
target_pos["Y"] = y
|
|
430
|
+
if has_z:
|
|
431
|
+
target_pos["Z"] = z
|
|
432
|
+
if has_a:
|
|
433
|
+
target_pos["A"] = a
|
|
212
434
|
|
|
213
|
-
# 3. Add on Feed Rate to command
|
|
214
435
|
feed_rate = feed if feed is not None else self._feed
|
|
215
|
-
# Final safety check on the rate used in the command
|
|
216
436
|
if feed_rate > self.MAX_FEEDRATE:
|
|
217
437
|
feed_rate = self.MAX_FEEDRATE
|
|
218
|
-
move_cmd += f" F{feed_rate}"
|
|
219
438
|
|
|
220
|
-
#
|
|
221
|
-
|
|
222
|
-
self._logger.warning(
|
|
223
|
-
"Move command issued without any axis movement. Skipping transmission."
|
|
224
|
-
)
|
|
225
|
-
return
|
|
439
|
+
# Ensure absolute mode is active
|
|
440
|
+
self._send_command(self._build_command("G90"))
|
|
226
441
|
|
|
227
|
-
#
|
|
228
|
-
|
|
229
|
-
self.
|
|
442
|
+
# Safe move pattern: Z to 0, then XY, then Z to target
|
|
443
|
+
needs_xy_move = has_x or has_y
|
|
444
|
+
current_z = self.current_position["Z"]
|
|
445
|
+
target_z = target_pos["Z"] if has_z else current_z
|
|
446
|
+
|
|
447
|
+
# Step 1: Move Z to safe height (0) if XY movement is needed and Z is not already at 0
|
|
448
|
+
if needs_xy_move and abs(current_z) > 0.001: # Small tolerance for floating point
|
|
449
|
+
# Validate safe height (Z=0) is within limits
|
|
450
|
+
self._validate_move_positions(z=0.0)
|
|
451
|
+
self._logger.info(
|
|
452
|
+
"Safe move: Raising Z to safe height (0) before XY movement"
|
|
453
|
+
)
|
|
454
|
+
move_cmd = f"G1 Z0 F{feed_rate}"
|
|
455
|
+
self._send_command(self._build_command(move_cmd))
|
|
456
|
+
self.current_position["Z"] = 0.0
|
|
457
|
+
self._logger.debug("Z moved to safe height (0)")
|
|
458
|
+
|
|
459
|
+
# Step 2: Move X, Y (and optionally A) to target
|
|
460
|
+
if needs_xy_move or has_a:
|
|
461
|
+
move_cmd = "G1"
|
|
462
|
+
if has_x:
|
|
463
|
+
move_cmd += f" X{target_pos['X']}"
|
|
464
|
+
if has_y:
|
|
465
|
+
move_cmd += f" Y{target_pos['Y']}"
|
|
466
|
+
if has_a:
|
|
467
|
+
move_cmd += f" A{target_pos['A']}"
|
|
468
|
+
move_cmd += f" F{feed_rate}"
|
|
469
|
+
|
|
470
|
+
self._logger.info("Executing XY move command: %s", move_cmd)
|
|
471
|
+
self._send_command(self._build_command(move_cmd))
|
|
472
|
+
|
|
473
|
+
# Update position for moved axes
|
|
474
|
+
if has_x:
|
|
475
|
+
self.current_position["X"] = target_pos["X"]
|
|
476
|
+
if has_y:
|
|
477
|
+
self.current_position["Y"] = target_pos["Y"]
|
|
478
|
+
if has_a:
|
|
479
|
+
self.current_position["A"] = target_pos["A"]
|
|
480
|
+
|
|
481
|
+
# Step 3: Move Z to target position (if Z movement was requested)
|
|
482
|
+
if has_z:
|
|
483
|
+
z_needs_move = abs(target_z - (0.0 if needs_xy_move else current_z)) > 0.001
|
|
484
|
+
if z_needs_move:
|
|
485
|
+
move_cmd = f"G1 Z{target_z} F{feed_rate}"
|
|
486
|
+
|
|
487
|
+
if needs_xy_move:
|
|
488
|
+
self._logger.info(
|
|
489
|
+
"Safe move: Lowering Z to target position: %s", target_z
|
|
490
|
+
)
|
|
491
|
+
else:
|
|
492
|
+
self._logger.info("Executing Z move command: %s", move_cmd)
|
|
493
|
+
|
|
494
|
+
self._send_command(self._build_command(move_cmd))
|
|
495
|
+
self.current_position["Z"] = target_z
|
|
230
496
|
|
|
231
497
|
self._logger.info(
|
|
232
|
-
|
|
498
|
+
"Move complete. Final position: %s", self.current_position
|
|
233
499
|
)
|
|
500
|
+
self._logger.debug("New internal position: %s", self.current_position)
|
|
234
501
|
|
|
235
|
-
|
|
236
|
-
self._logger.debug(f"New internal position: {self.current_position}")
|
|
237
|
-
|
|
238
|
-
# 6. Post-Move Position Synchronization Check
|
|
502
|
+
# Post-move position synchronization check
|
|
239
503
|
self.sync_position()
|
|
240
504
|
|
|
241
505
|
def query_position(self) -> Dict[str, float]:
|
|
242
506
|
"""
|
|
243
|
-
|
|
507
|
+
Query the current machine position (M114 command).
|
|
244
508
|
|
|
245
509
|
Returns:
|
|
246
|
-
|
|
510
|
+
Dictionary containing X, Y, Z, and A positions
|
|
511
|
+
|
|
512
|
+
Note:
|
|
513
|
+
Returns an empty dictionary if the query fails or no positions are found.
|
|
247
514
|
"""
|
|
248
515
|
self._logger.info("Querying current machine position (M114).")
|
|
249
|
-
self.
|
|
250
|
-
res: str = self._read_response()
|
|
516
|
+
res: str = self.execute(self._build_command("M114"))
|
|
251
517
|
|
|
252
518
|
# Extract position values using regex
|
|
253
|
-
pattern = re.compile(r"([
|
|
254
|
-
# Find all matches (e.g., [('X', '0.000'), ('Y', '0.000'), ...])
|
|
519
|
+
pattern = re.compile(r"([XYZA]):(-?\d+\.\d+)")
|
|
255
520
|
matches = pattern.findall(res)
|
|
256
521
|
|
|
257
522
|
position_data: Dict[str, float] = {}
|
|
@@ -261,7 +526,9 @@ class GCodeController(SerialController):
|
|
|
261
526
|
position_data[axis] = float(value_str)
|
|
262
527
|
except ValueError:
|
|
263
528
|
self._logger.error(
|
|
264
|
-
|
|
529
|
+
"Failed to convert position value '%s' for axis %s to float.",
|
|
530
|
+
value_str,
|
|
531
|
+
axis,
|
|
265
532
|
)
|
|
266
533
|
continue
|
|
267
534
|
|
|
@@ -269,31 +536,33 @@ class GCodeController(SerialController):
|
|
|
269
536
|
|
|
270
537
|
def sync_position(self) -> Tuple[bool, Dict[str, float]]:
|
|
271
538
|
"""
|
|
272
|
-
|
|
273
|
-
|
|
274
|
-
|
|
539
|
+
Synchronize internal position with actual machine position.
|
|
540
|
+
|
|
541
|
+
Queries the machine position and compares it with the internal position.
|
|
542
|
+
If a discrepancy greater than the tolerance is found, attempts to correct
|
|
543
|
+
it by moving to the internal position.
|
|
275
544
|
|
|
276
545
|
Returns:
|
|
277
|
-
Tuple
|
|
278
|
-
is True if a
|
|
279
|
-
|
|
546
|
+
Tuple of (adjustment_occurred: bool, final_position: Dict[str, float])
|
|
547
|
+
where adjustment_occurred is True if a correction move was made.
|
|
548
|
+
|
|
549
|
+
Note:
|
|
550
|
+
This method may recursively call itself if a correction move is made.
|
|
280
551
|
"""
|
|
281
552
|
self._logger.info("Starting position synchronization check (M114).")
|
|
282
553
|
|
|
283
|
-
#
|
|
554
|
+
# Query the actual machine position
|
|
284
555
|
queried_position = self.query_position()
|
|
285
556
|
|
|
286
|
-
# Check if the query was unsuccessful (only returns empty dict)
|
|
287
557
|
if not queried_position:
|
|
288
558
|
self._logger.warning("Query position failed. Cannot synchronize.")
|
|
289
559
|
return False, self.current_position
|
|
290
560
|
|
|
291
|
-
#
|
|
561
|
+
# Compare internal vs. queried position
|
|
292
562
|
axis_keys = ["X", "Y", "Z", "A"]
|
|
293
563
|
adjustment_needed = False
|
|
294
564
|
|
|
295
565
|
for axis in axis_keys:
|
|
296
|
-
# Check if axis exists in both dictionaries and if they differ significantly
|
|
297
566
|
if (
|
|
298
567
|
axis in self.current_position
|
|
299
568
|
and axis in queried_position
|
|
@@ -301,25 +570,24 @@ class GCodeController(SerialController):
|
|
|
301
570
|
> self.TOLERANCE
|
|
302
571
|
):
|
|
303
572
|
self._logger.warning(
|
|
304
|
-
|
|
305
|
-
|
|
306
|
-
|
|
573
|
+
"Position mismatch found on %s axis: Internal=%.3f, Queried=%.3f",
|
|
574
|
+
axis,
|
|
575
|
+
self.current_position[axis],
|
|
576
|
+
queried_position[axis],
|
|
307
577
|
)
|
|
308
|
-
|
|
309
578
|
adjustment_needed = True
|
|
310
|
-
|
|
311
579
|
elif axis in queried_position:
|
|
312
|
-
# Update internal position with
|
|
580
|
+
# Update internal position with queried position if it differs slightly
|
|
313
581
|
self.current_position[axis] = queried_position[axis]
|
|
314
582
|
|
|
315
|
-
#
|
|
583
|
+
# Perform re-synchronization move if needed
|
|
316
584
|
if adjustment_needed:
|
|
317
585
|
self._logger.info(
|
|
318
|
-
|
|
586
|
+
"** DISCREPANCY DETECTED. Moving robot back to internal position: %s **",
|
|
587
|
+
self.current_position,
|
|
319
588
|
)
|
|
320
589
|
|
|
321
590
|
try:
|
|
322
|
-
# Extract the X, Y, Z, A coordinates from the internal position for the move
|
|
323
591
|
target_x = self.current_position.get("X")
|
|
324
592
|
target_y = self.current_position.get("Y")
|
|
325
593
|
target_z = self.current_position.get("Z")
|
|
@@ -328,40 +596,37 @@ class GCodeController(SerialController):
|
|
|
328
596
|
self.move_absolute(x=target_x, y=target_y, z=target_z, a=target_a)
|
|
329
597
|
self._logger.info("Synchronization move successfully completed.")
|
|
330
598
|
|
|
331
|
-
#
|
|
332
|
-
self.sync_position()
|
|
333
|
-
except
|
|
334
|
-
self._logger.error(
|
|
335
|
-
adjustment_needed = False
|
|
336
|
-
|
|
337
|
-
final_position = self.current_position.copy()
|
|
599
|
+
# Recursive call to verify position after move
|
|
600
|
+
return self.sync_position()
|
|
601
|
+
except (ValueError, RuntimeError, OSError) as e:
|
|
602
|
+
self._logger.error("Synchronization move failed: %s", e)
|
|
603
|
+
adjustment_needed = False
|
|
338
604
|
|
|
339
605
|
if adjustment_needed:
|
|
340
606
|
self._logger.info(
|
|
341
|
-
|
|
607
|
+
"Position check complete. Internal position is synchronized with machine."
|
|
342
608
|
)
|
|
343
609
|
else:
|
|
344
610
|
self._logger.info("No adjustment was made.")
|
|
345
611
|
|
|
346
|
-
|
|
347
|
-
# We return the initial state of adjustment_needed (i.e., if a move was triggered).
|
|
348
|
-
return adjustment_needed, final_position
|
|
612
|
+
return adjustment_needed, self.current_position.copy()
|
|
349
613
|
|
|
350
614
|
def get_info(self) -> str:
|
|
351
615
|
"""
|
|
352
|
-
|
|
616
|
+
Query machine information (M115 command).
|
|
353
617
|
|
|
354
618
|
Returns:
|
|
355
|
-
|
|
619
|
+
Machine information string from the device
|
|
356
620
|
"""
|
|
357
621
|
self._logger.info("Querying machine information (M115).")
|
|
358
|
-
self.
|
|
359
|
-
res: str = self._read_response()
|
|
360
|
-
return res
|
|
622
|
+
return self.execute(self._build_command("M115"))
|
|
361
623
|
|
|
362
624
|
def get_internal_position(self) -> Dict[str, float]:
|
|
363
625
|
"""
|
|
364
|
-
|
|
626
|
+
Get the internally tracked position.
|
|
627
|
+
|
|
628
|
+
Returns:
|
|
629
|
+
Dictionary containing the current internal position for all axes
|
|
365
630
|
"""
|
|
366
|
-
self._logger.debug(
|
|
367
|
-
return self.current_position
|
|
631
|
+
self._logger.debug("Returning internal position: %s", self.current_position)
|
|
632
|
+
return self.current_position.copy()
|