Robot Tool Path for Sanding Disc

 Robot Tool Path for Sanding Disc

 

# Revision 5: Parameters consolidated into a single config dictionary (params)

import numpy as np
import matplotlib.pyplot as plt
import csv

def generate_toolpath(params):
    b1 = params['b1']
    b2 = params['b2']
    r_max = params['r_max']
    circle_pass = params['circle_pass']
    double_spiral = params['double_spiral']

    # Outward spiral
    theta_out = np.linspace(0, r_max / b1, 1000)
    r_out = b1 * theta_out
    x_out = r_out * np.cos(theta_out)
    y_out = r_out * np.sin(theta_out)

    # Full circle
    if circle_pass:
        theta_circle = np.linspace(0, 2 * np.pi, 500)
        r_circle = np.full_like(theta_circle, r_max)
        angle_offset = theta_out[-1]
        x_circle = r_circle * np.cos(theta_circle + angle_offset)
        y_circle = r_circle * np.sin(theta_circle + angle_offset)
        theta_after_circle = angle_offset + 2 * np.pi
    else:
        x_circle = np.array([])
        y_circle = np.array([])
        theta_after_circle = theta_out[-1]

    # Inward spiral
    if double_spiral:
        theta_in = np.linspace(0, r_max / b2, 1000)
        r_in = r_max - b2 * theta_in
        theta_in_total = theta_after_circle + theta_in
        x_in = r_in * np.cos(theta_in_total)
        y_in = r_in * np.sin(theta_in_total)
    else:
        x_in = np.array([])
        y_in = np.array([])

    x_total = np.concatenate([x_out, x_circle, x_in])
    y_total = np.concatenate([y_out, y_circle, y_in])
    return x_total, y_total

def plot_toolpath(x, y, params):
    b1 = params['b1']
    b2 = params['b2']
    circle_pass = params['circle_pass']
    double_spiral = params['double_spiral']

    plt.figure(figsize=(8, 8))
    if b1:
        plt.plot(x[:1000], y[:1000], label=f'Spiral Out (b1={b1:.2f})', linewidth=1.5)
    if circle_pass:
        plt.plot(x[1000:1500], y[1000:1500], label='Full Circle at r_max', linewidth=1.5, color='orange')
    if double_spiral:
        plt.plot(x[1500:], y[1500:], label=f'Spiral In (b2={b2:.2f})', linewidth=1.5, color='green')
    plt.scatter([0], [0], color='red', label='Center (0, 0)')
    plt.gca().set_aspect('equal')
    plt.title(f"Toolpath (circle_pass={circle_pass}, double_spiral={double_spiral})")
    plt.xlabel('X (mm)')
    plt.ylabel('Y (mm)')
    plt.grid(True)
    plt.legend()
    plt.tight_layout()
    plt.show()

def write_targets_csv(x, y, output_file='toolpath_targets.csv', spacing=10.0, fixed_z=0.0):
    coords = np.column_stack((x, y))
    targets = [[0.0, 0.0, fixed_z]]
    last = np.array([0.0, 0.0])

    for pt in coords:
        if np.linalg.norm(pt - last) >= spacing:
            targets.append([pt[0], pt[1], fixed_z])
            last = pt

    with open(output_file, 'w', newline='') as f:
        writer = csv.writer(f)
        writer.writerow(['X', 'Y', 'Z'])
        writer.writerows(targets)

    print(f"Exported {len(targets)} target points to: {output_file}")

# === Example usage ===
if __name__ == "__main__":
    # Define all parameters in one place
    params = {
        'b1': 40 / np.pi,
        'b2': 50 / np.pi,
        'r_max': 300,
        'circle_pass': True,
        'double_spiral': True
    }

    # Generate, plot, and write
    x, y = generate_toolpath(params)
    plot_toolpath(x, y, params)
    write_targets_csv(x, y, output_file='spiral_toolpath.csv', spacing=20, fixed_z=0.0)