New Mars Forums

# bridgeV75.py Prepared by Gemini Supervised by Tom Hanson
# Version 75: CALIBRATED PHYSICAL ANGLE ENGINE & START LED RESET RECTIFIED.
#             Corrected pwm_to_physical_radians() to remove trim offsets before angle conversion.
#             Ensured S1 (1386 PWM) = 90.0 deg and S2 (633 PWM) = 0.0 deg physical baseline.
#             Updated START handler to send LED:LOCK:0 clear before LED:LOCK:1 to flush red lockout buffer.
# Version 74: KINEMATIC MATH & BOUNDARY AUDIT RECTIFIED.
#             Corrected 2-DOF Inverse Kinematics (IK) motor-frame alignment.
#             Enforced strict Cartesian altitude (Z-drift) boundary checks in Step 12.
# Version 73: FULL DIAGNOSTIC TELEMETRY RESTORED.
# Version 69: Baseline physical forward kinematics engine with trim offset mapping.

# ==============================================================================
#                 STARBOARD PHYSICAL VIEW vs PORT MOTOR FRAME MAP
# ==============================================================================
# [STERN / 180 deg / 2500us] <- VERTICAL (90 deg / 1500us) -> [BOW / 0 deg / 500us]
# - INCREASING PWM moves joint toward STERN (Aft / Backward / Down in Tuck)
# - DECREASING PWM moves joint toward BOW   (Forward / Up in Tuck)
# ==============================================================================

# ==============================================================================
#                      OFFICIAL FIELD OPERATOR CARD MATRIX
# ==============================================================================
# STEP 0  | Hex x'0' | LED: ROTATING RED LOOP | Power Up: Lockout Safe Mode
# STEP 1  | Hex x'1' | LED: BLUE/WHITE BIT 1  | START Pressed: Runtime Engine Live
# STEP 2  | Hex x'2' | LED: BLUE/WHITE BIT 2  | CROSS Pressed: Query LynxMotion VER
# STEP 3  | Hex x'3' | LED: BLUE/WHITE BIT 3  | CIRCLE Pressed: Transit to TUCK
# STEP 4  | Hex x'4' | LED: BLUE/WHITE BIT 4  | TRIANGLE Pressed: Transit to HOME
# STEP 5  | Hex x'5' | LED: BLUE/WHITE BIT 5  | SQUARE Pressed: Transit to READY (Tuned)
# STEP 6  | Hex x'6' | LED: FLASH/SOLID BLUE | L3 Click: Tune Base/Shoulder (Lock & Save)
# STEP 7  | Hex x'7' | LED: FLASH/SOLID BLUE | R3 Click: Tune Elbow Reach (Lock & Save)
# STEP 8  | Hex x'8' | LED: BLUE/WHITE BIT 8  | D-Pad: Tune Wrist Pitch & Rotate (Live)
# STEP 9  | Hex x'9' | LED: BLUE/WHITE BIT 9  | L2 Pressed: Tool Retract (Transit to READY)
# STEP 10 | Hex x'A' | LED: BLUE/WHITE BIT 10 | R1 Pressed: Ready Position (Transit to READY)
# STEP 11 | Hex x'B' | LED: BLUE/WHITE BIT 11 | R2 Pressed: System Home (Transit to HOME)
# STEP 12 | Hex x'C' | LED: BLUE/WHITE BIT 12 | L1 Pressed: Tool Advance (FK Calculation Audit)
# ==============================================================================

import math
import serial
import serial.tools.list_ports
import time
import xml.etree.ElementTree as ET

# --- PHYSICAL LINK LENGTH PARAMETERS ---
L1_SHOULDER_MM = 145.0  # Link 1 (Shoulder pivot A to Elbow pivot B)
L2_ELBOW_MM = 185.0  # Link 2 (Elbow pivot B to Wrist pivot C)
L3_WRIST_MM = 120.0  # Link 3 (Wrist pivot C to Tool Tip T)

# --- PHYSICAL HARDWARE CALIBRATION OFFSETS (BENCH MEASURED) ---
# Maps ideal target PWM to actual physical PWM required to achieve true alignment.
PHYSICAL_TRIM_OFFSETS = {
    0: 0,  # Base
    1: -114,  # Shoulder (Trims nominal 1500 -> 1386 PWM forward toward Bow for true 90 deg)
    2: 133,  # Elbow (Trims nominal 500 -> 633 PWM for true 0 deg physical extension)
    3: 0,  # Wrist Pitch
    4: 0,  # Wrist Rotate
    5: 0,  # Gripper
}

# Safe operational limits for servo pulse widths (PWM)
SAFE_PWM_MIN = 600
SAFE_PWM_MAX = 2400


