22트러블슈팅과 종합 프로젝트

# CAN 상태를 볼 때 자주 쓰는 명령 (RS, Linux)
ip -br link | grep can            # 인터페이스 목록과 UP/DOWN
ip -details link show can0        # 비트레이트, 오류 상태(ERROR-ACTIVE/PASSIVE/BUS-OFF)
candump can0                      # 버스에 흐르는 프레임 엿보기 (can-utils)

SAFE = 0.10                       # 안전 높이: 큐브 위 10 cm

def reachable(p, dz=0.0):
    return arm.ik(p[0], p[1], p[2] + dz, down=True) is not None

def pick(p):
    x, y, z = p
    arm.open()
    arm.move_to(x, y, z + SAFE, t=1.5, down=True)   # 접근점
    arm.move_line(x, y, z, t=1.0)                    # 곧게 하강
    arm.close()
    arm.wait(0.5)                                    # 다 닫힐 때까지
    arm.move_line(x, y, z + SAFE, t=1.0)             # 곧게 상승

def place(p):
    x, y, z = p
    arm.move_to(x, y, z + SAFE, t=1.5, down=True)
    arm.move_line(x, y, z, t=1.0)
    arm.open()
    arm.wait(0.3)
    arm.move_line(x, y, z + SAFE, t=1.0)

start = arm.cube()
if start is None:
    print("보기 옵션에서 큐브를 켜 주세요.")
else:
    z0 = start[2]
    SLOTS = {"A": (0.30, 0.15, z0), "B": (0.40, 0.15, z0), "C": (0.30, -0.15, z0)}

    # R1: 움직이기 전에 전부 검사
    ok = reachable(start) and reachable(start, SAFE)
    for name, p in SLOTS.items():
        good = reachable(p) and reachable(p, SAFE)
        print(f"칸 {name} {p[:2]} → {'도달 가능' if good else '도달 불가'}")
        ok = ok and good

    if not ok:
        print("도달할 수 없는 칸이 있어 실행하지 않습니다.")
    else:
        arm.home()
        here = start                                  # R4: 큐브 위치 기억
        for name in ["A", "B", "C"]:
            pick(here)
            place(SLOTS[name])
            here = SLOTS[name]
            print(f"큐브를 칸 {name} 에 놓았습니다.")
        pick(here)
        place(start)
        print("큐브를 처음 자리로 되돌려 놓았습니다.")
        arm.home()