You are not logged in.
This post contains V55 of the Python bridge program.
This version adds functionality for three shoulder buttons.
We've decided to leave the L1 "thrust" function for last, since it requires calculation of movement of three arms.
# bridgeV55.py Prepared by Gemini Supervised by Tom Hanson
# Version 55: Implemented L2 (Tool Retract -> READY_TARGET), R1 (Ready Position -> READY_TARGET),
# and R2 (System Home -> HOME_TARGET). Updated Operator Card Matrix to Step 11.
# Version 54: Added Step 8 D-Pad Wrist Pitch & Wrist Rotate live tuning.
#
# ==============================================================================
# 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)
# ==============================================================================
import serial
import serial.tools.list_ports
import time
import xml.etree.ElementTree as ET
print("==================================================")
print("Initializing Robot Junction Bridge V55...")
print("Shoulder Execution Handlers Active (L2, R1, R2)")
print("==================================================")
# --- 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)
MIN_PULSE = int(root.find(".//min_pulse_width").text)
MAX_PULSE = int(root.find(".//max_pulse_width").text)
CENTER_PULSE = int(root.find(".//center_pulse_width").text)
def constraint_safety_clip(pulse):
return max(MIN_PULSE, min(MAX_PULSE, pulse))
# --- 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 ---
current_arm_positions = [CENTER_PULSE] * 6
HOME_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 500, 500, 1500]
READY_TARGET = [1500, 1200, 1500, 1500, 1500, 1500]
OTHER_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]
LEFT_STEER_LIVE = False
RIGHT_STEER_LIVE = False
current_arm_positions = list(TUCK_TARGET)
OTHER_TARGET = list(TUCK_TARGET)
BAUD_RATE = 9600
TRANSIT_TIME_MS = 3000
WRIST_STEP_SIZE = 25 # Microseconds per D-Pad click
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
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:"):
# Echo raw Cokoino traffic to Windows 7 display
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
send_led_binary_pattern(cokoino, 1)
win7.write(b"[OPERATOR CARD] -> Step 1: START Detected. 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)
motion_packet = f"#0P{current_arm_positions[0]}#1P{current_arm_positions[1]}T100\r"
lynx.write(motion_packet.encode('utf-8'))
win7.write(f" [TX -> LYNXMOTION]: {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)
motion_packet = f"#2P{current_arm_positions[2]}T100\r"
lynx.write(motion_packet.encode('utf-8'))
win7.write(f" [TX -> LYNXMOTION]: {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
READY_TARGET = list(current_arm_positions)
OTHER_TARGET = list(current_arm_positions)
send_led_binary_pattern(cokoino, 6)
win7.write(b"[OPERATOR CARD] -> Step 6 Locked. READY_TARGET Matrix Updated.\r\n")
win7.write(f" -> Snapshot Committed to READY: {READY_TARGET}\r\n\r\n".encode('utf-8'))
else:
LEFT_STEER_LIVE = True
RIGHT_STEER_LIVE = False
system_state = 6
win7.write(b"[OPERATOR CARD] -> Step 6: Base & Shoulder 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
READY_TARGET = list(current_arm_positions)
OTHER_TARGET = list(current_arm_positions)
send_led_binary_pattern(cokoino, 7)
win7.write(b"[OPERATOR CARD] -> Step 7 Locked. READY_TARGET Matrix Updated.\r\n")
win7.write(f" -> Snapshot Committed to READY: {READY_TARGET}\r\n\r\n".encode('utf-8'))
else:
RIGHT_STEER_LIVE = True
LEFT_STEER_LIVE = False
system_state = 7
win7.write(b"[OPERATOR CARD] -> Step 7: Elbow Axis 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]
motion_packet = f"#3P{current_arm_positions[3]}#5P{current_arm_positions[5]}T150\r"
lynx.write(motion_packet.encode('utf-8'))
win7.write(f" [TX -> LYNXMOTION]: {motion_packet.strip()}\r\n".encode('utf-8'))
win7.write(f"[OPERATOR CARD] -> Step 8: Wrist Adjusted -> Pitch(S3): {current_arm_positions[3]} | Rot(S5): {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 = "".join(f"#{j}P{READY_TARGET[j]}" for j in range(6)) + f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" [TX -> LYNXMOTION]: {macro_packet.strip()}\r\n".encode('utf-8'))
win7.write(b"[OPERATOR CARD] -> Step 9: Tool Retracted. 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) # Hex 'A'
current_arm_positions = list(READY_TARGET)
macro_packet = "".join(f"#{j}P{READY_TARGET[j]}" for j in range(6)) + f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" [TX -> LYNXMOTION]: {macro_packet.strip()}\r\n".encode('utf-8'))
win7.write(b"[OPERATOR CARD] -> Step 10: Executing 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) # Hex 'B'
current_arm_positions = list(HOME_TARGET)
macro_packet = "".join(f"#{j}P{HOME_TARGET[j]}" for j in range(6)) + f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" [TX -> LYNXMOTION]: {macro_packet.strip()}\r\n".encode('utf-8'))
win7.write(b"[OPERATOR CARD] -> Step 11: Executing transit to HOME Target.\r\n\r\n")
# --- OPERATOR CARD SEQUENCED BUTTON EXECUTIONS ---
# STEP 2: CROSS -> Query LynxMotion SSC-32U Firmware Version (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]: {ver_response}\r\n".encode('utf-8'))
win7.write(b"[OPERATOR CARD] -> Step 2: Firmware Version Verified Successfully.\r\n\r\n")
else:
win7.write(b"[OPERATOR CARD] -> Step 2: VER Query Sent (No direct response 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 = "".join(f"#{j}P{TUCK_TARGET[j]}" for j in range(6)) + f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" [TX -> LYNXMOTION]: {macro_packet.strip()}\r\n".encode('utf-8'))
win7.write(b"[OPERATOR CARD] -> Step 3: Traveling cleanly 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 = "".join(f"#{j}P{HOME_TARGET[j]}" for j in range(6)) + f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" [TX -> LYNXMOTION]: {macro_packet.strip()}\r\n".encode('utf-8'))
win7.write(b"[OPERATOR CARD] -> Step 4: Traveling cleanly 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 = "".join(f"#{j}P{READY_TARGET[j]}" for j in range(6)) + f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" [TX -> LYNXMOTION]: {macro_packet.strip()}\r\n".encode('utf-8'))
win7.write(b"[OPERATOR CARD] -> Step 5: Traveling to READY 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}")(th)
Online
Like button can go here