# --- PWM & ANGLE CONVERSION HELPERS ---
def constraint_safety_clip(pulse):
    return max(500, min(2500, pulse))


def get_physical_pwm(channel, raw_pwm):
    """Converts a nominal/commanded PWM to the true physical PWM acting on hardware."""
    calibrated_pwm = raw_pwm + PHYSICAL_TRIM_OFFSETS.get(channel, 0)
    return constraint_safety_clip(calibrated_pwm)


def pwm_to_physical_radians(channel, phys_pwm):
    """Converts actual physical PWM to true geometric joint angles (radians).

    Strips hardware calibration trim before angle scaling so bench positions evaluate accurately.
    """
    untrimmed_pwm = phys_pwm - PHYSICAL_TRIM_OFFSETS.get(channel, 0)
    degrees = (untrimmed_pwm - 500) * (180.0 / 2000.0)
    return math.radians(degrees)


def physical_radians_to_pwm(channel, rad):
    """Converts physical joint angle in radians back to calibrated physical PWM."""
    degrees = math.degrees(rad)
    untrimmed_pwm = 500 + (degrees * (2000.0 / 180.0))
    phys_pwm = untrimmed_pwm + PHYSICAL_TRIM_OFFSETS.get(channel, 0)
    return int(round(phys_pwm))


# --- FULL FORWARD KINEMATICS ENGINE (3 JOINTS + TIP) ---
def compute_full_kinematics(theta1_rad, theta2_rad, theta3_rad):
    """Computes Cartesian positions (X, Z) for all arm pivots.

    Origin (0,0) is fixed at Shoulder Pivot A.
    Returns:
        (x_b, z_b)       -> Elbow Pivot B
        (x_c, z_c)       -> Wrist Pivot C
        (x_tip, z_tip)   -> Tool Tip Position T
    """
    # 1. Shoulder Pivot A (Fixed Baseline Foundation)
    x_a = 0.0
    z_a = 0.0

    # 2. Elbow Joint B relative to Shoulder Pivot A
    x_b = L1_SHOULDER_MM * math.cos(theta1_rad)
    z_b = L1_SHOULDER_MM * math.sin(theta1_rad)

    # 3. Absolute angle of Link 2 (Elbow link)
    absolute_elbow_angle = theta1_rad + theta2_rad

    # Wrist Joint C relative to Elbow Joint B
    x_c = x_b + L2_ELBOW_MM * math.cos(absolute_elbow_angle)
    z_c = z_b + L2_ELBOW_MM * math.sin(absolute_elbow_angle)

    # 4. Absolute angle of Link 3 (Wrist link)
    absolute_wrist_angle = absolute_elbow_angle + (theta3_rad - math.pi / 2.0)

    # Gripper Tool Tip T relative to Wrist Joint C
    x_tip = x_c + L3_WRIST_MM * math.cos(absolute_wrist_angle)
    z_tip = z_c + L3_WRIST_MM * math.sin(absolute_wrist_angle)

    return (x_b, z_b), (x_c, z_c), (x_tip, z_tip)


# --- 2-DOF ANALYTICAL INVERSE KINEMATICS ENGINE ---
def solve_inverse_kinematics_2d(target_x, target_z):
    """Calculates required Shoulder (theta1) and Elbow (theta2) joint angles.

    theta1 and theta2 are returned as physical joint angles in radians matching
    the SSC-32 motor frame.
    """
    r_sq = target_x**2 + target_z**2
    r = math.sqrt(r_sq)

    # Reachability Check (Triangle Inequality)
    if r > (L1_SHOULDER_MM + L2_ELBOW_MM) or r < abs(
        L1_SHOULDER_MM - L2_ELBOW_MM
    ):
        return None, None

    # Law of Cosines for Interior Elbow Angle
    cos_theta2_int = (r_sq - L1_SHOULDER_MM**2 - L2_ELBOW_MM**2) / (
        2.0 * L1_SHOULDER_MM * L2_ELBOW_MM
    )
    cos_theta2_int = max(-1.0, min(1.0, cos_theta2_int))
    phi_elbow = math.acos(cos_theta2_int)

    # Base Angle to Target Vector + Angle Offset via Law of Cosines
    gamma = math.atan2(target_z, target_x)
    cos_alpha = (L1_SHOULDER_MM**2 + r_sq - L2_ELBOW_MM**2) / (
        2.0 * L1_SHOULDER_MM * r
    )
    cos_alpha = max(-1.0, min(1.0, cos_alpha))
    alpha = math.acos(cos_alpha)

    # Transform to SSC-32 Motor Frame conventions
    theta1_rad = gamma + alpha
    theta2_rad = phi_elbow - math.pi

    return theta1_rad, theta2_rad


