Backlash_fix

import re
import argparse

class FluidNCSafeBacklash:
    def __init__(self, bx, by, threshold=0.05, safe_feed=200):
        # Compensation parameters (in mm)
        self.bx = bx
        self.by = by
        # Noise filter threshold (mm). Moves below this value will not trigger compensation.
        self.threshold = threshold
        # Safe feedrate for compensation move (F200 is safe for slow motors)
        self.safe_feed = safe_feed
        
        # Tracking internal state (Logical position)
        self.current_x = 0.0
        self.current_y = 0.0
        
        # Tracking the actual offset added to G-code (Physical position)
        self.offset_x = 0.0
        self.offset_y = 0.0
        
        # Directions: 1 (Positive), -1 (Negative), 0 (Uninitialized)
        self.dir_x = 0
        self.dir_y = 0

    def process(self, input_file, output_file):
        with open(input_file, 'r') as f:
            lines = f.readlines()
            
        new_gcode = []
        
        # G-code header for FluidNC
        new_gcode.append("G90 ; Absolute positioning")
        new_gcode.append("G21 ; Millimeters")
        
        # Regular expressions
        p_x = re.compile(r'X([-\d\.]+)')
        p_y = re.compile(r'Y([-\d\.]+)')
        p_z = re.compile(r'Z([-\d\.]+)')
        p_f = re.compile(r'F([-\d\.]+)')

        current_feed = 500.0 # Default feedrate used for restore

        for line in lines:
            original_line = line.strip()
            if not original_line or original_line.startswith(';'):
                new_gcode.append(original_line)
                continue
            
            # --- 1. PARSING ---
            target_x = self.current_x
            target_y = self.current_y
            target_z = 0.0 # Z coordinate is only read
            has_move = False

            mx = p_x.search(original_line)
            if mx: 
                target_x = float(mx.group(1))
                has_move = True
            
            my = p_y.search(original_line)
            if my: 
                target_y = float(my.group(1))
                has_move = True
                
            mz = p_z.search(original_line)
            if mz: target_z = float(mz.group(1))

            mf = p_f.search(original_line)
            if mf: current_feed = float(mf.group(1)) # Update feedrate

            if not has_move:
                new_gcode.append(original_line)
                continue

            # --- 2. LOGIC FOR X-AXIS (BACKLASH MOVE) ---
            dx = target_x - self.current_x
            if abs(dx) > self.threshold:
                new_dir_x = 1 if dx > 0 else -1
                
                # If direction has changed
                if self.dir_x != 0 and new_dir_x != self.dir_x:
                    # Calculate the move to be added to the offset
                    change = self.bx if new_dir_x == 1 else -self.bx
                    self.offset_x += change
                    
                    # *Insert intermediate compensation move*
                    # Moving to the NEW physical position USING G0
                    physical_x_fix = self.current_x + self.offset_x
                    physical_y_fix = self.current_y + self.offset_y
                    
                    # SAFETY: Prevent Soft Limit Alarm if 0,0 is Home
                    if physical_x_fix < 0: physical_x_fix = 0
                    if physical_y_fix < 0: physical_y_fix = 0

                    new_gcode.append(f"; --- FIX X Backlash: {change:.3f}mm ---")
                    # Use G0 for rapid move, but specify a safe F-speed
                    new_gcode.append(f"G0 X{physical_x_fix:.3f} Y{physical_y_fix:.3f} F{self.safe_feed}")
                    
                self.dir_x = new_dir_x

            # --- 3. LOGIC FOR Y-AXIS (BACKLASH MOVE) ---
            dy = target_y - self.current_y
            if abs(dy) > self.threshold:
                new_dir_y = 1 if dy > 0 else -1
                
                if self.dir_y != 0 and new_dir_y != self.dir_y:
                    change = self.by if new_dir_y == 1 else -self.by
                    self.offset_y += change
                    
                    physical_x_fix = self.current_x + self.offset_x
                    physical_y_fix = self.current_y + self.offset_y

                    if physical_x_fix < 0: physical_x_fix = 0
                    if physical_y_fix < 0: physical_y_fix = 0

                    new_gcode.append(f"; --- FIX Y Backlash: {change:.3f}mm ---")
                    new_gcode.append(f"G0 X{physical_x_fix:.3f} Y{physical_y_fix:.3f} F{self.safe_feed}")

                self.dir_y = new_dir_y

            # --- 4. OUTPUT ORIGINAL MOVE ---
            # Calculate final coordinates with corrections (Offset)
            final_x = target_x + self.offset_x
            final_y = target_y + self.offset_y
            
            # SAFETY: Final coordinates are also limited to 0 (prevents alarm)
            if final_x < 0: final_x = 0
            if final_y < 0: final_y = 0

            # Determine whether G0 or G1 and construct the line
            cmd = "G1" 
            if "G0" in original_line.upper(): cmd = "G0"
            
            out = f"{cmd} X{final_x:.3f} Y{final_y:.3f}"
            if target_z != 0.0: out += f" Z{target_z:.3f}"
            
            # For G1, always add F so FluidNC knows the feedrate.
            if cmd == "G1":
                out += f" F{int(current_feed)}"
            
            new_gcode.append(out)
            
            # Update logical position
            self.current_x = target_x
            self.current_y = target_y

        new_gcode.append("M2 ; End of G-code")

        with open(output_file, 'w') as f:
            f.write("\n".join(new_gcode))
        print(f"FluidNC G-code ready. Saved to: {output_file}")

if __name__ == "__main__":
    parser = argparse.ArgumentParser(description="Processes G-code to insert mechanical backlash compensation moves.")
    parser.add_argument("input", help="Input G-code file (e.g., raw_cut.gcode)")
    parser.add_argument("output", nargs='?', help="Output G-code file (optional, defaults to 'fixed_<input_file>')")
    parser.add_argument("--bx", type=float, default=0.5, help="Backlash magnitude for X-axis (mm)")
    parser.add_argument("--by", type=float, default=1.6, help="Backlash magnitude for Y-axis (mm)")
    
    args = parser.parse_args()
    
    # Set output file default if not provided
    output_filename = args.output if args.output else "fixed_" + args.input

    # Use default values 
    fixer = FluidNCSafeBacklash(args.bx, args.by)
    
    fixer.process(args.input, output_filename)