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)
Offline
Like button can go here
This post will hold Version 56 of the Python script to implement forward movement of the gripper as directed by the Left Should button #1. The goal is to move the gripper 5 centimeters forward in X while holding Y and Z constant. GW Johnson already provided an outline of what this is going to look like, in the Robot Education topic.
# bridgeV56.py Prepared by Gemini Supervised by Tom Hanson
# Version 56: Implemented L1 (Tool Advance Phase 1: Shoulder Arc Drive & Telemetry Logging).
# Calculates radians, FK joint coordinates (X_B, Z_B), and tip advance per 1cm arc step.
# Version 55: Implemented L2, R1, and R2 shoulder buttons.
#
# ==============================================================================
# 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 (Phase 1 Arc Drive)
# ==============================================================================
import serial
import serial.tools.list_ports
import time
import math
import xml.etree.ElementTree as ET
print("==================================================")
print("Initializing Robot Junction Bridge V56...")
print("Phase 1 Kinematic Engine Active (L1 Tool Advance)")
print("==================================================")
# --- PHYSICAL LINK LENGTH PARAMETERS ---
L1_SHOULDER_MM = 145.0 # Link 1 (Shoulder pivot to Elbow pivot)
L2_ELBOW_MM = 185.0 # Link 2 (Elbow pivot to Wrist pivot)
# --- PWM CONVERSION HELPERS ---
def pwm_to_radians(pwm):
degrees = (pwm - 500) * (180.0 / 2000.0)
return math.radians(degrees)
def radians_to_pwm(rad):
degrees = math.degrees(rad)
pwm = 500 + (degrees * (2000.0 / 180.0))
return int(round(pwm))
def constraint_safety_clip(pulse):
return max(500, min(2500, pulse))
# --- FORWARD KINEMATICS ENGINE ---
def compute_forward_kinematics(theta1_rad, theta2_rad, theta3_rad):
"""
Computes Cartesian coordinates for Elbow (B) and Gripper Tip (T)
based on absolute joint angles referenced to horizontal/vertical frame.
"""
# Elbow Joint B position relative to Shoulder Pivot A (0,0)
x_b = -L1_SHOULDER_MM * math.cos(theta1_rad)
z_b = L1_SHOULDER_MM * math.sin(theta1_rad)
# Absolute angle of Link 2 (Elbow link)
absolute_elbow_angle = theta1_rad + (theta2_rad - math.pi / 2.0)
# Gripper Tip position T
x_tip = x_b - L2_ELBOW_MM * math.cos(absolute_elbow_angle)
z_tip = z_b + L2_ELBOW_MM * math.sin(absolute_elbow_angle)
return x_b, z_b, x_tip, z_tip
# --- 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, 1500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 500, 500, 1500]
READY_TARGET = [1500, 1200, 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
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
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)
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)
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)
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)
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")
# --- STEP 12: L1 (TOOL ADVANCE - PHASE 1 KINEMATIC DRIVE) ---
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 (PHASE 1 OBSERVATION) ---\r\n")
win7.write(b"==================================================\r\n")
# Initial state conversion to Radians & FK
s1_start_pwm = READY_TARGET[1]
s2_start_pwm = READY_TARGET[2]
s3_start_pwm = READY_TARGET[3]
th1_start = pwm_to_radians(s1_start_pwm)
th2_start = pwm_to_radians(s2_start_pwm)
th3_start = pwm_to_radians(s3_start_pwm)
xb_0, zb_0, xtip_0, ztip_0 = compute_forward_kinematics(th1_start, th2_start, th3_start)
win7.write(f" Baseline Ready PWMs -> S1:{s1_start_pwm} | S2:{s2_start_pwm} | S3:{s3_start_pwm}\r\n".encode('utf-8'))
win7.write(f" Baseline Angles(rad)-> S1:{th1_start:.4f} | S2:{th2_start:.4f} | S3:{th3_start:.4f}\r\n".encode('utf-8'))
win7.write(f" Baseline FK Pos (mm)-> Elbow(X:{xb_0:.1f}, Z:{zb_0:.1f}) | Tip(X:{xtip_0:.1f}, Z:{ztip_0:.1f})\r\n\r\n".encode('utf-8'))
# Arc drive parameters (1 cm arc steps, 5 steps total)
arc_step_mm = 10.0
delta_theta1 = arc_step_mm / L1_SHOULDER_MM # ~0.06897 rad per 1cm
for step_i in range(1, 6):
th1_current = th1_start + (step_i * delta_theta1)
s1_next_pwm = constraint_safety_clip(radians_to_pwm(th1_current))
current_arm_positions[1] = s1_next_pwm
xb_cur, zb_cur, xtip_cur, ztip_cur = compute_forward_kinematics(th1_current, th2_start, th3_start)
dx_tip = xtip_cur - xtip_0
dz_tip = ztip_cur - ztip_0
# Physical drive to Servo 1
step_packet = f"#1P{s1_next_pwm}T300\r"
lynx.write(step_packet.encode('utf-8'))
time.sleep(0.35)
# Log step telemetry to Windows 7
deg1 = math.degrees(th1_current)
win7.write(f" [ARC STEP {step_i}/5 (s={step_i}cm)] S1 PWM:{s1_next_pwm} | rad:{th1_current:.4f} ({deg1:.1f}deg)\r\n".encode('utf-8'))
win7.write(f" -> Elbow (X_B:{xb_cur:.1f}, Z_B:{zb_cur:.1f}) mm\r\n".encode('utf-8'))
win7.write(f" -> Tip (X_T:{xtip_cur:.1f}, Z_T:{ztip_cur:.1f}) mm | dX:{dx_tip:+.1f}mm, dZ:{dz_tip:+.1f}mm\r\n".encode('utf-8'))
win7.write(b"\r\n[OPERATOR CARD] -> Step 12 Complete. Phase 1 Telemetry Recorded.\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]: {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)
Offline
Like button can go here
This post will hold V59 of a Python program to bridge between Cokoino game controller and LynxMotion robot arm.
# bridgeV59.py Prepared by Gemini Supervised by Tom Hanson
# Version 59: Integrated ANALOG:ENABLE / ANALOG:DISABLE handshakes into L3/R3 stick toggles.
# Version 58: No change while Cokoino is updated to V58.
# Version 57: Corrected Servo 1 drive direction in L1 to advance forward toward Bow (+X).
# ==============================================================================
# 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 (Forward Bow Drive)
# ==============================================================================
import serial
import serial.tools.list_ports
import time
import math
import xml.etree.ElementTree as ET
print("==================================================")
print("Initializing Robot Junction Bridge V59...")
print("Phase 1 Forward Kinematic Engine Active (L1 Tool Advance)")
print("==================================================")
# --- PHYSICAL LINK LENGTH PARAMETERS ---
L1_SHOULDER_MM = 145.0 # Link 1 (Shoulder pivot to Elbow pivot)
L2_ELBOW_MM = 185.0 # Link 2 (Elbow pivot to Wrist pivot)
# --- PWM CONVERSION HELPERS ---
def pwm_to_radians(pwm):
degrees = (pwm - 500) * (180.0 / 2000.0)
return math.radians(degrees)
def radians_to_pwm(rad):
degrees = math.degrees(rad)
pwm = 500 + (degrees * (2000.0 / 180.0))
return int(round(pwm))
def constraint_safety_clip(pulse):
return max(500, min(2500, pulse))
# --- FORWARD KINEMATICS ENGINE ---
def compute_forward_kinematics(theta1_rad, theta2_rad, theta3_rad):
# Elbow Joint B position relative to Shoulder Pivot A (0,0)
x_b = -L1_SHOULDER_MM * math.cos(theta1_rad)
z_b = L1_SHOULDER_MM * math.sin(theta1_rad)
# Absolute angle of Link 2 (Elbow link)
absolute_elbow_angle = theta1_rad + (theta2_rad - math.pi / 2.0)
# Gripper Tip position T
x_tip = x_b - L2_ELBOW_MM * math.cos(absolute_elbow_angle)
z_tip = z_b + L2_ELBOW_MM * math.sin(absolute_elbow_angle)
return x_b, z_b, x_tip, z_tip
# --- 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, 1500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 500, 500, 1500]
READY_TARGET = [1500, 1200, 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
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
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
cokoino.write(b"ANALOG:DISABLE\n") # Mute analog stream
READY_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
cokoino.write(b"ANALOG:ENABLE\n") # Enable analog stream
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
cokoino.write(b"ANALOG:DISABLE\n") # Mute analog stream
READY_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
cokoino.write(b"ANALOG:ENABLE\n") # Enable analog stream
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)
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)
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")
# --- STEP 12: L1 (TOOL ADVANCE - PHASE 1 FORWARD BOW DRIVE) ---
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 (FORWARD BOW DRIVE) ---\r\n")
win7.write(b"==================================================\r\n")
# Baseline state from READY_TARGET
s1_start_pwm = READY_TARGET[1]
s2_start_pwm = READY_TARGET[2]
s3_start_pwm = READY_TARGET[3]
th1_start = pwm_to_radians(s1_start_pwm)
th2_start = pwm_to_radians(s2_start_pwm)
th3_start = pwm_to_radians(s3_start_pwm)
xb_0, zb_0, xtip_0, ztip_0 = compute_forward_kinematics(th1_start, th2_start, th3_start)
win7.write(f" Baseline Ready PWMs -> S1:{s1_start_pwm} | S2:{s2_start_pwm} | S3:{s3_start_pwm}\r\n".encode('utf-8'))
win7.write(f" Baseline Angles(rad)-> S1:{th1_start:.4f} | S2:{th2_start:.4f} | S3:{th3_start:.4f}\r\n".encode('utf-8'))
win7.write(f" Baseline FK Pos (mm)-> Elbow(X:{xb_0:.1f}, Z:{zb_0:.1f}) | Tip(X:{xtip_0:.1f}, Z:{ztip_0:.1f})\r\n\r\n".encode('utf-8'))
# Arc drive parameters (1 cm arc steps, 5 steps total)
arc_step_mm = 10.0
delta_theta1 = arc_step_mm / L1_SHOULDER_MM # ~0.06897 rad per 1cm
for step_i in range(1, 6):
# INVERTED SIGN: Subtract angle step to advance physically forward toward Bow (+X)
th1_current = th1_start - (step_i * delta_theta1)
s1_next_pwm = constraint_safety_clip(radians_to_pwm(th1_current))
current_arm_positions[1] = s1_next_pwm
xb_cur, zb_cur, xtip_cur, ztip_cur = compute_forward_kinematics(th1_current, th2_start, th3_start)
dx_tip = xtip_cur - xtip_0
dz_tip = ztip_cur - ztip_0
# Physical drive command to Servo 1
step_packet = f"#1P{s1_next_pwm}T300\r"
lynx.write(step_packet.encode('utf-8'))
time.sleep(0.35)
deg1 = math.degrees(th1_current)
win7.write(f" [ARC STEP {step_i}/5 (s={step_i}cm)] S1 PWM:{s1_next_pwm} | rad:{th1_current:.4f} ({deg1:.1f}deg)\r\n".encode('utf-8'))
win7.write(f" -> Elbow (X_B:{xb_cur:.1f}, Z_B:{zb_cur:.1f}) mm\r\n".encode('utf-8'))
win7.write(f" -> Tip (X_T:{xtip_cur:.1f}, Z_T:{ztip_cur:.1f}) mm | dX:{dx_tip:+.1f}mm, dZ:{dz_tip:+.1f}mm\r\n".encode('utf-8'))
win7.write(b"\r\n[OPERATOR CARD] -> Step 12 Complete. Phase 1 Forward Drive Recorded.\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]: {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)
Offline
Like button can go here
This is version 60 of a Python program to bridge between a Cokoino Controller and a LynxMotion robot arm. Gemini and i decided to take the risk of attempting to implement the full mathematical operation to advance the complete linkage 1 centimeter along the arc of the Shoulder arm in the +X direction. I made the decision to take this major step because Gemini seems to be working in top level mode this evening.
# bridgeV60.py Prepared by Gemini Supervised by Tom Hanson
# Version 60: Upgraded Step 12 (L1) from single-joint arc drive to 2D Right-Triangle Inverse Kinematics (+X linear push, Z locked).
# Version 59: Integrated ANALOG:ENABLE / ANALOG:DISABLE handshakes into L3/R3 stick toggles.
# Version 58: No change while Cokoino is updated to V58.
# ==============================================================================
# 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 (Linear IK Thrust +X)
# ==============================================================================
import serial
import serial.tools.list_ports
import time
import math
import xml.etree.ElementTree as ET
print("==================================================")
print("Initializing Robot Junction Bridge V60...")
print("Phase 2 Linear IK Thrust Engine Active (L1 Tool Advance)")
print("==================================================")
# --- PHYSICAL LINK LENGTH PARAMETERS ---
L1_SHOULDER_MM = 145.0 # Link 1 (Shoulder pivot to Elbow pivot)
L2_ELBOW_MM = 185.0 # Link 2 (Elbow pivot to Wrist pivot)
# --- PWM CONVERSION HELPERS ---
def pwm_to_radians(pwm):
degrees = (pwm - 500) * (180.0 / 2000.0)
return math.radians(degrees)
def radians_to_pwm(rad):
degrees = math.degrees(rad)
pwm = 500 + (degrees * (2000.0 / 180.0))
return int(round(pwm))
def constraint_safety_clip(pulse):
return max(500, min(2500, pulse))
# --- FORWARD KINEMATICS ENGINE ---
def compute_forward_kinematics(theta1_rad, theta2_rad, theta3_rad):
# Elbow Joint B position relative to Shoulder Pivot A (0,0)
x_b = -L1_SHOULDER_MM * math.cos(theta1_rad)
z_b = L1_SHOULDER_MM * math.sin(theta1_rad)
# Absolute angle of Link 2 (Elbow link)
absolute_elbow_angle = theta1_rad + (theta2_rad - math.pi / 2.0)
# Gripper Tip position T
x_tip = x_b - L2_ELBOW_MM * math.cos(absolute_elbow_angle)
z_tip = z_b + L2_ELBOW_MM * math.sin(absolute_elbow_angle)
return x_b, z_b, x_tip, z_tip
# --- 2D RIGHT-TRIANGLE INVERSE KINEMATICS ENGINE ---
def compute_inverse_kinematics(x_target, z_target):
"""
Computes required theta1 (shoulder) and theta2 (elbow) angles in radians
to reach target (X, Z) coordinate using 2D geometric right-triangle decomposition.
Returns (theta1_rad, theta2_rad, success_flag).
"""
# 1. Hypotenuse R from Shoulder (0,0) to target (X,Z)
r_hypotenuse = math.sqrt(x_target**2 + z_target**2)
# Physical reach limits check
max_reach = L1_SHOULDER_MM + L2_ELBOW_MM
min_reach = abs(L1_SHOULDER_MM - L2_ELBOW_MM)
if r_hypotenuse > max_reach or r_hypotenuse < min_reach:
return 0.0, 0.0, False # Reach out of bounds
# 2. Base vector angle (beta) from ground to reach line
# Note: X is negative in our convention relative to shoulder orientation
beta_rad = math.atan2(z_target, abs(x_target))
# 3. Interior triangle angle (alpha) via Law of Cosines
cos_alpha = (L1_SHOULDER_MM**2 + r_hypotenuse**2 - L2_ELBOW_MM**2) / (2.0 * L1_SHOULDER_MM * r_hypotenuse)
cos_alpha = max(-1.0, min(1.0, cos_alpha)) # Clamp precision error
alpha_rad = math.acos(cos_alpha)
# 4. Shoulder angle theta1
theta1_rad = beta_rad + alpha_rad
# 5. Interior elbow angle (gamma) via Law of Cosines
cos_gamma = (L1_SHOULDER_MM**2 + L2_ELBOW_MM**2 - r_hypotenuse**2) / (2.0 * L1_SHOULDER_MM * L2_ELBOW_MM)
cos_gamma = max(-1.0, min(1.0, cos_gamma)) # Clamp precision error
gamma_rad = math.acos(cos_gamma)
# Convert interior angle gamma to Servo 2 joint angle definition
theta2_rad = gamma_rad
return theta1_rad, theta2_rad, True
# --- 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, 1500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 500, 500, 1500]
READY_TARGET = [1500, 1200, 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
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
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
cokoino.write(b"ANALOG:DISABLE\n") # Mute analog stream
READY_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
cokoino.write(b"ANALOG:ENABLE\n") # Enable analog stream
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
cokoino.write(b"ANALOG:DISABLE\n") # Mute analog stream
READY_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
cokoino.write(b"ANALOG:ENABLE\n") # Enable analog stream
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)
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)
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")
# --- STEP 12: L1 (TOOL ADVANCE - PHASE 2 LINEAR IK THRUST) ---
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 (2D IK LINEAR THRUST) ---\r\n")
win7.write(b"==================================================\r\n")
# Baseline state from READY_TARGET
s1_start_pwm = READY_TARGET[1]
s2_start_pwm = READY_TARGET[2]
s3_start_pwm = READY_TARGET[3]
th1_start = pwm_to_radians(s1_start_pwm)
th2_start = pwm_to_radians(s2_start_pwm)
th3_start = pwm_to_radians(s3_start_pwm)
xb_0, zb_0, xtip_0, ztip_0 = compute_forward_kinematics(th1_start, th2_start, th3_start)
win7.write(f" Baseline Ready PWMs -> S1:{s1_start_pwm} | S2:{s2_start_pwm} | S3:{s3_start_pwm}\r\n".encode('utf-8'))
win7.write(f" Baseline Angles(rad)-> S1:{th1_start:.4f} | S2:{th2_start:.4f} | S3:{th3_start:.4f}\r\n".encode('utf-8'))
win7.write(f" Baseline Cartesian -> Tip Origin (X:{xtip_0:.1f}, Z:{ztip_0:.1f}) mm\r\n\r\n".encode('utf-8'))
# Linear thrust parameters (10 mm linear steps along +X, Z held constant)
step_delta_x_mm = 10.0
for step_i in range(1, 6):
# Target coordinates in Cartesian space
target_x = xtip_0 - (step_i * step_delta_x_mm) # Moving forward toward bow (+X direction)
target_z = ztip_0 # Lock vertical height steady
th1_ik, th2_ik, ik_success = compute_inverse_kinematics(target_x, target_z)
if not ik_success:
win7.write(f" [IK ERROR] Step {step_i}: Reach out of physical bounds! Aborting thrust.\r\n".encode('utf-8'))
break
s1_next_pwm = constraint_safety_clip(radians_to_pwm(th1_ik))
s2_next_pwm = constraint_safety_clip(radians_to_pwm(th2_ik))
current_arm_positions[1] = s1_next_pwm
current_arm_positions[2] = s2_next_pwm
# FK verification check of solved target
xb_cur, zb_cur, xtip_cur, ztip_cur = compute_forward_kinematics(th1_ik, th2_ik, th3_start)
dx_tip = xtip_cur - xtip_0
dz_tip = ztip_cur - ztip_0
# Dual-servo physical drive command to LynxMotion
step_packet = f"#1P{s1_next_pwm}#2P{s2_next_pwm}T300\r"
lynx.write(step_packet.encode('utf-8'))
time.sleep(0.35)
deg1 = math.degrees(th1_ik)
deg2 = math.degrees(th2_ik)
win7.write(f" [IK STEP {step_i}/5 (s={step_i}cm)] S1:{s1_next_pwm} ({deg1:.1f}deg) | S2:{s2_next_pwm} ({deg2:.1f}deg)\r\n".encode('utf-8'))
win7.write(f" -> Target Cartesian: (X:{target_x:.1f}, Z:{target_z:.1f}) mm\r\n".encode('utf-8'))
win7.write(f" -> Verif FK Location: Tip(X:{xtip_cur:.1f}, Z:{ztip_cur:.1f}) mm | dX:{dx_tip:+.1f}mm, dZ:{dz_tip:+.1f}mm\r\n".encode('utf-8'))
win7.write(b"\r\n[OPERATOR CARD] -> Step 12 Complete. Phase 2 Linear IK Thrust Recorded.\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]: {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}")There are several steps I will perform with this code before I attempt a run. First, I will record the count of bytes that cross between Windows 7 and Linux on the RP5. The count should go way up.
Next, I will run a comparison of V60 with V59. Only new code should appear, and all the existing code should still be present.
Finally, I will read the new code to attempt to get a sense of what it will be doing at run time compared to our plan as posted in the Robotics Education topic this evening.
(th)
Offline
Like button can go here