소스 검색

Switch from machine coordinates (G53) to work coordinates with reset

This prevents MPos from accumulating indefinitely and hitting soft limits
(error:30) during long pattern sequences.

Changes:
- Add reset_work_coordinates() to clear G92/G54 offsets on connection
- Remove G53 from G1 commands to use work coordinates (G54 default)
- Add G92 X0 in reset_theta() to reset work X position each pattern
- Reduce coordinate precision to 2 decimals

The G92 X0 command sets current position as X=0 without moving, keeping
coordinates bounded. Soft limits still check MPos, so safety is preserved.

Co-Authored-By: Claude Opus 4.5 <noreply@anthropic.com>
tuanchris 1 주 전
부모
커밋
b9276764cc
2개의 변경된 파일123개의 추가작업 그리고 4개의 파일을 삭제
  1. 88 2
      modules/connection/connection_manager.py
  2. 35 2
      modules/core/pattern_manager.py

+ 88 - 2
modules/connection/connection_manager.py

@@ -200,7 +200,7 @@ def device_init(homing=True):
     try:
         if get_machine_steps():
             logger.info(f"x_steps_per_mm: {state.x_steps_per_mm}, y_steps_per_mm: {state.y_steps_per_mm}, gear_ratio: {state.gear_ratio}")
-        else: 
+        else:
             logger.fatal("Failed to get machine steps")
             state.conn.close()
             return False
@@ -209,6 +209,10 @@ def device_init(homing=True):
         state.conn.close()
         return False
 
+    # Reset work coordinate offsets for a clean start
+    # This ensures we're using work coordinates (G54) starting from 0
+    reset_work_coordinates()
+
     machine_x, machine_y = get_machine_position()
     if machine_x != state.machine_x or machine_y != state.machine_y:
         logger.info(f'x, y; {machine_x}, {machine_y}')
@@ -469,7 +473,7 @@ async def send_grbl_coordinates(x, y, speed=600, timeout=30, home=False):
             return False
 
         try:
-            gcode = f"$J=G91 G21 Y{y} F{speed}" if home else f"G1 G53 X{x} Y{y} F{speed}"
+            gcode = f"$J=G91 G21 Y{y:.2f} F{speed}" if home else f"G1 X{x:.2f} Y{y:.2f} F{speed}"
             await asyncio.to_thread(state.conn.send, gcode + "\n")
             logger.debug(f"Sent command: {gcode}")
 
@@ -1259,6 +1263,88 @@ async def update_machine_position():
         except Exception as e:
             logger.error(f"Error updating machine position: {e}")
 
