main.py

"""
RSIPI Test Runner
=================
Uncomment one example at a time and run this file.
Uses RSI_EthernetConfig.xml from the project root by default.

Usage:
    python main.py
    python main.py --config path/to/other_config.xml
"""

import argparse
import time  # noqa: F401 - used by commented examples
from multiprocessing import freeze_support
from RSIPI import RSIAPI

if __name__ == '__main__':
    freeze_support()

    parser = argparse.ArgumentParser(description="RSIPI Test Runner")
    parser.add_argument("--config", type=str, default="RSI_EthernetConfig.xml",
                        help="Path to RSI config XML file")
    parser.add_argument("--mode", type=str, default="relative", choices=["absolute", "relative"],
                        help="RSI correction mode (must match KRL program)")
    parser.add_argument("--max-cart-rate", type=float, default=0.5,
                        help="Max Cartesian correction per cycle in mm (0 = no limit)")
    parser.add_argument("--max-joint-rate", type=float, default=0.2,
                        help="Max joint correction per cycle in degrees (0 = no limit)")
    parser.add_argument("--cycle-time", type=float, default=0.004,
                        help="RSI cycle time in seconds (0.004 = 4ms/250Hz, 0.012 = 12ms/83Hz)")
    args = parser.parse_args()

    api = RSIAPI(
        args.config,
        rsi_mode=args.mode,
        max_cartesian_rate=args.max_cart_rate,
        max_joint_rate=args.max_joint_rate,
        cycle_time=args.cycle_time
    )

    # =========================================================================
    # Example 01: Start / Stop
    # =========================================================================
    # api.start()
    # print("RSI started. Press Enter to stop.")
    # input()
    # api.stop()

    # =========================================================================
    # Example 02: Send Cartesian correction (move TCP 50mm along X)
    # =========================================================================
    # api.start()
    # print("Waiting for robot connection...")
    # if not api.wait_for_connection(timeout=10):
    #     print("Timeout waiting for robot. Exiting.")
    #     api.stop()
    #     exit(1)
    # print(f"Connected. IPOC: {api.monitoring.get_ipoc()}")
    # api.motion.update_cartesian(X=0.01, Y=0, Z=0)
    # print("Sent Cartesian correction. Press Enter to stop.")
    # input()
    # api.stop()

    # =========================================================================
    # Example 03: Send Joint correction (move A1 by 10 degrees) DOESNT WORK
    # =========================================================================
    # api.start()
    # time.sleep(1)
    # api.motion.update_joints(A1=50)
    # print("Sent joint correction. Press Enter to stop.")
    # input()
    # api.stop()

    # =========================================================================
    # Example 04: External axes (move E1 by 100mm)
    # =========================================================================
    # api.start()
    # time.sleep(1)
    # api.motion.move_external_axis('E1', 100)
    # print("Sent external axis correction. Press Enter to stop.")
    # input()
    # api.stop()

    # =========================================================================
    # Example 05: Digital I/O (toggle outputs)
    # =========================================================================
    # api.start()
    # time.sleep(1)
    # api.io.set_output(1, True)
    # time.sleep(5)
    # api.io.set_output(1, False)
  
    # print("Set digital outputs. Press Enter to stop.")
    # input()
    # api.stop()

    # =========================================================================
    # Example 06: CSV Logging
    # =========================================================================
    # api.logging.start()
    # api.start()
    # print("Logging to CSV. Press Enter to stop.")
    # input()
    # api.logging.stop()
    # api.stop()

    # =========================================================================
    # Example 07: Live Graphing
    # =========================================================================
    # api.start()
    # api.viz.start_live_plot('3d')
    # print("Live graph running. Press Enter to stop.")
    # input()
    # api.viz.stop_live_plot()
    # api.stop()

    # =========================================================================
    # Example 08: Safety Limits (restrict X-axis to +/-500mm)
    # =========================================================================
    # api.safety.set_limit("RKorr.X", -500, 500)
    # api.start()
    # print("Safety limits active. Press Enter to stop.")
    # input()
    # api.stop()

    # =========================================================================
    # Example 09: Cartesian Trajectory (square pattern)
    # =========================================================================
    # api.start()
    # time.sleep(1)
    # waypoints = [
    #     {"X": 50, "Y": 0, "Z": 0},
    #     {"X": 50, "Y": 50, "Z": 0},
    #     {"X": 0, "Y": 50, "Z": 0},
    #     {"X": 0, "Y": 0, "Z": 0},
    # ]
    # for wp in waypoints:
    #     api.motion.update_cartesian(**wp)
    #     time.sleep(0.5)
    # print("Trajectory complete. Press Enter to stop.")
    # input()
    # api.stop()

    # =========================================================================
    # Example 10: Safe Shutdown (Ctrl+C handler)
    # =========================================================================
    # try:
    #     api.start()
    #     print("Running. Press Ctrl+C for safe shutdown.")
    #     while True:
    #         time.sleep(0.1)
    # except KeyboardInterrupt:
    #     print("Emergency stop triggered.")
    #     api.safety.stop()
    #     api.stop()

    # =========================================================================
    # Coordination 01: Basic Handshake (KRL <-> Python I/O signalling)
    # =========================================================================
    # api.start()
    # time.sleep(1)
    # print("Waiting for KRL ready signal on input 1...")
    # if api.krl.wait_for_signal(1, timeout=10.0):
    #     print("KRL ready. Processing...")
    #     time.sleep(1)
    #     api.krl.signal_complete(1)
    #     print("Handshake complete.")
    # else:
    #     print("Timeout waiting for KRL signal.")
    # input("Press Enter to stop.")
    # api.stop()

    # =========================================================================
    # Coordination 02: Parameter Passing (read/write Tech variables)
    # =========================================================================
    # api.start()
    # time.sleep(1)
    # pos_x = api.krl.read_param('T11')
    # pos_y = api.krl.read_param('T12')
    # pos_z = api.krl.read_param('T13')
    # print(f"Current position from KRL: X={pos_x}, Y={pos_y}, Z={pos_z}")
    # api.krl.write_param('C11', pos_x + 50)
    # api.krl.write_param('C12', pos_y)
    # api.krl.write_param('C13', pos_z)
    # api.krl.signal_complete(1)
    # print("Parameters sent. Press Enter to stop.")
    # input()
    # api.stop()

    # =========================================================================
    # Coordination 03: State Machine (multi-state workflow)
    # =========================================================================
    # IDLE, CALIBRATING, READY, EXECUTING, COMPLETE, ERROR = 0, 1, 2, 3, 4, 5
    # api.start()
    # time.sleep(1)
    # state = IDLE
    # print("State machine running. Press Ctrl+C to exit.")
    # try:
    #     while state != COMPLETE:
    #         krl_state = int(api.krl.read_param('T11'))
    #         if krl_state == CALIBRATING:
    #             print("Calibrating...")
    #             time.sleep(2)
    #             api.krl.write_param('C11', READY)
    #             state = READY
    #         elif krl_state == EXECUTING:
    #             print("Executing motion...")
    #             state = EXECUTING
    #         elif krl_state == COMPLETE:
    #             print("Complete.")
    #             state = COMPLETE
    #         elif krl_state == ERROR:
    #             print("Error detected!")
    #             break
    #         time.sleep(0.05)
    # except KeyboardInterrupt:
    #     pass
    # api.stop()

    # =========================================================================
    # Advanced Motion 01: Velocity Profiles (trapezoidal vs S-curve)
    # =========================================================================
    # api.start()
    # time.sleep(4)
    # # Relative mode: each point is a per-cycle delta
    # # 200 steps × 0.5mm = 100mm total at max rate limit
    # traj = api.motion.generate_trajectory(
    #     {"X": 0, "Y": 0, "Z": 0},
    #     {"X": 100, "Y": 0, "Z": 0},
    #     steps=200,
    #     mode="relative")
    # print(f"Executing trajectory: {len(traj)} steps, {traj[0]['X']:.2f}mm per step")
    # api.motion.execute_trajectory(traj, space="cartesian", rate=0.012)
    # print("Trajectory executed. Press Enter to stop.")
    # input()
    # api.stop()

    # =========================================================================
    # Advanced Motion 02: Geometric Primitives (arc, circle, spiral)
    # =========================================================================
    api.start()
    print("Waiting for robot connection...")
    if not api.wait_for_connection(timeout=10):
        print("Timeout waiting for robot. Exiting.")
        api.stop()
        exit(1)
    print(f"Connected. IPOC: {api.monitoring.get_ipoc()}")
    input("Press Enter to start movement...")

    # Generate absolute circle waypoints, convert to relative deltas
    circle_abs = api.motion.generate_circle(
        center={"X": 0, "Y": 0, "Z": 0},
        radius=5, steps=200)

    # Convert absolute → relative (delta between consecutive points)
    # Start prev at first point so there's no initial jump
    circle_rel = []
    prev = circle_abs[0]
    for pt in circle_abs[1:]:
        delta = {k: pt[k] - prev.get(k, 0) for k in pt}
        circle_rel.append(delta)
        prev = pt

    print(f"Executing circle: {len(circle_rel)} relative steps, radius=5mm")
    api.motion.execute_trajectory(circle_rel, space="cartesian", rate=0.012)
    # Zero out corrections so robot stops moving in relative mode
    api.motion.update_cartesian(X=0, Y=0, Z=0)
    print("Circle complete. Press Enter to stop.")
    input()
    api.stop()

    # =========================================================================
    # Advanced Motion 03: Path Blending (smooth corner transitions)
    # =========================================================================
    # api.start()
    # time.sleep(1)
    # traj1 = [{"X": i, "Y": 0, "Z": 0} for i in range(0, 50, 5)]
    # traj2 = [{"X": 50, "Y": i, "Z": 0} for i in range(0, 50, 5)]
    # blended = api.motion.blend_trajectories(traj1, traj2, blend_radius=15)
    # print(f"Blended trajectory: {len(blended)} points")
    # print("Press Enter to stop.")
    # input()
    # api.stop()

    # =========================================================================
    # Advanced Motion 04: Coordinate Transforms (frame conversions)
    # =========================================================================
    # api.start()
    # time.sleep(1)
    # base_pos = {"X": 500, "Y": 100, "Z": 800, "A": 0, "B": 0, "C": 0}
    # world_pos = api.motion.transform_coordinates(
    #     base_pos, from_frame="BASE", to_frame="WORLD",
    #     frame_offset={"X": 100, "Y": 50, "Z": 0})
    # print(f"Base: {base_pos}")
    # print(f"World: {world_pos}")
    # print("Press Enter to stop.")
    # input()
    # api.stop()

    # =========================================================================
    # Advanced Motion 05: Combined Motion (production drilling pattern)
    # =========================================================================
    # api.start()
    # time.sleep(1)
    # # Generate trajectory to work area
    # nav_traj = api.motion.generate_trajectory(
    #     {"X": 0, "Y": 0, "Z": 0},
    #     {"X": 200, "Y": 200, "Z": 0},
    #     steps=30)
    # # Apply S-curve velocity profile
    # profiled = api.motion.generate_velocity_profile(
    #     nav_traj, max_velocity=80, max_acceleration=200, profile='s-curve')
    # print(f"Navigation: {len(profiled)} profiled points")
    # # Spiral inspection pattern
    # inspection = api.motion.generate_spiral(
    #     center={"X": 200, "Y": 200, "Z": 0},
    #     start_radius=5, end_radius=40, pitch=0, revolutions=2, steps=40)
    # # Drilling descent spiral
    # drill = api.motion.generate_spiral(
    #     center={"X": 200, "Y": 200, "Z": 0},
    #     start_radius=3, end_radius=3, pitch=-5.0, revolutions=5, steps=50)
    # print(f"Inspection: {len(inspection)} pts, Drill: {len(drill)} pts")
    # print("Press Enter to stop.")
    # input()
    # api.stop()