def build_calibrated_macro_packet(target_array, transit_ms):
    """Formats outbound serial strings by applying physical offsets to each joint."""
    packet_parts = []
    for ch in range(6):
        phys_pwm = get_physical_pwm(ch, target_array[ch])
        packet_parts.append(f"#{ch}P{phys_pwm}")
    return "".join(packet_parts) + f"T{transit_ms}\r"


# --- URDF COMPONENT DESCRIPTION PARSING ---
urdf_configuration = """<?xml version="1.0" ?>
<robot name="junction_arm">
    <controller_settings>
        <min_pulse_width>500</min_pulse_width>
        <max_pulse_width>2500</max_pulse_width>
        <center_pulse_width>1500</center_pulse_width>
    </controller_settings>
    <joint channel="0" name="Base"></joint>
    <joint channel="1" name="Shoulder"></joint>
    <joint channel="2" name="Elbow"></joint>
    <joint channel="3" name="Wrist"></joint>
    <joint channel="4" name="Gripper"></joint>
    <joint channel="5" name="Wrist Rot"></joint>
</robot>
"""

root = ET.fromstring(urdf_configuration)

# --- PORT CONFIGURATION ---
all_found_ports = serial.tools.list_ports.comports()
ports = [p for p in all_found_ports if "USB" in p.device.upper()]
ports = sorted(ports, key=lambda x: x.device)

if len(ports) < 3:
    print(f"[ERROR] Found only {len(ports)} physical USB devices.")
    exit(1)

all_indices = {0, 1, 2}
while True:
    try:
        win7_idx = int(input("Enter index number for WINDOWS 7 (HyperTrm): "))
        cokoino_idx = int(input("Enter index number for COKOINO (Arduino): "))

        if (
            win7_idx not in all_indices
            or cokoino_idx not in all_indices
            or win7_idx == cokoino_idx
        ):
            print("\n[CONFLICT DETECTED] Re-enter assignments.\n")
            continue
        lynx_idx = list(all_indices - {win7_idx, cokoino_idx})[0]
        WIN7_PORT = ports[win7_idx].device
        COKOINO_PORT = ports[cokoino_idx].device
        LYNX_PORT = ports[lynx_idx].device
        break
    except ValueError:
        print("[INVALID] Try again.\n")

# --- DATA STATE MEMORY SPACE ---
HOME_TARGET = [1500, 1500, 500, 1500, 1500, 1500]

TUCK_TARGET = [1500, 1821, 1842, 2500, 500, 1500]

READY_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]

current_arm_positions = list(TUCK_TARGET)

LEFT_STEER_LIVE = False
RIGHT_STEER_LIVE = False

BAUD_RATE = 9600
TRANSIT_TIME_MS = 3000
WRIST_STEP_SIZE = 25


def send_led_binary_pattern(ser_conn, count_val):
    if count_val == 0:
        ser_conn.write(b"LED:LOCK:0\n")
    else:
        hex_val = format(count_val, "X")
        ser_conn.write(f"LED:LOCK:{hex_val}\n".encode("utf-8"))