+
+def reset_work_coordinates():
+    """
+    Clear all work coordinate offsets for a clean start.
+
+    This ensures the work coordinate system starts fresh on each connection,
+    preventing accumulated offsets from previous sessions from affecting
+    pattern execution.
+
+    G92.1: Clears any G92 offset (resets work coordinates to machine coordinates)
+    G10 L2 P1 X0 Y0: Sets G54 work offset to 0 (for completeness)
+    """
+    if not state.conn or not state.conn.is_connected():
+        logger.warning("Cannot reset work coordinates: no active connection")
+        return False
+
+    try:
+        logger.info("Resetting work coordinate offsets")
+
+        # Clear any stale input data first
+        try:
+            while state.conn.in_waiting() > 0:
+                state.conn.readline()
+        except Exception:
+            pass
+
+        # Clear G92 offset
+        state.conn.send("G92.1\n")
+        time.sleep(0.2)
+
+        # Wait for 'ok' response
+        start_time = time.time()
+        got_ok = False
+        while time.time() - start_time < 2.0:
+            if state.conn.in_waiting() > 0:
+                response = state.conn.readline()
+                if response:
+                    logger.debug(f"G92.1 response: {response}")
+                    if response.lower() == "ok":
+                        got_ok = True
+                        break
+                    elif "error" in response.lower():
+                        logger.warning(f"G92.1 error: {response}")
+                        break
+            time.sleep(0.05)
+
+        if not got_ok:
+            logger.warning("Did not receive 'ok' for G92.1, continuing anyway")
+
+        # Set G54 offset to 0 (optional, for completeness)
+        state.conn.send("G10 L2 P1 X0 Y0\n")
+        time.sleep(0.2)
+
+        # Wait for 'ok' response
+        start_time = time.time()
+        got_ok = False
+        while time.time() - start_time < 2.0:
+            if state.conn.in_waiting() > 0:
+                response = state.conn.readline()
+                if response:
+                    logger.debug(f"G10 response: {response}")
+                    if response.lower() == "ok":
+                        got_ok = True
+                        break
+                    elif "error" in response.lower():
+                        logger.warning(f"G10 error: {response}")
+                        break
+            time.sleep(0.05)
+
+        if not got_ok:
+            logger.warning("Did not receive 'ok' for G10 L2 P1 X0 Y0, continuing anyway")
+
+        # Reset machine_x to 0 since work coordinates now start at 0
+        state.machine_x = 0.0
+        logger.info("Work coordinates reset complete (machine_x set to 0)")
+        return True
+
+    except Exception as e:
+        logger.error(f"Error resetting work coordinates: {e}")
+        return False
+
+
 def restart_connection(homing=False):
     """
     Restart the connection. If a connection exists, close it and attempt to establish a new one.

+ 35 - 2
modules/core/pattern_manager.py

@@ -522,7 +522,8 @@ class MotionControlThread:
             return
 
         # Call sync version of send_grbl_coordinates in this thread
-        self._send_grbl_coordinates_sync(round(new_x_abs, 3), round(new_y_abs, 3), actual_speed)
+        # Use 2 decimal precision to reduce GRBL parsing overhead
+        self._send_grbl_coordinates_sync(round(new_x_abs, 2), round(new_y_abs, 2), actual_speed)
 
         # Update state
         state.current_theta = theta
@@ -539,7 +540,7 @@ class MotionControlThread:
 
         Includes retry logic for serial corruption errors (common on Pi 3B+).
         """
-        gcode = f"$J=G91 G21 Y{y} F{speed}" if home else f"G1 G53 X{x} Y{y} F{speed}"
+        gcode = f"$J=G91 G21 Y{y:.2f} F{speed}" if home else f"G1 X{x:.2f} Y{y:.2f} F{speed}"
         max_wait_time = 120  # Maximum seconds to wait for 'ok' response
         max_corruption_retries = 3  # Max retries for corruption-type errors
         max_timeout_retries = 2  # Max retries for timeout (lost 'ok' response)
@@ -1675,8 +1676,40 @@ def resume_execution():
     return True
     
 async def reset_theta():
+    """
+    Reset theta to [0, 2π) range and reset work X coordinate.
+
+    G92 X0 sets current work position to X=0 without moving.
+    This keeps coordinates bounded and prevents soft limit errors.
+    The soft limits check against MPos (machine position), which doesn't
+    change with G92, so this is safe for the hardware.
+    """
     logger.info('Resetting Theta')
     state.current_theta = state.current_theta % (2 * pi)
+
+    # Reset work X coordinate to prevent accumulation
+    if state.conn and state.conn.is_connected():
+        try:
+            logger.info(f"Resetting work X position (was: {state.machine_x:.2f})")
+            state.conn.send("G92 X0\n")
+
+            # Wait for ok response
+            start_time = time.time()
+            while time.time() - start_time < 2.0:
+                response = state.conn.readline()
+                if response:
+                    logger.debug(f"G92 X0 response: {response}")
+                    if response.lower() == "ok":
+                        state.machine_x = 0.0
+                        logger.info("Work X position reset to 0")
+                        break
+                    elif "error" in response.lower():
+                        logger.error(f"G92 X0 error: {response}")
+                        break
+                await asyncio.sleep(0.05)
+        except Exception as e:
+            logger.error(f"Error resetting work position: {e}")
+
     # Call async function directly since we're in async context
     await connection_manager.update_machine_position()