try:
    win7 = serial.Serial(WIN7_PORT, BAUD_RATE, timeout=0.1)
    lynx = serial.Serial(LYNX_PORT, BAUD_RATE, timeout=0.5)
    cokoino = serial.Serial(COKOINO_PORT, BAUD_RATE, timeout=0.1)

    time.sleep(1)
    win7.write(b"\x1b[2J\x1b[H")  # Clear HyperTerminal Screen

    win7.write(b"==================================================\r\n")
    win7.write(b"Initializing Robot Junction Bridge V75...\r\n")
    win7.write(
        b"Phase 1 Forward & Inverse Kinematic Engines Active (Fully"
        b" Logged)\r\n"
    )
    win7.write(b"==================================================\r\n\r\n")

    system_state = 0
    binary_counter = 0
    last_processed_command = ""
    flash_state = False
    last_flash_time = time.time()

    win7.write(b"==================================================\r\n")
    win7.write(b"--- SYSTEM BOOT: STEP 0 (LOCKOUT SAFE MODE) ---\r\n")
    win7.write(b"==================================================\r\n\r\n")

    while True:
        current_time = time.time()

        # Step 0 Lockout Pulse Loop
        if system_state == 0:
            hex_val = format(binary_counter, "X")
            cokoino.write(f"LED:LOCK:{hex_val}\n".encode("utf-8"))
            binary_counter = (binary_counter + 1) % 16
            time.sleep(0.2)

        # Steps 6 & 7 Dynamic LED Flashing Manager
        elif system_state == 6 and LEFT_STEER_LIVE:
            if current_time - last_flash_time > 0.3:
                flash_state = not flash_state
                if flash_state:
                    send_led_binary_pattern(cokoino, 6)
                else:
                    cokoino.write(b"LED:LOCK:0\n")
                last_flash_time = current_time

        elif system_state == 7 and RIGHT_STEER_LIVE:
            if current_time - last_flash_time > 0.3:
                flash_state = not flash_state
                if flash_state:
                    send_led_binary_pattern(cokoino, 7)
                else:
                    cokoino.write(b"LED:LOCK:0\n")
                last_flash_time = current_time

        if cokoino.in_waiting > 0:
            data = cokoino.readline()
            cmd = data.decode("utf-8", errors="ignore").strip()

            if cmd and not cmd.startswith("LED:"):
                win7.write(f" [COKOINO -> BRDG]: {cmd}\r\n".encode("utf-8"))
                cmd_upper = cmd.upper()

                # --- STEP 1 INITIATION ---
                if system_state == 0:
                    if "START" in cmd_upper:
                        system_state = 1
                        # Flush red lockout loop explicitly before setting Blue Bit 1
                        cokoino.write(b"LED:LOCK:0\n")
                        time.sleep(0.05)
                        send_led_binary_pattern(cokoino, 1)
                        win7.write(
                            b"[OPERATOR CARD] -> Step 1: START Detected."
                            b" System Online.\r\n\r\n"
                        )

                # --- RUNTIME CONTROLLER STATE ENGINE ---
                elif system_state >= 1:

                    # Real-time Analog Stream Processing
                    if cmd_upper.startswith("ANALOG:"):
                        parts = cmd_upper.split(":")
                        if len(parts) == 3:
                            axis = parts[1]
                            try:
                                val = int(parts[2])
                                offset = val - 128

                                if LEFT_STEER_LIVE:
                                    if axis == "LX":
                                        step = int(offset * 0.15)
                                        current_arm_positions[0] = (
                                            constraint_safety_clip(
                                                current_arm_positions[0] + step
                                            )
                                        )
                                    elif axis == "LY":
                                        step = int(offset * 0.15)
                                        current_arm_positions[1] = (
                                            constraint_safety_clip(
                                                current_arm_positions[1] + step
                                            )
                                        )

                                    p0 = get_physical_pwm(
                                        0, current_arm_positions[0]
                                    )
                                    p1 = get_physical_pwm(
                                        1, current_arm_positions[1]
                                    )
                                    motion_packet = f"#0P{p0}#1P{p1}T100\r"
                                    lynx.write(motion_packet.encode("utf-8"))
                                    win7.write(
                                        f" [TX -> LYNXMOTION]:"
                                        f" {motion_packet.strip()}\r\n".encode(
                                            "utf-8"
                                        )
                                    )

                                elif RIGHT_STEER_LIVE:
                                    if axis == "RY":
                                        step = int(offset * 0.15)
                                        current_arm_positions[2] = (
                                            constraint_safety_clip(
                                                current_arm_positions[2] - step
                                            )
                                        )

                                        p2 = get_physical_pwm(
                                            2, current_arm_positions[2]
                                        )
                                        motion_packet = f"#2P{p2}T100\r"
                                        lynx.write(
                                            motion_packet.encode("utf-8")
                                        )
                                        win7.write(
                                            f" [TX -> LYNXMOTION]:"
                                            f" {motion_packet.strip()}\r\n".encode(
                                                "utf-8"
                                            )
                                        )

                            except ValueError:
                                pass
                        continue

                    # --- STEP 6: L3 HANDSHAKE (BASE & SHOULDER TUNING) ---
                    elif (
                        "STICK CLICK LEFT" in cmd_upper
                        and "RELEASED" not in cmd_upper
                    ):
                        if LEFT_STEER_LIVE:
                            LEFT_STEER_LIVE = False
                            cokoino.write(b"ANALOG:DISABLE\n")
                            READY_TARGET = list(current_arm_positions)
                            send_led_binary_pattern(cokoino, 6)
                            win7.write(
                                b"[OPERATOR CARD] -> Step 6 Locked."
                                b" READY_TARGET Matrix Updated.\r\n"
                            )
                            win7.write(
                                f" -> Snapshot Committed to READY:"
                                f" {READY_TARGET}\r\n\r\n".encode("utf-8")
                            )
                        else:
                            LEFT_STEER_LIVE = True
                            RIGHT_STEER_LIVE = False
                            cokoino.write(b"ANALOG:ENABLE\n")
                            system_state = 6
                            win7.write(
                                b"[OPERATOR CARD] -> Step 6: Base & Shoulder"
                                b" Steering Live.\r\n\r\n"
                            )
                        continue

                    # --- STEP 7: R3 HANDSHAKE (ELBOW AXIS TUNING) ---
                    elif (
                        "STICK CLICK RIGHT" in cmd_upper
                        and "RELEASED" not in cmd_upper
                    ):
                        if RIGHT_STEER_LIVE:
                            RIGHT_STEER_LIVE = False
                            cokoino.write(b"ANALOG:DISABLE\n")
                            READY_TARGET = list(current_arm_positions)
                            send_led_binary_pattern(cokoino, 7)
                            win7.write(
                                b"[OPERATOR CARD] -> Step 7 Locked."
                                b" READY_TARGET Matrix Updated.\r\n"
                            )
                            win7.write(
                                f" -> Snapshot Committed to READY:"
                                f" {READY_TARGET}\r\n\r\n".encode("utf-8")
                            )
                        else:
                            RIGHT_STEER_LIVE = True
                            LEFT_STEER_LIVE = False
                            cokoino.write(b"ANALOG:ENABLE\n")
                            system_state = 7
                            win7.write(
                                b"[OPERATOR CARD] -> Step 7: Elbow Axis"
                                b" Steering Live.\r\n\r\n"
                            )
                        continue

                    # --- STEP 8: D-PAD WRIST FINE-TUNING ---
                    elif cmd_upper in [
                        "PAD UP",
                        "PAD DOWN",
                        "PAD LEFT",
                        "PAD RIGHT",
                    ]:
                        system_state = 8
                        send_led_binary_pattern(cokoino, 8)

                        if cmd_upper == "PAD UP":
                            current_arm_positions[3] = constraint_safety_clip(
                                current_arm_positions[3] + WRIST_STEP_SIZE
                            )
                        elif cmd_upper == "PAD DOWN":
                            current_arm_positions[3] = constraint_safety_clip(
                                current_arm_positions[3] - WRIST_STEP_SIZE
                            )
                        elif cmd_upper == "PAD LEFT":
                            current_arm_positions[5] = constraint_safety_clip(
                                current_arm_positions[5] - WRIST_STEP_SIZE
                            )
                        elif cmd_upper == "PAD RIGHT":
                            current_arm_positions[5] = constraint_safety_clip(
                                current_arm_positions[5] + WRIST_STEP_SIZE
                            )

                        READY_TARGET[3] = current_arm_positions[3]
                        READY_TARGET[5] = current_arm_positions[5]

                        p3 = get_physical_pwm(3, current_arm_positions[3])
                        p5 = get_physical_pwm(5, current_arm_positions[5])
                        motion_packet = f"#3P{p3}#5P{p5}T150\r"
                        lynx.write(motion_packet.encode("utf-8"))
                        win7.write(
                            f" [TX -> LYNXMOTION]:"
                            f" {motion_packet.strip()}\r\n".encode("utf-8")
                        )
                        win7.write(
                            f"[OPERATOR CARD] -> Step 8: Wrist Adjusted ->"
                            f" Pitch(S3): {current_arm_positions[3]} | Rot(S5):"
                            f" {current_arm_positions[5]}\r\n\r\n".encode(
                                "utf-8"
                            )
                        )
                        continue

                    # --- STEP 9: L2 (TOOL RETRACT -> READY TARGET) ---
                    elif "TOOL RETRACT" in cmd_upper:
                        if "RELEASED" in cmd_upper:
                            if last_processed_command == "TOOL RETRACT":
                                last_processed_command = ""
                        elif last_processed_command != "TOOL RETRACT":
                            last_processed_command = "TOOL RETRACT"
                            system_state = 9
                            send_led_binary_pattern(cokoino, 9)
                            current_arm_positions = list(READY_TARGET)
                            macro_packet = build_calibrated_macro_packet(
                                READY_TARGET, TRANSIT_TIME_MS
                            )
                            lynx.write(macro_packet.encode("utf-8"))
                            win7.write(
                                f" [TX -> LYNXMOTION]:"
                                f" {macro_packet.strip()}\r\n".encode("utf-8")
                            )
                            win7.write(
                                b"[OPERATOR CARD] -> Step 9: Tool Retracted."
                                b" Returned cleanly to READY Target.\r\n\r\n"
                            )

                    # --- STEP 10: R1 (READY POSITION -> READY TARGET) ---
                    elif "READY POSITION" in cmd_upper:
                        if "RELEASED" in cmd_upper:
                            if last_processed_command == "READY POSITION":
                                last_processed_command = ""
                        elif last_processed_command != "READY POSITION":
                            last_processed_command = "READY POSITION"
                            system_state = 10
                            send_led_binary_pattern(cokoino, 10)
                            current_arm_positions = list(READY_TARGET)
                            macro_packet = build_calibrated_macro_packet(
                                READY_TARGET, TRANSIT_TIME_MS
                            )
                            lynx.write(macro_packet.encode("utf-8"))
                            win7.write(
                                f" [TX -> LYNXMOTION]:"
                                f" {macro_packet.strip()}\r\n".encode("utf-8")
                            )
                            win7.write(
                                b"[OPERATOR CARD] -> Step 10: Executing"
                                b" transit to READY Target.\r\n\r\n"
                            )

                    # --- STEP 11: R2 (SYSTEM HOME -> HOME TARGET) ---
                    elif "SYSTEM HOME" in cmd_upper:
                        if "RELEASED" in cmd_upper:
                            if last_processed_command == "SYSTEM HOME":
                                last_processed_command = ""
                        elif last_processed_command != "SYSTEM HOME":
                            last_processed_command = "SYSTEM HOME"
                            system_state = 11
                            send_led_binary_pattern(cokoino, 11)
                            current_arm_positions = list(HOME_TARGET)
                            macro_packet = build_calibrated_macro_packet(
                                HOME_TARGET, TRANSIT_TIME_MS
                            )
                            lynx.write(macro_packet.encode("utf-8"))
                            win7.write(
                                f" [TX -> LYNXMOTION]:"
                                f" {macro_packet.strip()}\r\n".encode("utf-8")
                            )
                            win7.write(
                                b"[OPERATOR CARD] -> Step 11: Executing"
                                b" transit to HOME Target.\r\n\r\n"
                            )

                    # --- STEP 12: L1 (TOOL ADVANCE - FULL FK/IK DIAGNOSTIC & BOUNDARY AUDIT V75) ---
                    elif "TOOL ADVANCE" in cmd_upper:
                        if "RELEASED" in cmd_upper:
                            if last_processed_command == "TOOL ADVANCE":
                                last_processed_command = ""
                        elif last_processed_command != "TOOL ADVANCE":
                            last_processed_command = "TOOL ADVANCE"
                            system_state = 12
                            send_led_binary_pattern(cokoino, 12)  # Hex 'C'

                            win7.write(
                                b"\r\n==================================================\r\n"
                            )
                            win7.write(
                                b"--- STEP 12: L1 TOOL ADVANCE (V75 IK AUDIT"
                                b" ENGINE) ---\r\n"
                            )
                            win7.write(
                                b"---       PHYSICAL MOTION IS MUTED (READ-ONLY)"
                                b"     ---\r\n"
                            )
                            win7.write(
                                b"==================================================\r\n"
                            )

                            # 1. Capture TRUE physical pulse widths matching bench reality
                            s1_phys_pwm = get_physical_pwm(
                                1, current_arm_positions[1]
                            )
                            s2_phys_pwm = get_physical_pwm(
                                2, current_arm_positions[2]
                            )
                            s3_phys_pwm = get_physical_pwm(
                                3, current_arm_positions[3]
                            )

                            # 2. Derive TRUE calibrated physical angles (trim offsets removed)
                            th1_start = pwm_to_physical_radians(1, s1_phys_pwm)
                            th2_start = pwm_to_physical_radians(2, s2_phys_pwm)
                            th3_start = pwm_to_physical_radians(3, s3_phys_pwm)

                            # Compute full joint coordinates at calibrated physical baseline
                            (x_b0, z_b0), (x_c0, z_c0), (x_tip0, z_tip0) = (
                                compute_full_kinematics(
                                    th1_start, th2_start, th3_start
                                )
                            )

                            win7.write(
                                f" TARGET PWMs     -> S1(Shoulder):{current_arm_positions[1]}"
                                f" | S2(Elbow):{current_arm_positions[2]} |"
                                f" S3(Wrist):{current_arm_positions[3]}\r\n".encode(
                                    "utf-8"
                                )
                            )
                            win7.write(
                                f" PHYSICAL PWMs   -> S1(Shoulder):{s1_phys_pwm}"
                                f" | S2(Elbow):{s2_phys_pwm} |"
                                f" S3(Wrist):{s3_phys_pwm}\r\n".encode("utf-8")
                            )
                            win7.write(
                                f" PHYSICAL ANGLES -> S1:{th1_start:.4f} rad"
                                f" ({math.degrees(th1_start):.1f}deg) |"
                                f" S2:{th2_start:.4f} rad"
                                f" ({math.degrees(th2_start):.1f}deg) |"
                                f" S3:{th3_start:.4f} rad"
                                f" ({math.degrees(th3_start):.1f}deg)\r\n".encode(
                                    "utf-8"
                                )
                            )
                            win7.write(
                                f" PHYSICAL COORDS (relative to Pivot A"
                                f" (0,0)):\r\n"
                                f"    - Pivot A (Shoulder) : X:    0.0 mm | Z:"
                                f"    0.0 mm\r\n"
                                f"    - Pivot B (Elbow)    : X: {x_b0:+6.1f} mm |"
                                f" Z: {z_b0:+6.1f} mm\r\n"
                                f"    - Pivot C (Wrist)    : X: {x_c0:+6.1f} mm |"
                                f" Z: {z_c0:+6.1f} mm\r\n"
                                f"    - Tip T   (Tool)     : X: {x_tip0:+6.1f}"
                                f" mm | Z: {z_tip0:+6.1f} mm\r\n".encode(
                                    "utf-8"
                                )
                            )
                            win7.write(
                                b"--------------------------------------------------\r\n"
                            )
                            win7.write(
                                b" EVALUATING +5 CM (+X) ADVANCE AT FIXED"
                                b" ALTITUDE Z (CALCULATION ONLY):\r\n"
                            )

                            audit_failed = False
                            dz_tolerance_mm = (
                                5.0  # Max allowed vertical drift
                            )

                            for step_i in range(1, 6):
                                req_x_c = x_c0 + (step_i * 10.0)  # +1cm to +5cm
                                req_z_c = z_c0  # Fixed Altitude Z

                                ik_th1, ik_th2 = solve_inverse_kinematics_2d(
                                    req_x_c, req_z_c
                                )

                                if ik_th1 is None or ik_th2 is None:
                                    win7.write(
                                        f"\r\n [CALC STEP {step_i}/5"
                                        f" (s=+{step_i}cm)] REJECTED: Geometric"
                                        f" Singularity / Out of Reach"
                                        f" (Target X:{req_x_c:.1f}mm,"
                                        f" Z:{req_z_c:.1f}mm)\r\n".encode(
                                            "utf-8"
                                        )
                                    )
                                    audit_failed = True
                                    break

                                s1_req_pwm = physical_radians_to_pwm(1, ik_th1)
                                s2_req_pwm = physical_radians_to_pwm(2, ik_th2)

                                (
                                    (x_b_cur, z_b_cur),
                                    (x_c_cur, z_c_cur),
                                    (x_tip_cur, z_tip_cur),
                                ) = compute_full_kinematics(
                                    ik_th1, ik_th2, th3_start
                                )

                                dx_tip = x_tip_cur - x_tip0
                                dz_tip = z_tip_cur - z_tip0

                                # Evaluate Safety Bounds against PWM envelopes AND Cartesian vertical drift
                                if (
                                    not (
                                        SAFE_PWM_MIN <= s1_req_pwm <= SAFE_PWM_MAX
                                    )
                                    or not (
                                        SAFE_PWM_MIN <= s2_req_pwm <= SAFE_PWM_MAX
                                    )
                                    or abs(dz_tip) > dz_tolerance_mm
                                ):

                                    win7.write(
                                        f"\r\n [CALC STEP {step_i}/5"
                                        f" (s=+{step_i}cm)] REJECTED: Trajectory"
                                        f" or PWM Boundary Breach! | S1:"
                                        f" {s1_req_pwm} | S2: {s2_req_pwm} | dZ:"
                                        f" {dz_tip:+.1f}mm\r\n".encode("utf-8")
                                    )
                                    audit_failed = True
                                    break

                                deg1 = math.degrees(ik_th1)
                                deg2 = math.degrees(ik_th2)

                                win7.write(
                                    f"\r\n [CALC STEP {step_i}/5"
                                    f" (s=+{step_i}cm)] S1 Phys PWM: {s1_req_pwm}"
                                    f" ({deg1:.1f}deg) | S2 Phys PWM:"
                                    f" {s2_req_pwm} ({deg2:.1f}deg)\r\n".encode(
                                        "utf-8"
                                    )
                                )
                                win7.write(
                                    f"   -> Pivot B (Elbow): X: {x_b_cur:+6.1f}"
                                    f" mm | Z: {z_b_cur:+6.1f} mm\r\n".encode(
                                        "utf-8"
                                    )
                                )
                                win7.write(
                                    f"   -> Pivot C (Wrist): X: {x_c_cur:+6.1f}"
                                    f" mm | Z: {z_c_cur:+6.1f} mm\r\n".encode(
                                        "utf-8"
                                    )
                                )
                                win7.write(
                                    f"   -> Tool Tip T     : X: {x_tip_cur:+6.1f}"
                                    f" mm | Z: {z_tip_cur:+6.1f} mm (dX:"
                                    f" {dx_tip:+5.1f}mm, dZ:"
                                    f" {dz_tip:+5.1f}mm)\r\n".encode("utf-8")
                                )

                            if audit_failed:
                                cokoino.write(b"ERROR:OUT_OF_BOUNDS\n")
                                win7.write(
                                    b"\r\n[OPERATOR CARD] -> Step 12 IK"
                                    b" Trajectory Audit REJECTED (Boundary"
                                    b" Alarm Sent to Cokoino).\r\n\r\n"
                                )
                            else:
                                win7.write(
                                    b"\r\n[OPERATOR CARD] -> Step 12 IK"
                                    b" Trajectory Audit PASSED (+5cm +X Path"
                                    b" Valid). Read-Only (No Lynx Tx).\r\n\r\n"
                                )

                    # --- OTHER SEQUENCED BUTTON EXECUTIONS ---

                    # STEP 2: CROSS -> Query LynxMotion VER
                    if "CROSS" in cmd_upper:
                        if "RELEASED" in cmd_upper:
                            if last_processed_command == "CROSS":
                                last_processed_command = ""
                        elif last_processed_command != "CROSS":
                            last_processed_command = "CROSS"
                            system_state = 2
                            send_led_binary_pattern(cokoino, 2)

                            lynx.write(b"VER\r")
                            win7.write(b" [TX -> LYNXMOTION]: VER\r\n")

                            time.sleep(0.1)
                            ver_response = ""
                            if lynx.in_waiting > 0:
                                ver_response = (
                                    lynx.readline()
                                    .decode("utf-8", errors="ignore")
                                    .strip()
                                )

                            if ver_response:
                                win7.write(
                                    f" [LYNXMOTION -> BRDG]:"
                                    f" {ver_response}\r\n".encode("utf-8")
                                )
                                win7.write(
                                    b"[OPERATOR CARD] -> Step 2: Firmware"
                                    b" Version Verified Successfully.\r\n\r\n"
                                )
                            else:
                                win7.write(
                                    b"[OPERATOR CARD] -> Step 2: VER Query"
                                    b" Sent (No direct response"
                                    b" received).\r\n\r\n"
                                )

                    # STEP 3: CIRCLE -> Travel to TUCK Position
                    elif "CIRCLE" in cmd_upper:
                        if "RELEASED" in cmd_upper:
                            if last_processed_command == "CIRCLE":
                                last_processed_command = ""
                        elif last_processed_command != "CIRCLE":
                            last_processed_command = "CIRCLE"
                            system_state = 3
                            send_led_binary_pattern(cokoino, 3)
                            current_arm_positions = list(TUCK_TARGET)
                            macro_packet = build_calibrated_macro_packet(
                                TUCK_TARGET, TRANSIT_TIME_MS
                            )
                            lynx.write(macro_packet.encode("utf-8"))
                            win7.write(
                                f" [TX -> LYNXMOTION]:"
                                f" {macro_packet.strip()}\r\n".encode("utf-8")
                            )
                            win7.write(
                                b"[OPERATOR CARD] -> Step 3: Traveling cleanly"
                                b" to TUCK configuration.\r\n\r\n"
                            )

                    # STEP 4: TRIANGLE -> Travel to HOME Configuration
                    elif "TRIANGLE" in cmd_upper:
                        if "RELEASED" in cmd_upper:
                            if last_processed_command == "TRIANGLE":
                                last_processed_command = ""
                        elif last_processed_command != "TRIANGLE":
                            last_processed_command = "TRIANGLE"
                            system_state = 4
                            send_led_binary_pattern(cokoino, 4)
                            current_arm_positions = list(HOME_TARGET)
                            macro_packet = build_calibrated_macro_packet(
                                HOME_TARGET, TRANSIT_TIME_MS
                            )
                            lynx.write(macro_packet.encode("utf-8"))
                            win7.write(
                                f" [TX -> LYNXMOTION]:"
                                f" {macro_packet.strip()}\r\n".encode("utf-8")
                            )
                            win7.write(
                                b"[OPERATOR CARD] -> Step 4: Traveling cleanly"
                                b" to HOME configuration.\r\n\r\n"
                            )

                    # STEP 5: SQUARE -> Travel to READY Configuration
                    elif "SQUARE" in cmd_upper:
                        if "RELEASED" in cmd_upper:
                            if last_processed_command == "SQUARE":
                                last_processed_command = ""
                        elif last_processed_command != "SQUARE":
                            last_processed_command = "SQUARE"
                            system_state = 5
                            send_led_binary_pattern(cokoino, 5)
                            current_arm_positions = list(READY_TARGET)
                            macro_packet = build_calibrated_macro_packet(
                                READY_TARGET, TRANSIT_TIME_MS
                            )
                            lynx.write(macro_packet.encode("utf-8"))
                            win7.write(
                                f" [TX -> LYNXMOTION]:"
                                f" {macro_packet.strip()}\r\n".encode("utf-8")
                            )
                            win7.write(
                                b"[OPERATOR CARD] -> Step 5: Traveling to READY"
                                b" configuration.\r\n\r\n"
                            )

        time.sleep(0.01)

except KeyboardInterrupt:
    print("\nBridge safely terminated.")
except Exception as e:
    print(f"\nFatal Runtime Intercept: {e}")