You are not logged in.
This post contains Version 32 of a Python program to interface Cokoino to LynxMotion.
This version employs ASCII strings coming from Version 17 of Cokoino sketch.
# bridgeV32.py Prepared by Gemini Supervised by Tom Hanson
import serial
import serial.tools.list_ports
import time
import math
print("==================================================")
print("Initializing Robot Junction Bridge V32...")
print("Aligned with Cokoino V17 Serial Protocol")
print("==================================================\n")
# --- PHYSICAL ROBOT ARM PARAMETERS (Limb Dimensions in mm) ---
L1 = 147.0
L2 = 187.5
def pulse_to_radians(pulse):
degrees = (pulse - 1500) / 11.11
return math.radians(degrees + 90.0)
def radians_to_pulse(rad):
degrees = math.degrees(rad) - 90.0
return int(1500 + (degrees * 11.11))
def solve_ik(x, z):
d = math.sqrt(x*x + z*z)
if d > (L1 + L2) or d < abs(L1 - L2):
raise ValueError("Target coordinate mathematically out of physical reach.")
cos_angle_2 = (d*d - L1*L1 - L2*L2) / (2.0 * L1 * L2)
cos_angle_2 = max(-1.0, min(1.0, cos_angle_2))
angle_2 = math.acos(cos_angle_2)
angle_1 = math.atan2(z, x) + math.acos((L1*L1 + d*d - L2*L2) / (2.0 * L1 * d))
angle_3 = math.pi - angle_1 - angle_2
return radians_to_pulse(angle_1), radians_to_pulse(angle_2), radians_to_pulse(angle_3)
# --- STEP 1: SCAN & FILTER FOR PHYSICAL USB DEVICES ---
all_found_ports = serial.tools.list_ports.comports()
ports = []
for p in all_found_ports:
if "USB" in p.device.upper():
ports.append(p)
ports = sorted(ports, key=lambda x: x.device)
if len(ports) < 3:
print(f"[ERROR] Found only {len(ports)} physical USB devices.")
exit(1)
print("Detected Physical USB Serial Devices:")
for idx, port in enumerate(ports):
print(f" [{idx}] {port.device} - {port.description}")
print("")
# --- STEP 2: INTERACTIVE MENU WITH AUTO-DEDUCTION ---
all_indices = {0, 1, 2}
while True:
try:
win7_idx = int(input("Enter index number for WINDOWS 7 (HyperTrm): "))
if win7_idx not in all_indices: continue
cokoino_idx = int(input("Enter index number for COKOINO (Arduino): "))
if cokoino_idx not in all_indices: continue
if 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")
# --- STEP 3: ROBOT CONTEXT QUESTIONNAIRE ---
current_arm_positions = [1500, 1500, 1500, 1500, 1500, 1500]
HOME_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 500, 500, 1500]
dynamic_ready_positions = list(HOME_TARGET)
is_ready_state_set = False
print("\n==================================================")
print(" ROBOT ARM POSITION INITIALIZATION ")
print("==================================================")
print("Where is the physical arm positioned right now?")
print(" [T] Secured in TUCK position (Taped Plate Numbers)")
print(" [H] Standing upright at HOME position (All 1500s)")
print(" [O] OTHER (Manual eyeball estimates)")
print("--------------------------------------------------")
while True:
choice = input("Enter alignment choice (T/H/O): ").strip().upper()
if choice == 'T':
current_arm_positions = list(TUCK_TARGET)
print("\n[SUCCESS] Loaded verified TUCK coordinates into memory.")
break
elif choice == 'H':
current_arm_positions = list(HOME_TARGET)
print("\n[SUCCESS] Loaded standard HOME coordinates into memory.")
break
elif choice == 'O':
print("\nEnter custom pulse width estimates (500-2500):")
names = ["0 (Base)", "1 (Shoulder)", "2 (Elbow)", "3 (Wrist)", "4 (Gripper)", "5 (Wrist Rot)"]
for i in range(6):
while True:
try:
val = int(input(f" Servo {names[i]} position [Default 1500]: ") or 1500)
if 500 <= val <= 2500:
current_arm_positions[i] = val
break
print(" [OUT OF BOUNDS] Keep value between 500 and 2500.")
except ValueError:
print(" [INVALID] Please enter an integer number.")
break
print(f"\nFinalized Startup Vector: {current_arm_positions}\n")
# --- STEP 4: RUNTIME ENGAGEMENT ---
BAUD_RATE = 9600
TRANSIT_TIME_MS = 3000
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")
system_state = 0
binary_counter = 0
win7.write(b"==================================================\r\n")
win7.write(b"--- SYSTEM BOOT: LOCKOUT ACTIVE (SAFE MODE) ---\r\n")
win7.write(b"--- Physical Cokoino NeoPixels: Looping Red ---\r\n")
win7.write(b"==================================================\r\n\r\n")
last_processed_command = ""
while True:
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)
if cokoino.in_waiting > 0:
data = cokoino.readline()
cmd = data.decode('utf-8', errors='ignore').strip()
if cmd and not cmd.startswith("LED:"):
cmd_upper = cmd.upper()
if cmd != last_processed_command:
last_processed_command = ""
if "RELEASE" in cmd_upper:
last_processed_command = ""
continue
# STATE 0: Safe Handshake Look for "START"
if system_state == 0:
if "START" in cmd_upper:
system_state = 1
cokoino.write(b"LED:STATE:GREEN\n")
win7.write(b"[!!!] START DETECTED [!!!]\r\n")
win7.write(b"--- COKOINO READY FOR PS/2 COMMANDS ---\r\n\r\n")
# STATE 1: Unlocked Commands
elif system_state == 1:
# CROSS (Diagnostics)
if "CROSS" in cmd_upper:
if last_processed_command != "CROSS":
last_processed_command = "CROSS"
win7.write(b"[FROM GAMEPAD] -> CROSS\r\n")
win7.write(b"[TO LYNXMOTION] -> Exact String: 'VER\\r'\r\n")
lynx.reset_input_buffer()
lynx.write(b"VER\r")
time.sleep(0.1)
if lynx.in_waiting > 0:
response = lynx.readline().decode('utf-8', errors='ignore').strip()
if response:
win7.write(f"==> LYNXMOTION RESPONSE: '{response}'\r\n\r\n".encode('utf-8'))
cokoino.write(b"LED:VER:SUCCESS\n")
else:
cokoino.write(b"LED:VER:FAULT\n")
else:
cokoino.write(b"LED:VER:FAULT\n")
# TRIANGLE (Home Macro)
elif "TRIANGLE" in cmd_upper:
if last_processed_command != "TRIANGLE":
last_processed_command = "TRIANGLE"
win7.write(b"[FROM GAMEPAD] -> TRIANGLE\r\n")
win7.write(b"[TO LYNXMOTION] -> Dispatching Synchronized Home Macro:\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = HOME_TARGET[joint]
macro_packet += f"#{joint}P{HOME_TARGET[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
# CIRCLE (Tuck Macro)
elif "CIRCLE" in cmd_upper:
if last_processed_command != "CIRCLE":
last_processed_command = "CIRCLE"
win7.write(b"[FROM GAMEPAD] -> CIRCLE\r\n")
win7.write(b"[TO LYNXMOTION] -> Dispatching Synchronized Tuck Macro:\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = TUCK_TARGET[joint]
macro_packet += f"#{joint}P{TUCK_TARGET[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
# SELECT/RESET (Memory Lock for Ready State)
elif "SELECT" in cmd_upper:
if last_processed_command != "SELECT":
last_processed_command = "SELECT"
dynamic_ready_positions = list(current_arm_positions)
is_ready_state_set = True
win7.write(b"==================================================\r\n")
win7.write(b"[MEMORY LOCK] Dynamic READY Position Established!\r\n")
win7.write(f"Captured Hardware Footprint: {dynamic_ready_positions}\r\n".encode('utf-8'))
win7.write(b"==================================================\r\n\r\n")
cokoino.write(b"LED:SET:SUCCESS\n")
# ALIGNED MATCH: L1 ADVANCE THRUST
elif "TOOL ADVANCE" in cmd_upper:
if last_processed_command != "ADVANCE":
last_processed_command = "ADVANCE"
win7.write(b"[FROM GAMEPAD] -> L1: TOOL ADVANCE DETECTED\r\n")
if not is_ready_state_set:
win7.write(b"[ABORT] Cannot thrust. Operator must press SELECT first to lock baseline.\r\n\r\n")
continue
try:
win7.write(b"[INVERSE KINEMATICS] Computing 3cm Linear Path...\r\n")
th1_start = pulse_to_radians(dynamic_ready_positions[1])
th2_start = pulse_to_radians(dynamic_ready_positions[2])
x_start = L1 * math.cos(th1_start) + L2 * math.cos(th1_start + th2_start)
z_start = L1 * math.sin(th1_start) + L2 * math.sin(th1_start + th2_start)
win7.write(f" Calculated Start Plane: X={x_start:.1f}mm, Z={z_start:.1f}mm\r\n".encode('utf-8'))
win7.write(b" Executing Damped Incremental Forward Push:\r\n")
for step in [10.0, 20.0, 30.0]:
target_x = x_start + step
target_z = z_start
p1, p2, p3 = solve_ik(target_x, target_z)
current_arm_positions[1] = p1
current_arm_positions[2] = p2
current_arm_positions[3] = p3
packet = f"#1P{p1}#2P{p2}#3P{p3}T1000\r"
lynx.write(packet.encode('utf-8'))
win7.write(f" Segment {step/10:.0f}cm -> {packet.strip()} [\\r]\r\n".encode('utf-8'))
time.sleep(1.0)
win7.write(b"[THRUST COMPLETE]\r\n\r\n")
except Exception as math_err:
win7.write(f"[MATH ERROR] Trajectory boundary reached: {math_err}\r\n\r\n".encode('utf-8'))
# ALIGNED MATCH: L2 RETRACT TO READY
elif "TOOL RETRACT" in cmd_upper:
if last_processed_command != "RETRACT":
last_processed_command = "RETRACT"
win7.write(b"[FROM GAMEPAD] -> L2: TOOL RETRACT DETECTED\r\n")
win7.write(b"[TO LYNXMOTION] -> Retracting Linear Tool Head back to Ready Matrix:\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = dynamic_ready_positions[joint]
macro_packet += f"#{joint}P{dynamic_ready_positions[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
time.sleep(0.01)
except KeyboardInterrupt:
print("\nBridge safely terminated.")
except Exception as e:
print(f"\nBridge Error: {e}")In this version we are testing new code to perform a linear motion in X.
This movement requires complex calculation of linked physical parts of the arm.
(th)
Offline
Like button can go here
This post contains Version 33 of a Python program to interface between Cokoino LKK0017 and LynxMotion robot arm. In this version, we attempt to address the issue of direction when we move the arms. In addition, we restore the limit on values that can be transmitted. LynxMotion servos are limited to values of 500 to 2500. Finally, we protect the gripper, which must not change once a tool has been mounted.
# bridgeV33.py Created by Gemini Supervised by Tom Hanson
import serial
import serial.tools.list_ports
import time
import math
print("==================================================")
print("Initializing Robot Junction Bridge V33...")
print("Directional Awareness & Pulse Boundary Safety Active")
print("==================================================\n")
# --- PHYSICAL ROBOT ARM PARAMETERS (Limb Dimensions in mm) ---
L1 = 147.0
L2 = 187.5
def pulse_to_radians(pulse):
degrees = (pulse - 1500) / 11.11
return math.radians(degrees + 90.0)
def radians_to_pulse(rad):
degrees = math.degrees(rad) - 90.0
return int(1500 + (degrees * 11.11))
def constraint_safety_clip(pulse):
"""Enforces strict LynxMotion hardware boundaries (500 to 2500)."""
return max(500, min(2500, pulse))
def solve_ik(x, z):
d = math.sqrt(x*x + z*z)
if d > (L1 + L2) or d < abs(L1 - L2):
raise ValueError("Target coordinate mathematically out of physical reach.")
cos_angle_2 = (d*d - L1*L1 - L2*L2) / (2.0 * L1 * L2)
cos_angle_2 = max(-1.0, min(1.0, cos_angle_2))
angle_2 = math.acos(cos_angle_2)
angle_1 = math.atan2(z, x) + math.acos((L1*L1 + d*d - L2*L2) / (2.0 * L1 * d))
angle_3 = math.pi - angle_1 - angle_2
# Raw trigonometric conversions
raw_p1 = radians_to_pulse(angle_1)
p2 = constraint_safety_clip(radians_to_pulse(angle_2))
p3 = constraint_safety_clip(radians_to_pulse(angle_3))
# DIRECTIONAL AWARENESS FIX:
# If the math wants to go above 1500 to reach forward, invert it so the pulse
# decreases below 1500 instead, driving the shoulder "North".
delta_p1 = raw_p1 - 1500
p1 = constraint_safety_clip(1500 - delta_p1)
return p1, p2, p3
# --- STEP 1: SCAN & FILTER FOR PHYSICAL USB DEVICES ---
all_found_ports = serial.tools.list_ports.comports()
ports = []
for p in all_found_ports:
if "USB" in p.device.upper():
ports.append(p)
ports = sorted(ports, key=lambda x: x.device)
if len(ports) < 3:
print(f"[ERROR] Found only {len(ports)} physical USB devices.")
exit(1)
print("Detected Physical USB Serial Devices:")
for idx, port in enumerate(ports):
print(f" [{idx}] {port.device} - {port.description}")
print("")
# --- STEP 2: INTERACTIVE MENU WITH AUTO-DEDUCTION ---
all_indices = {0, 1, 2}
while True:
try:
win7_idx = int(input("Enter index number for WINDOWS 7 (HyperTrm): "))
if win7_idx not in all_indices: continue
cokoino_idx = int(input("Enter index number for COKOINO (Arduino): "))
if cokoino_idx not in all_indices: continue
if 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")
# --- STEP 3: ROBOT CONTEXT QUESTIONNAIRE ---
current_arm_positions = [1500, 1500, 1500, 1500, 1500, 1500]
HOME_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 500, 500, 1500]
dynamic_ready_positions = list(HOME_TARGET)
is_ready_state_set = False
print("\n==================================================")
print(" ROBOT ARM POSITION INITIALIZATION ")
print("==================================================")
print("Where is the physical arm positioned right now?")
print(" [T] Secured in TUCK position (Taped Plate Numbers)")
print(" [H] Standing upright at HOME position (All 1500s)")
print(" [O] OTHER (Manual eyeball estimates)")
print("--------------------------------------------------")
while True:
choice = input("Enter alignment choice (T/H/O): ").strip().upper()
if choice == 'T':
current_arm_positions = list(TUCK_TARGET)
print("\n[SUCCESS] Loaded verified TUCK coordinates into memory.")
break
elif choice == 'H':
current_arm_positions = list(HOME_TARGET)
print("\n[SUCCESS] Loaded standard HOME coordinates into memory.")
break
elif choice == 'O':
print("\nEnter custom pulse width estimates (500-2500):")
names = ["0 (Base)", "1 (Shoulder)", "2 (Elbow)", "3 (Wrist)", "4 (Gripper)", "5 (Wrist Rot)"]
for i in range(6):
while True:
try:
val = int(input(f" Servo {names[i]} position [Default 1500]: ") or 1500)
if 500 <= val <= 2500:
current_arm_positions[i] = val
break
print(" [OUT OF BOUNDS] Keep value between 500 and 2500.")
except ValueError:
print(" [INVALID] Please enter an integer number.")
break
print(f"\nFinalized Startup Vector: {current_arm_positions}\n")
# --- STEP 4: RUNTIME ENGAGEMENT ---
BAUD_RATE = 9600
TRANSIT_TIME_MS = 3000
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")
system_state = 0
binary_counter = 0
win7.write(b"==================================================\r\n")
win7.write(b"--- SYSTEM BOOT: LOCKOUT ACTIVE (SAFE MODE) ---\r\n")
win7.write(b"--- Physical Cokoino NeoPixels: Looping Red ---\r\n")
win7.write(b"==================================================\r\n\r\n")
last_processed_command = ""
while True:
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)
if cokoino.in_waiting > 0:
data = cokoino.readline()
cmd = data.decode('utf-8', errors='ignore').strip()
if cmd and not cmd.startswith("LED:"):
cmd_upper = cmd.upper()
if cmd != last_processed_command:
last_processed_command = ""
if "RELEASE" in cmd_upper:
last_processed_command = ""
continue
if system_state == 0:
if "START" in cmd_upper:
system_state = 1
cokoino.write(b"LED:STATE:GREEN\n")
win7.write(b"[!!!] START DETECTED [!!!]\r\n")
win7.write(b"--- COKOINO READY FOR PS/2 COMMANDS ---\r\n\r\n")
elif system_state == 1:
# CROSS (Diagnostics)
if "CROSS" in cmd_upper:
if last_processed_command != "CROSS":
last_processed_command = "CROSS"
win7.write(b"[FROM GAMEPAD] -> CROSS\r\n")
win7.write(b"[TO LYNXMOTION] -> Exact String: 'VER\\r'\r\n")
lynx.reset_input_buffer()
lynx.write(b"VER\r")
time.sleep(0.1)
if lynx.in_waiting > 0:
response = lynx.readline().decode('utf-8', errors='ignore').strip()
if response:
win7.write(f"==> LYNXMOTION RESPONSE: '{response}'\r\n\r\n".encode('utf-8'))
cokoino.write(b"LED:VER:SUCCESS\n")
else:
cokoino.write(b"LED:VER:FAULT\n")
else:
cokoino.write(b"LED:VER:FAULT\n")
# TRIANGLE (Home Macro)
elif "TRIANGLE" in cmd_upper:
if last_processed_command != "TRIANGLE":
last_processed_command = "TRIANGLE"
win7.write(b"[FROM GAMEPAD] -> TRIANGLE\r\n")
win7.write(b"[TO LYNXMOTION] -> Dispatching Synchronized Home Macro:\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = HOME_TARGET[joint]
macro_packet += f"#{joint}P{HOME_TARGET[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
# CIRCLE (Tuck Macro)
elif "CIRCLE" in cmd_upper:
if last_processed_command != "CIRCLE":
last_processed_command = "CIRCLE"
win7.write(b"[FROM GAMEPAD] -> CIRCLE\r\n")
win7.write(b"[TO LYNXMOTION] -> Dispatching Synchronized Tuck Macro:\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = TUCK_TARGET[joint]
macro_packet += f"#{joint}P{TUCK_TARGET[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
# SELECT/RESET (Memory Lock for Ready State)
elif "SELECT" in cmd_upper:
if last_processed_command != "SELECT":
last_processed_command = "SELECT"
dynamic_ready_positions = list(current_arm_positions)
is_ready_state_set = True
win7.write(b"==================================================\r\n")
win7.write(b"[MEMORY LOCK] Dynamic READY Position Established!\r\n")
win7.write(f"Captured Hardware Footprint: {dynamic_ready_positions}\r\n".encode('utf-8'))
win7.write(b"==================================================\r\n\r\n")
cokoino.write(b"LED:SET:SUCCESS\n")
# L1 BUTTON: COORDINATED LINEAR THRUST
elif "TOOL ADVANCE" in cmd_upper:
if last_processed_command != "ADVANCE":
last_processed_command = "ADVANCE"
win7.write(b"[FROM GAMEPAD] -> L1: TOOL ADVANCE DETECTED\r\n")
if not is_ready_state_set:
win7.write(b"[ABORT] Cannot thrust. Operator must press SELECT first to lock baseline.\r\n\r\n")
continue
try:
win7.write(b"[INVERSE KINEMATICS] Computing Direction-Aware 3cm Linear Path...\r\n")
# Use a virtual copy of the shoulder position to establish initial math space
# If the physical shoulder position is inverted, we must virtualize it back to standard trig space
physical_p1 = dynamic_ready_positions[1]
virtual_p1 = 1500 - (physical_p1 - 1500)
th1_start = pulse_to_radians(virtual_p1)
th2_start = pulse_to_radians(dynamic_ready_positions[2])
x_start = L1 * math.cos(th1_start) + L2 * math.cos(th1_start + th2_start)
z_start = L1 * math.sin(th1_start) + L2 * math.sin(th1_start + th2_start)
win7.write(f" Calculated Start Plane: X={x_start:.1f}mm, Z={z_start:.1f}mm\r\n".encode('utf-8'))
win7.write(b" Executing Damped Incremental Forward Push:\r\n")
for step in [10.0, 20.0, 30.0]:
target_x = x_start + step
target_z = z_start
p1, p2, p3 = solve_ik(target_x, target_z)
current_arm_positions[1] = p1
current_arm_positions[2] = p2
current_arm_positions[3] = p3
# Hard requirement 3: Gripper (Ch 4) remains completely untouched
packet = f"#1P{p1}#2P{p2}#3P{p3}T1000\r"
lynx.write(packet.encode('utf-8'))
win7.write(f" Segment {step/10:.0f}cm -> {packet.strip()} [\\r]\r\n".encode('utf-8'))
time.sleep(1.0)
win7.write(b"[THRUST COMPLETE]\r\n\r\n")
except Exception as math_err:
win7.write(f"[MATH ERROR] Trajectory boundary reached: {math_err}\r\n\r\n".encode('utf-8'))
# L2 BUTTON: RETRACT TO READY
elif "TOOL RETRACT" in cmd_upper:
if last_processed_command != "RETRACT":
last_processed_command = "RETRACT"
win7.write(b"[FROM GAMEPAD] -> L2: TOOL RETRACT DETECTED\r\n")
win7.write(b"[TO LYNXMOTION] -> Retracting Linear Tool Head back to Ready Matrix:\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = dynamic_ready_positions[joint]
macro_packet += f"#{joint}P{dynamic_ready_positions[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
time.sleep(0.01)
except KeyboardInterrupt:
print("\nBridge safely terminated.")
except Exception as e:
print(f"\nBridge Error: {e}")(th)
Offline
Like button can go here
This post contains Version 34 of a series of Python programs designed to interface between a Cokoino CKK0017 control board and a LynxMotion robot arm.
In this version, we are abstracting the hardware layer after initial experiments with had coded dimensions led to unexpected behavior.
With the addition of programmed movements, we are entering a new (higher) level of design. in earlier versions we were just trying to achieve connectivity at all, and then we performed simple movements based upon known position data. Now we are attempting to compute a complex movement, and our initial experiments have not turned out well.
If you examine the new version, you should find an XML structure that (I'm told) models an industry standard.
# bridgeV34.py Prepared by Gemini Supervised by Tom Hanson
import serial
import serial.tools.list_ports
import time
import math
import xml.etree.ElementTree as ET
print("==================================================")
print("Initializing Robot Junction Bridge V34...")
print("XML Hardware Abstraction Layer (HAL) Integrated")
print("==================================================\n")
# ==============================================================================
# STEP 0: UNIFIED ROBOT DESCRIPTION FORMAT (URDF) - XML PARAMETERS
# ==============================================================================
# This XML string represents the industry-standard way to describe a robot's
# physical limbs (links) and motors (joints). The Python engine parses this
# dynamically so that no dimensions or boundaries are hardcoded in the loop.
# ==============================================================================
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>
<pulses_per_degree>11.11</pulses_per_degree>
</controller_settings>
<link name="link_1_shoulder">
<length_mm>147.0</length_mm>
<mass_g>PLACEHOLDER</mass_g>
</link>
<link name="link_2_elbow">
<length_mm>187.5</length_mm>
<mass_g>PLACEHOLDER</mass_g>
</link>
<link name="link_3_tool_head">
<length_mm>0.0</length_mm> </link>
<joint channel="0" name="Base">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>Z_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="1" name="Shoulder">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>true</invert_direction> <axis_orientation>Y_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="2" name="Elbow">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>Y_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="3" name="Wrist">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>Y_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="4" name="Gripper">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>IMMUTABLE_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="5" name="Wrist Rot">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>X_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
</robot>
"""
# --- DYNAMIC XML PARSER AND HAL ROUTINES ---
root = ET.fromstring(urdf_configuration)
# 1. Parse Controller Constants
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)
P_PER_DEG = float(root.find(".//pulses_per_degree").text)
# 2. Parse Physical Link Lengths dynamically into the Kinematics variables
L1 = float(root.find(".//link[@name='link_1_shoulder']/length_mm").text)
L2 = float(root.find(".//link[@name='link_2_elbow']/length_mm").text)
# 3. Parse Joint Inversion Map
JOINT_INVERT_MAP = {}
for joint in root.findall(".//joint"):
ch = int(joint.get("channel"))
inv_str = joint.find("invert_direction").text.lower()
JOINT_INVERT_MAP[ch] = (inv_str == "true")
def pulse_to_radians(pulse):
degrees = (pulse - CENTER_PULSE) / P_PER_DEG
return math.radians(degrees + 90.0)
def radians_to_pulse(rad):
degrees = math.degrees(rad) - 90.0
return int(CENTER_PULSE + (degrees * P_PER_DEG))
def constraint_safety_clip(pulse):
"""Enforces strict software limits dynamically parsed from XML."""
return max(MIN_PULSE, min(MAX_PULSE, pulse))
def solve_ik(x, z):
d = math.sqrt(x*x + z*z)
if d > (L1 + L2) or d < abs(L1 - L2):
raise ValueError("Target coordinate mathematically out of physical reach.")
cos_angle_2 = (d*d - L1*L1 - L2*L2) / (2.0 * L1 * L2)
cos_angle_2 = max(-1.0, min(1.0, cos_angle_2))
angle_2 = math.acos(cos_angle_2)
angle_1 = math.atan2(z, x) + math.acos((L1*L1 + d*d - L2*L2) / (2.0 * L1 * d))
angle_3 = math.pi - angle_1 - angle_2
# Calculate raw trigonometric pulses
raw_p1 = radians_to_pulse(angle_1)
raw_p2 = radians_to_pulse(angle_2)
raw_p3 = radians_to_pulse(angle_3)
# DYNAMIC INVERSION HANDLING:
# Look up Channel 1 property in XML map. If invert_direction is True, mirror it.
if JOINT_INVERT_MAP.get(1, False):
delta_p1 = raw_p1 - CENTER_PULSE
p1 = constraint_safety_clip(CENTER_PULSE - delta_p1)
else:
p1 = constraint_safety_clip(raw_p1)
# Handle Channel 2 and 3 using the XML map setup for generic safety mapping
p2 = constraint_safety_clip(CENTER_PULSE - (raw_p2 - CENTER_PULSE)) if JOINT_INVERT_MAP.get(2, False) else constraint_safety_clip(raw_p2)
p3 = constraint_safety_clip(CENTER_PULSE - (raw_p3 - CENTER_PULSE)) if JOINT_INVERT_MAP.get(3, False) else constraint_safety_clip(raw_p3)
return p1, p2, p3
# --- STEP 1: SCAN & FILTER FOR PHYSICAL USB DEVICES ---
all_found_ports = serial.tools.list_ports.comports()
ports = []
for p in all_found_ports:
if "USB" in p.device.upper():
ports.append(p)
ports = sorted(ports, key=lambda x: x.device)
if len(ports) < 3:
print(f"[ERROR] Found only {len(ports)} physical USB devices.")
exit(1)
print("Detected Physical USB Serial Devices:")
for idx, port in enumerate(ports):
print(f" [{idx}] {port.device} - {port.description}")
print("")
# --- STEP 2: INTERACTIVE MENU WITH AUTO-DEDUCTION ---
all_indices = {0, 1, 2}
while True:
try:
win7_idx = int(input("Enter index number for WINDOWS 7 (HyperTrm): "))
if win7_idx not in all_indices: continue
cokoino_idx = int(input("Enter index number for COKOINO (Arduino): "))
if cokoino_idx not in all_indices: continue
if 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")
# --- STEP 3: ROBOT CONTEXT QUESTIONNAIRE ---
current_arm_positions = [CENTER_PULSE] * 6
HOME_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 500, 500, 1500]
dynamic_ready_positions = list(HOME_TARGET)
is_ready_state_set = False
print("\n==================================================")
print(" ROBOT ARM POSITION INITIALIZATION ")
print("==================================================")
print("Where is the physical arm positioned right now?")
print(" [T] Secured in TUCK position (Taped Plate Numbers)")
print(" [H] Standing upright at HOME position (All 1500s)")
print(" [O] OTHER (Manual eyeball estimates)")
print("--------------------------------------------------")
while True:
choice = input("Enter alignment choice (T/H/O): ").strip().upper()
if choice == 'T':
current_arm_positions = list(TUCK_TARGET)
print("\n[SUCCESS] Loaded verified TUCK coordinates into memory.")
break
elif choice == 'H':
current_arm_positions = list(HOME_TARGET)
print("\n[SUCCESS] Loaded standard HOME coordinates into memory.")
break
elif choice == 'O':
print("\nEnter custom pulse width estimates (500-2500):")
names = ["0 (Base)", "1 (Shoulder)", "2 (Elbow)", "3 (Wrist)", "4 (Gripper)", "5 (Wrist Rot)"]
for i in range(6):
while True:
try:
val = int(input(f" Servo {names[i]} position [Default 1500]: ") or 1500)
if MIN_PULSE <= val <= MAX_PULSE:
current_arm_positions[i] = val
break
print(f" [OUT OF BOUNDS] Keep value between {MIN_PULSE} and {MAX_PULSE}.")
except ValueError:
print(" [INVALID] Please enter an integer number.")
break
print(f"\nFinalized Startup Vector: {current_arm_positions}\n")
# --- STEP 4: RUNTIME ENGAGEMENT ---
BAUD_RATE = 9600
TRANSIT_TIME_MS = 3000
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")
system_state = 0
binary_counter = 0
win7.write(b"==================================================\r\n")
win7.write(b"--- SYSTEM BOOT: LOCKOUT ACTIVE (SAFE MODE) ---\r\n")
win7.write(b"--- Physical Cokoino NeoPixels: Looping Red ---\r\n")
win7.write(b"==================================================\r\n\r\n")
last_processed_command = ""
while True:
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)
if cokoino.in_waiting > 0:
data = cokoino.readline()
cmd = data.decode('utf-8', errors='ignore').strip()
if cmd and not cmd.startswith("LED:"):
cmd_upper = cmd.upper()
if cmd != last_processed_command:
last_processed_command = ""
if "RELEASE" in cmd_upper:
last_processed_command = ""
continue
if system_state == 0:
if "START" in cmd_upper:
system_state = 1
cokoino.write(b"LED:STATE:GREEN\n")
win7.write(b"[!!!] START DETECTED [!!!]\r\n")
win7.write(b"--- COKOINO READY FOR PS/2 COMMANDS ---\r\n\r\n")
elif system_state == 1:
# CROSS (Diagnostics)
if "CROSS" in cmd_upper:
if last_processed_command != "CROSS":
last_processed_command = "CROSS"
win7.write(b"[FROM GAMEPAD] -> CROSS\r\n")
win7.write(b"[TO LYNXMOTION] -> Exact String: 'VER\\r'\r\n")
lynx.reset_input_buffer()
lynx.write(b"VER\r")
time.sleep(0.1)
if lynx.in_waiting > 0:
response = lynx.readline().decode('utf-8', errors='ignore').strip()
if response:
win7.write(f"==> LYNXMOTION RESPONSE: '{response}'\r\n\r\n".encode('utf-8'))
cokoino.write(b"LED:VER:SUCCESS\n")
else:
cokoino.write(b"LED:VER:FAULT\n")
else:
cokoino.write(b"LED:VER:FAULT\n")
# TRIANGLE (Home Macro)
elif "TRIANGLE" in cmd_upper:
if last_processed_command != "TRIANGLE":
last_processed_command = "TRIANGLE"
win7.write(b"[FROM GAMEPAD] -> TRIANGLE\r\n")
win7.write(b"[TO LYNXMOTION] -> Dispatching Synchronized Home Macro:\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = HOME_TARGET[joint]
macro_packet += f"#{joint}P{HOME_TARGET[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
# CIRCLE (Tuck Macro)
elif "CIRCLE" in cmd_upper:
if last_processed_command != "CIRCLE":
last_processed_command = "CIRCLE"
win7.write(b"[FROM GAMEPAD] -> CIRCLE\r\n")
win7.write(b"[TO LYNXMOTION] -> Dispatching Synchronized Tuck Macro:\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = TUCK_TARGET[joint]
macro_packet += f"#{joint}P{TUCK_TARGET[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
# SELECT/RESET (Memory Lock for Ready State)
elif "SELECT" in cmd_upper:
if last_processed_command != "SELECT":
last_processed_command = "SELECT"
dynamic_ready_positions = list(current_arm_positions)
is_ready_state_set = True
win7.write(b"==================================================\r\n")
win7.write(b"[MEMORY LOCK] Dynamic READY Position Established!\r\n")
win7.write(f"Captured Hardware Footprint: {dynamic_ready_positions}\r\n".encode('utf-8'))
win7.write(b"==================================================\r\n\r\n")
cokoino.write(b"LED:SET:SUCCESS\n")
# L1 BUTTON: COORDINATED LINEAR THRUST
elif "TOOL ADVANCE" in cmd_upper:
if last_processed_command != "ADVANCE":
last_processed_command = "ADVANCE"
win7.write(b"[FROM GAMEPAD] -> L1: TOOL ADVANCE DETECTED\r\n")
if not is_ready_state_set:
win7.write(b"[ABORT] Cannot thrust. Operator must press SELECT first to lock baseline.\r\n\r\n")
continue
try:
win7.write(b"[INVERSE KINEMATICS] Computing XML-Driven 3cm Linear Path...\r\n")
physical_p1 = dynamic_ready_positions[1]
virtual_p1 = CENTER_PULSE - (physical_p1 - CENTER_PULSE) if JOINT_INVERT_MAP.get(1, False) else physical_p1
th1_start = pulse_to_radians(virtual_p1)
th2_start = pulse_to_radians(dynamic_ready_positions[2])
x_start = L1 * math.cos(th1_start) + L2 * math.cos(th1_start + th2_start)
z_start = L1 * math.sin(th1_start) + L2 * math.sin(th1_start + th2_start)
win7.write(f" Calculated Start Plane: X={x_start:.1f}mm, Z={z_start:.1f}mm\r\n".encode('utf-8'))
win7.write(b" Executing Damped Incremental Forward Push:\r\n")
for step in [10.0, 20.0, 30.0]:
target_x = x_start + step
target_z = z_start
p1, p2, p3 = solve_ik(target_x, target_z)
current_arm_positions[1] = p1
current_arm_positions[2] = p2
current_arm_positions[3] = p3
packet = f"#1P{p1}#2P{p2}#3P{p3}T1000\r"
lynx.write(packet.encode('utf-8'))
win7.write(f" Segment {step/10:.0f}cm -> {packet.strip()} [\\r]\r\n".encode('utf-8'))
time.sleep(1.0)
win7.write(b"[THRUST COMPLETE]\r\n\r\n")
except Exception as math_err:
win7.write(f"[MATH ERROR] Trajectory boundary reached: {math_err}\r\n\r\n".encode('utf-8'))
# L2 BUTTON: RETRACT TO READY
elif "TOOL RETRACT" in cmd_upper:
if last_processed_command != "RETRACT":
last_processed_command = "RETRACT"
win7.write(b"[FROM GAMEPAD] -> L2: TOOL RETRACT DETECTED\r\n")
win7.write(b"[TO LYNXMOTION] -> Retracting Linear Tool Head back to Ready Matrix:\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = dynamic_ready_positions[joint]
macro_packet += f"#{joint}P{dynamic_ready_positions[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
time.sleep(0.01)
except KeyboardInterrupt:
print("\nBridge safely terminated.")
except Exception as e:
print(f"\nBridge Error: {e}")(th)
Offline
Like button can go here
This post contains Version 35 of a Python program to integrate a Cokoino controller with a LynxMotion robot arm. This version addresses the math problem by concentrating on only one arm and attempting to move the tip of arm 1 exactly 3 centimeters along the arc of the circle described by the tip moving as the arm pivots around it's axis.
# bridgeV35.py Prepared by Gemini Supervised by Tom Hanson
# Staged Kinematics Isolation: Phase 1 (Single Joint Forward Arc Expansion)
import serial
import serial.tools.list_ports
import time
import math
import xml.etree.ElementTree as ET
print("==================================================")
print("Initializing Robot Junction Bridge V35...")
print("XML Hardware Abstraction Layer (HAL) Integrated")
print("Staged Kinematics Phase 1: Arm 1 Arc Move Active")
print("==================================================\n")
# ==============================================================================
# STEP 0: UNIFIED ROBOT DESCRIPTION FORMAT (URDF) - XML PARAMETERS
# ==============================================================================
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>
<pulses_per_degree>11.11</pulses_per_degree>
</controller_settings>
<link name="link_1_shoulder">
<length_mm>147.0</length_mm>
<mass_g>PLACEHOLDER</mass_g>
</link>
<link name="link_2_elbow">
<length_mm>187.5</length_mm>
<mass_g>PLACEHOLDER</mass_g>
</link>
<link name="link_3_tool_head">
<length_mm>0.0</length_mm> </link>
<joint channel="0" name="Base">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>Z_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="1" name="Shoulder">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>true</invert_direction>
<axis_orientation>Y_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="2" name="Elbow">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>Y_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="3" name="Wrist">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>Y_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="4" name="Gripper">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>IMMUTABLE_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="5" name="Wrist Rot">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>X_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
</robot>
"""
# --- DYNAMIC XML PARSER AND HAL ROUTINES ---
root = ET.fromstring(urdf_configuration)
# 1. Parse Controller Constants
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)
P_PER_DEG = float(root.find(".//pulses_per_degree").text)
# 2. Parse Physical Link Lengths dynamically into the Kinematics variables
L1 = float(root.find(".//link[@name='link_1_shoulder']/length_mm").text)
L2 = float(root.find(".//link[@name='link_2_elbow']/length_mm").text)
def constraint_safety_clip(pulse):
"""Enforces strict software limits dynamically parsed from XML."""
return max(MIN_PULSE, min(MAX_PULSE, pulse))
# --- STEP 1: SCAN & FILTER FOR PHYSICAL USB DEVICES ---
all_found_ports = serial.tools.list_ports.comports()
ports = []
for p in all_found_ports:
if "USB" in p.device.upper():
ports.append(p)
ports = sorted(ports, key=lambda x: x.device)
if len(ports) < 3:
print(f"[ERROR] Found only {len(ports)} physical USB devices.")
exit(1)
print("Detected Physical USB Serial Devices:")
for idx, port in enumerate(ports):
print(f" [{idx}] {port.device} - {port.description}")
print("")
# --- STEP 2: INTERACTIVE MENU WITH AUTO-DEDUCTION ---
all_indices = {0, 1, 2}
while True:
try:
win7_idx = int(input("Enter index number for WINDOWS 7 (HyperTrm): "))
if win7_idx not in all_indices: continue
cokoino_idx = int(input("Enter index number for COKOINO (Arduino): "))
if cokoino_idx not in all_indices: continue
if 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")
# --- STEP 3: ROBOT CONTEXT QUESTIONNAIRE ---
current_arm_positions = [CENTER_PULSE] * 6
HOME_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 500, 500, 1500]
dynamic_ready_positions = list(HOME_TARGET)
is_ready_state_set = False
print("\n==================================================")
print(" ROBOT ARM POSITION INITIALIZATION ")
print("==================================================")
print("Where is the physical arm positioned right now?")
print(" [T] Secured in TUCK position (Taped Plate Numbers)")
print(" [H] Standing upright at HOME position (All 1500s)")
print(" [O] OTHER (Manual eyeball estimates)")
print("--------------------------------------------------")
while True:
choice = input("Enter alignment choice (T/H/O): ").strip().upper()
if choice == 'T':
current_arm_positions = list(TUCK_TARGET)
print("\n[SUCCESS] Loaded verified TUCK coordinates into memory.")
break
elif choice == 'H':
current_arm_positions = list(HOME_TARGET)
print("\n[SUCCESS] Loaded standard HOME coordinates into memory.")
break
elif choice == 'O':
print("\nEnter custom pulse width estimates (500-2500):")
names = ["0 (Base)", "1 (Shoulder)", "2 (Elbow)", "3 (Wrist)", "4 (Gripper)", "5 (Wrist Rot)"]
for i in range(6):
while True:
try:
val = int(input(f" Servo {names[i]} position [Default 1500]: ") or 1500)
if MIN_PULSE <= val <= MAX_PULSE:
current_arm_positions[i] = val
break
print(f" [OUT OF BOUNDS] Keep value between {MIN_PULSE} and {MAX_PULSE}.")
except ValueError:
print(" [INVALID] Please enter an integer number.")
break
print(f"\nFinalized Startup Vector: {current_arm_positions}\n")
# --- STEP 4: RUNTIME ENGAGEMENT ---
BAUD_RATE = 9600
TRANSIT_TIME_MS = 3000
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")
system_state = 0
binary_counter = 0
win7.write(b"==================================================\r\n")
win7.write(b"--- SYSTEM BOOT: LOCKOUT ACTIVE (SAFE MODE) ---\r\n")
win7.write(b"--- Physical Cokoino NeoPixels: Looping Red ---\r\n")
win7.write(b"==================================================\r\n\r\n")
last_processed_command = ""
while True:
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)
if cokoino.in_waiting > 0:
data = cokoino.readline()
cmd = data.decode('utf-8', errors='ignore').strip()
if cmd and not cmd.startswith("LED:"):
cmd_upper = cmd.upper()
if cmd != last_processed_command:
last_processed_command = ""
if "RELEASE" in cmd_upper:
last_processed_command = ""
continue
if system_state == 0:
if "START" in cmd_upper:
system_state = 1
cokoino.write(b"LED:STATE:GREEN\n")
win7.write(b"[!!!] START DETECTED [!!!]\r\n")
win7.write(b"--- COKOINO READY FOR PS/2 COMMANDS ---\r\n\r\n")
elif system_state == 1:
# CROSS (Diagnostics)
if "CROSS" in cmd_upper:
if last_processed_command != "CROSS":
last_processed_command = "CROSS"
win7.write(b"[FROM GAMEPAD] -> CROSS\r\n")
win7.write(b"[TO LYNXMOTION] -> Exact String: 'VER\\r'\r\n")
lynx.reset_input_buffer()
lynx.write(b"VER\r")
time.sleep(0.1)
if lynx.in_waiting > 0:
response = lynx.readline().decode('utf-8', errors='ignore').strip()
if response:
win7.write(f"==> LYNXMOTION RESPONSE: '{response}'\r\n\r\n".encode('utf-8'))
cokoino.write(b"LED:VER:SUCCESS\n")
else:
cokoino.write(b"LED:VER:FAULT\n")
else:
cokoino.write(b"LED:VER:FAULT\n")
# TRIANGLE (Home Macro)
elif "TRIANGLE" in cmd_upper:
if last_processed_command != "TRIANGLE":
last_processed_command = "TRIANGLE"
win7.write(b"[FROM GAMEPAD] -> TRIANGLE\r\n")
win7.write(b"[TO LYNXMOTION] -> Dispatching Synchronized Home Macro:\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = HOME_TARGET[joint]
macro_packet += f"#{joint}P{HOME_TARGET[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
# CIRCLE (Tuck Macro)
elif "CIRCLE" in cmd_upper:
if last_processed_command != "CIRCLE":
last_processed_command = "CIRCLE"
win7.write(b"[FROM GAMEPAD] -> CIRCLE\r\n")
win7.write(b"[TO LYNXMOTION] -> Dispatching Synchronized Tuck Macro:\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = TUCK_TARGET[joint]
macro_packet += f"#{joint}P{TUCK_TARGET[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
# SELECT/RESET (Memory Lock for Ready State)
elif "SELECT" in cmd_upper:
if last_processed_command != "SELECT":
last_processed_command = "SELECT"
dynamic_ready_positions = list(current_arm_positions)
is_ready_state_set = True
win7.write(b"==================================================\r\n")
win7.write(b"[MEMORY LOCK] Dynamic READY Position Established!\r\n")
win7.write(f"Captured Hardware Footprint: {dynamic_ready_positions}\r\n".encode('utf-8'))
win7.write(b"==================================================\r\n\r\n")
cokoino.write(b"LED:SET:SUCCESS\n")
# L1 BUTTON: STAGED ISOLATION - SINGLE-JOINT ARC ADVANCE
elif "TOOL ADVANCE" in cmd_upper:
if last_processed_command != "ADVANCE":
last_processed_command = "ADVANCE"
win7.write(b"[FROM GAMEPAD] -> L1: TOOL ADVANCE DETECTED\r\n")
if not is_ready_state_set:
win7.write(b"[ABORT] Operator must press SELECT first to lock baseline.\r\n\r\n")
continue
win7.write(b"[KINEMATICS] Executing Staged Phase 1: Outward Arm 1 Arc Move...\r\n")
# 1. Fetch locked physical baseline values
baseline_p1 = dynamic_ready_positions[1]
baseline_p2 = dynamic_ready_positions[2]
baseline_p3 = dynamic_ready_positions[3]
# 2. Calculate pulse delta for a 30mm outward arc travel along link radius
arc_target_mm = 30.0
delta_radians = arc_target_mm / L1
delta_degrees = math.degrees(delta_radians)
pulse_delta = int(round(delta_degrees * P_PER_DEG))
win7.write(f" Arc Radius (L1): {L1}mm, Target: {arc_target_mm}mm\r\n".encode('utf-8'))
win7.write(f" Calculated Absolute Delta: {pulse_delta} pulses\r\n".encode('utf-8'))
# 3. Step forward in 3 distinct 10mm increments
for step in [1, 2, 3]:
fractional_delta = int(round((pulse_delta / 3.0) * step))
# PHYSICAL GROUND TRUTH: Outward movement drops pulse width below baseline
target_p1 = constraint_safety_clip(baseline_p1 - fractional_delta)
current_arm_positions[1] = target_p1
current_arm_positions[2] = baseline_p2
current_arm_positions[3] = baseline_p3
packet = f"#1P{target_p1}#2P{baseline_p2}#3P{baseline_p3}T1000\r"
lynx.write(packet.encode('utf-8'))
win7.write(f" Segment {step}cm -> {packet.strip()} [\\r]\r\n".encode('utf-8'))
time.sleep(1.0)
win7.write(b"[ARC MOVE COMPLETE] Arm 1 reached outward target safely.\r\n\r\n")
# L2 BUTTON: SIMPLIFIED ABSOLUTE RETURN TO READY
elif "TOOL RETRACT" in cmd_upper:
if last_processed_command != "RETRACT":
last_processed_command = "RETRACT"
win7.write(b"[FROM GAMEPAD] -> L2: TOOL RETRACT DETECTED\r\n")
if not is_ready_state_set:
win7.write(b"[ABORT] No locked footprint. Cannot snap back.\r\n\r\n")
continue
win7.write(b"[SAFETY RESET] Snapping absolute tool position back to Ready matrix...\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = dynamic_ready_positions[joint]
macro_packet += f"#{joint}P{dynamic_ready_positions[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
time.sleep(0.01)
except KeyboardInterrupt:
print("\nBridge safely terminated.")
except Exception as e:
print(f"\nBridge Error: {e}")(th)
Offline
Like button can go here
This post contains Version 36 of a series to integrate Cokoino with LynxMotion.
Here we attempt to added Arm 2 to the calculation of a coordinated movement.
# bridgeV36.py Prepared by Gemini Supervised by Tom Hanson
# Staged Kinematics Isolation: Phase 2 (Coordinated Two-Link Horizontal Line Push)
import serial
import serial.tools.list_ports
import time
import math
import xml.etree.ElementTree as ET
print("==================================================")
print("Initializing Robot Junction Bridge V36...")
print("XML Hardware Abstraction Layer (HAL) Integrated")
print("Staged Kinematics Phase 2: 5cm Flat Line Push Active")
print("==================================================\n")
# ==============================================================================
# STEP 0: UNIFIED ROBOT DESCRIPTION FORMAT (URDF) - XML PARAMETERS
# ==============================================================================
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>
<pulses_per_degree>11.11</pulses_per_degree>
</controller_settings>
<link name="link_1_shoulder">
<length_mm>147.0</length_mm>
<mass_g>PLACEHOLDER</mass_g>
</link>
<link name="link_2_elbow">
<length_mm>187.5</length_mm>
<mass_g>PLACEHOLDER</mass_g>
</link>
<link name="link_3_tool_head">
<length_mm>0.0</length_mm> </link>
<joint channel="0" name="Base">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>Z_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="1" name="Shoulder">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>true</invert_direction>
<axis_orientation>Y_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="2" name="Elbow">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>Y_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="3" name="Wrist">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>Y_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="4" name="Gripper">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>IMMUTABLE_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
<joint channel="5" name="Wrist Rot">
<angular_sweep_deg>180</angular_sweep_deg>
<invert_direction>false</invert_direction>
<axis_orientation>X_AXIS</axis_orientation>
<velocity_limit_deg_sec>PLACEHOLDER</velocity_limit_deg_sec>
</joint>
</robot>
"""
# --- DYNAMIC XML PARSER AND HAL ROUTINES ---
root = ET.fromstring(urdf_configuration)
# 1. Parse Controller Constants
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)
P_PER_DEG = float(root.find(".//pulses_per_degree").text)
# 2. Parse Physical Link Lengths dynamically into the Kinematics variables
L1 = float(root.find(".//link[@name='link_1_shoulder']/length_mm").text)
L2 = float(root.find(".//link[@name='link_2_elbow']/length_mm").text)
def constraint_safety_clip(pulse):
"""Enforces strict software limits dynamically parsed from XML."""
return max(MIN_PULSE, min(MAX_PULSE, pulse))
# --- STEP 1: SCAN & FILTER FOR PHYSICAL USB DEVICES ---
all_found_ports = serial.tools.list_ports.comports()
ports = []
for p in all_found_ports:
if "USB" in p.device.upper():
ports.append(p)
ports = sorted(ports, key=lambda x: x.device)
if len(ports) < 3:
print(f"[ERROR] Found only {len(ports)} physical USB devices.")
exit(1)
print("Detected Physical USB Serial Devices:")
for idx, port in enumerate(ports):
print(f" [{idx}] {port.device} - {port.description}")
print("")
# --- STEP 2: INTERACTIVE MENU WITH AUTO-DEDUCTION ---
all_indices = {0, 1, 2}
while True:
try:
win7_idx = int(input("Enter index number for WINDOWS 7 (HyperTrm): "))
if win7_idx not in all_indices: continue
cokoino_idx = int(input("Enter index number for COKOINO (Arduino): "))
if cokoino_idx not in all_indices: continue
if 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")
# --- STEP 3: ROBOT CONTEXT QUESTIONNAIRE ---
current_arm_positions = [CENTER_PULSE] * 6
HOME_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 500, 500, 1500]
dynamic_ready_positions = list(HOME_TARGET)
is_ready_state_set = False
print("\n==================================================")
print(" ROBOT ARM POSITION INITIALIZATION ")
print("==================================================")
print("Where is the physical arm positioned right now?")
print(" [T] Secured in TUCK position (Taped Plate Numbers)")
print(" [H] Standing upright at HOME position (All 1500s)")
print(" [O] OTHER (Manual eyeball estimates)")
print("--------------------------------------------------")
while True:
choice = input("Enter alignment choice (T/H/O): ").strip().upper()
if choice == 'T':
current_arm_positions = list(TUCK_TARGET)
print("\n[SUCCESS] Loaded verified TUCK coordinates into memory.")
break
elif choice == 'H':
current_arm_positions = list(HOME_TARGET)
print("\n[SUCCESS] Loaded standard HOME coordinates into memory.")
break
elif choice == 'O':
print("\nEnter custom pulse width estimates (500-2500):")
names = ["0 (Base)", "1 (Shoulder)", "2 (Elbow)", "3 (Wrist)", "4 (Gripper)", "5 (Wrist Rot)"]
for i in range(6):
while True:
try:
val = int(input(f" Servo {names[i]} position [Default 1500]: ") or 1500)
if MIN_PULSE <= val <= MAX_PULSE:
current_arm_positions[i] = val
break
print(f" [OUT OF BOUNDS] Keep value between {MIN_PULSE} and {MAX_PULSE}.")
except ValueError:
print(" [INVALID] Please enter an integer number.")
break
print(f"\nFinalized Startup Vector: {current_arm_positions}\n")
# --- STEP 4: RUNTIME ENGAGEMENT ---
BAUD_RATE = 9600
TRANSIT_TIME_MS = 3000
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")
system_state = 0
binary_counter = 0
win7.write(b"==================================================\r\n")
win7.write(b"--- SYSTEM BOOT: LOCKOUT ACTIVE (SAFE MODE) ---\r\n")
win7.write(b"--- Physical Cokoino NeoPixels: Looping Red ---\r\n")
win7.write(b"==================================================\r\n\r\n")
last_processed_command = ""
while True:
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)
if cokoino.in_waiting > 0:
data = cokoino.readline()
cmd = data.decode('utf-8', errors='ignore').strip()
if cmd and not cmd.startswith("LED:"):
cmd_upper = cmd.upper()
if cmd != last_processed_command:
last_processed_command = ""
if "RELEASE" in cmd_upper:
last_processed_command = ""
continue
if system_state == 0:
if "START" in cmd_upper:
system_state = 1
cokoino.write(b"LED:STATE:GREEN\n")
win7.write(b"[!!!] START DETECTED [!!!]\r\n")
win7.write(b"--- COKOINO READY FOR PS/2 COMMANDS ---\r\n\r\n")
elif system_state == 1:
# CROSS (Diagnostics)
if "CROSS" in cmd_upper:
if last_processed_command != "CROSS":
last_processed_command = "CROSS"
win7.write(b"[FROM GAMEPAD] -> CROSS\r\n")
win7.write(b"[TO LYNXMOTION] -> Exact String: 'VER\\r'\r\n")
lynx.reset_input_buffer()
lynx.write(b"VER\r")
time.sleep(0.1)
if lynx.in_waiting > 0:
response = lynx.readline().decode('utf-8', errors='ignore').strip()
if response:
win7.write(f"==> LYNXMOTION RESPONSE: '{response}'\r\n\r\n".encode('utf-8'))
cokoino.write(b"LED:VER:SUCCESS\n")
else:
cokoino.write(b"LED:VER:FAULT\n")
else:
cokoino.write(b"LED:VER:FAULT\n")
# TRIANGLE (Home Macro)
elif "TRIANGLE" in cmd_upper:
if last_processed_command != "TRIANGLE":
last_processed_command = "TRIANGLE"
win7.write(b"[FROM GAMEPAD] -> TRIANGLE\r\n")
win7.write(b"[TO LYNXMOTION] -> Dispatching Synchronized Home Macro:\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = HOME_TARGET[joint]
macro_packet += f"#{joint}P{HOME_TARGET[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
# CIRCLE (Tuck Macro)
elif "CIRCLE" in cmd_upper:
if last_processed_command != "CIRCLE":
last_processed_command = "CIRCLE"
win7.write(b"[FROM GAMEPAD] -> CIRCLE\r\n")
win7.write(b"[TO LYNXMOTION] -> Dispatching Synchronized Tuck Macro:\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = TUCK_TARGET[joint]
macro_packet += f"#{joint}P{TUCK_TARGET[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
# SELECT/RESET (Memory Lock for Ready State)
elif "SELECT" in cmd_upper:
if last_processed_command != "SELECT":
last_processed_command = "SELECT"
dynamic_ready_positions = list(current_arm_positions)
is_ready_state_set = True
win7.write(b"==================================================\r\n")
win7.write(b"[MEMORY LOCK] Dynamic READY Position Established!\r\n")
win7.write(f"Captured Hardware Footprint: {dynamic_ready_positions}\r\n".encode('utf-8'))
win7.write(b"==================================================\r\n\r\n")
cokoino.write(b"LED:SET:SUCCESS\n")
# L1 BUTTON: COORDINATED TWO-LINK HORIZONTAL 50MM PUSH
elif "TOOL ADVANCE" in cmd_upper:
if last_processed_command != "ADVANCE":
last_processed_command = "ADVANCE"
win7.write(b"[FROM GAMEPAD] -> L1: TOOL ADVANCE DETECTED\r\n")
if not is_ready_state_set:
win7.write(b"[ABORT] Operator must press SELECT first to lock baseline.\r\n\r\n")
continue
win7.write(b"[KINEMATICS] Initiating Coordinated 50mm Horizontal Push...\r\n")
# 1. Fetch locked baseline pulse values
b_p1 = dynamic_ready_positions[1]
b_p2 = dynamic_ready_positions[2]
# 2. Convert baseline pulses to physical angles (in Radians)
# Joint 1: 1500 is straight up (90 deg). Decreasing pulse moves it forward.
deg1 = 90.0 + ((1500 - b_p1) / P_PER_DEG)
rad1 = math.radians(deg1)
# Joint 2: 1500 is perpendicular to Arm 1 (90 deg offset)
# Relative angle to Arm 1 changes with pulse. Increasing pulse moves it outward/down.
deg2 = 90.0 - ((b_p2 - 1500) / P_PER_DEG)
rad2 = math.radians(deg2)
# 3. Calculate current starting coordinates (Forward Kinematics)
# Elbow angle relative to horizon is (rad1 + rad2 - pi) due to mechanical layout
angle_link2 = rad1 + rad2 - math.pi
x_start = L1 * math.cos(rad1) + L2 * math.cos(angle_link2)
z_start = L1 * math.sin(rad1) + L2 * math.sin(angle_link2)
# Lock Z height for a perfectly flat horizontal path
z_target = z_start
# 4. Generate the 5-Step Predictive Matrix for the Win 7 Monitor
win7.write(f" Initial Position: X = {x_start:.1f}mm, Z = {z_start:.1f}mm (Locked)\r\n".encode('utf-8'))
win7.write(b" -------------------------------------------------------------\r\n")
win7.write(b" SEGMENT TARGET X TARGET Z SHOULDER (P1) ELBOW (P2)\r\n")
win7.write(b" -------------------------------------------------------------\r\n")
steps_pulses = []
valid_path = True
for step in [1, 2, 3, 4, 5]:
x_target = x_start + (step * 10.0) # Move forward 10mm per segment
# Inverse Kinematics (Law of Cosines) to find required joint angles
try:
# Distance from origin to target
D_sq = x_target**2 + z_target**2
# Solve for Elbow Angle
cos_theta2 = (D_sq - L1**2 - L2**2) / (2.0 * L1 * L2)
theta2 = math.acos(cos_theta2)
# Solve for Shoulder Angle
theta1 = math.atan2(z_target, x_target) - math.acos((L1**2 + D_sq - L2**2) / (2.0 * L1 * math.sqrt(D_sq)))
# Convert geometry angles back to hardware pulses
target_deg1 = math.degrees(theta1)
target_deg2 = math.degrees(theta2)
# Map angles back to specific physical directions
t_p1 = int(round(1500 - ((target_deg1 - 90.0) * P_PER_DEG)))
t_p2 = int(round(1500 + ((90.0 - target_deg2) * P_PER_DEG)))
# Clip to safety boundaries
t_p1 = constraint_safety_clip(t_p1)
t_p2 = constraint_safety_clip(t_p2)
steps_pulses.append((step, x_target, t_p1, t_p2))
win7.write(f" {step} cm {x_target:.1f}mm {z_target:.1f}mm {t_p1} {t_p2}\r\n".encode('utf-8'))
except ValueError:
win7.write(f" {step} cm [MATH ERROR: TARGET OUT OF PHYSICAL REACH]\r\n".encode('utf-8'))
valid_path = False
break
win7.write(b" -------------------------------------------------------------\r\n")
if not valid_path:
win7.write(b"[ABORT] Motion trajectory contains unreachable points.\r\n\r\n")
continue
# 5. Execute calculated path sequentially if all points are valid
win7.write(b"Executing coordinated line movement...\r\n")
for step, x_t, t_p1, t_p2 in steps_pulses:
# Apply wrist leveling logic based on Elbow delta
el_delta = t_p2 - 1500
t_p3 = constraint_safety_clip(1500 + el_delta)
current_arm_positions[1] = t_p1
current_arm_positions[2] = t_p2
current_arm_positions[3] = t_p3
# Dispatch coordinated multi-axis packet to LynxMotion
packet = f"#1P{t_p1}#2P{t_p2}#3P{t_p3}T1000\r"
lynx.write(packet.encode('utf-8'))
win7.write(f" Moving to Segment {step}cm -> {packet.strip()} [\\r]\r\n".encode('utf-8'))
time.sleep(1.0)
win7.write(b"[LINE PUSH COMPLETE] Coordinated horizontal thrust verified.\r\n\r\n")
# L2 BUTTON: SIMPLIFIED ABSOLUTE SNAP BACK TO READY
elif "TOOL RETRACT" in cmd_upper:
if last_processed_command != "RETRACT":
last_processed_command = "RETRACT"
win7.write(b"[FROM GAMEPAD] -> L2: TOOL RETRACT DETECTED\r\n")
if not is_ready_state_set:
win7.write(b"[ABORT] No locked footprint. Cannot snap back.\r\n\r\n")
continue
win7.write(b"[SAFETY RESET] Snapping absolute tool position back to Ready matrix...\r\n")
macro_packet = ""
for joint in range(6):
current_arm_positions[joint] = dynamic_ready_positions[joint]
macro_packet += f"#{joint}P{dynamic_ready_positions[joint]}"
macro_packet += f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
time.sleep(0.01)
except KeyboardInterrupt:
print("\nBridge safely terminated.")
except Exception as e:
print(f"\nBridge Error: {e}")(th)
Offline
Like button can go here
This post contains Version 37 ... we are working on the problem of how to adjust the angle of three robot arms in order to deliver a movement in X that does not vary in Y. When the pivot of Arm 1 turns, the pivot for Arm 2 and Wrist 1 must change simultaneously.
Update: Version 37 came very close ... a sign was set incorrectly so the arm #2 went up instead of down. Version 38 will have the sign reversed.
# bridgeV37.py Prepared by Gemini Supervised by Tom Hanson
# Staged Kinematics Isolation: Phase 2 (Proportional Linear Equalization)
import serial
import serial.tools.list_ports
import time
import math
import xml.etree.ElementTree as ET
print("==================================================")
print("Initializing Robot Junction Bridge V37...")
print("XML Hardware Abstraction Layer (HAL) Integrated")
print("Proportional Coordinated 5cm Horizontal Push")
print("==================================================\n")
# ==============================================================================
# STEP 0: UNIFIED ROBOT DESCRIPTION FORMAT (URDF) - XML PARAMETERS
# ==============================================================================
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>
<pulses_per_degree>11.11</pulses_per_degree>
</controller_settings>
<link name="link_1_shoulder">
<length_mm>147.0</length_mm>
</link>
<link name="link_2_elbow">
<length_mm>187.5</length_mm>
</link>
<link name="link_3_tool_head">
<length_mm>0.0</length_mm>
</link>
<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>
"""
# --- DYNAMIC XML PARSER ---
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)
P_PER_DEG = float(root.find(".//pulses_per_degree").text)
L1 = float(root.find(".//link[@name='link_1_shoulder']/length_mm").text)
def constraint_safety_clip(pulse):
return max(MIN_PULSE, min(MAX_PULSE, pulse))
# --- STEP 1: SCAN & FILTER FOR PHYSICAL USB DEVICES ---
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)
print("Detected Physical USB Serial Devices:")
for idx, port in enumerate(ports):
print(f" [{idx}] {port.device} - {port.description}")
print("")
# --- STEP 2: INTERACTIVE MENU ---
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")
# --- STEP 3: ROBOT CONTEXT QUESTIONNAIRE ---
current_arm_positions = [CENTER_PULSE] * 6
HOME_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 500, 500, 1500]
dynamic_ready_positions = list(HOME_TARGET)
is_ready_state_set = False
print("\n==================================================")
print(" ROBOT ARM POSITION INITIALIZATION ")
print("==================================================")
print(" [T] Secured in TUCK position")
print(" [H] Standing upright at HOME position")
print(" [O] OTHER (Manual estimates)")
print("--------------------------------------------------")
while True:
choice = input("Enter alignment choice (T/H/O): ").strip().upper()
if choice == 'T':
current_arm_positions = list(TUCK_TARGET)
break
elif choice == 'H':
current_arm_positions = list(HOME_TARGET)
break
elif choice == 'O':
break
# --- STEP 4: RUNTIME ENGAGEMENT ---
BAUD_RATE = 9600
TRANSIT_TIME_MS = 3000
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")
system_state = 0
binary_counter = 0
win7.write(b"==================================================\r\n")
win7.write(b"--- SYSTEM BOOT: LOCKOUT ACTIVE (SAFE MODE) ---\r\n")
win7.write(b"==================================================\r\n\r\n")
last_processed_command = ""
while True:
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)
if cokoino.in_waiting > 0:
data = cokoino.readline()
cmd = data.decode('utf-8', errors='ignore').strip()
if cmd and not cmd.startswith("LED:"):
cmd_upper = cmd.upper()
if cmd != last_processed_command:
last_processed_command = ""
if "RELEASE" in cmd_upper:
last_processed_command = ""
continue
if system_state == 0:
if "START" in cmd_upper:
system_state = 1
cokoino.write(b"LED:STATE:GREEN\n")
win7.write(b"[!!!] START DETECTED [!!!]\r\n\r\n")
elif system_state == 1:
if "CROSS" in cmd_upper:
if last_processed_command != "CROSS":
last_processed_command = "CROSS"
win7.write(b"[FROM GAMEPAD] -> CROSS\r\n")
lynx.write(b"VER\r")
time.sleep(0.1)
if lynx.in_waiting > 0:
response = lynx.readline().decode('utf-8', errors='ignore').strip()
win7.write(f"==> LYNXMOTION RESPONSE: '{response}'\r\n\r\n".encode('utf-8'))
elif "TRIANGLE" in cmd_upper:
if last_processed_command != "TRIANGLE":
last_processed_command = "TRIANGLE"
win7.write(b"[FROM GAMEPAD] -> TRIANGLE\r\n")
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" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
elif "CIRCLE" in cmd_upper:
if last_processed_command != "CIRCLE":
last_processed_command = "CIRCLE"
win7.write(b"[FROM GAMEPAD] -> CIRCLE\r\n")
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" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
elif "SELECT" in cmd_upper:
if last_processed_command != "SELECT":
last_processed_command = "SELECT"
dynamic_ready_positions = list(current_arm_positions)
is_ready_state_set = True
win7.write(b"==================================================\r\n")
win7.write(b"[MEMORY LOCK] Dynamic READY Position Established!\r\n")
win7.write(f"Captured Hardware Footprint: {dynamic_ready_positions}\r\n".encode('utf-8'))
win7.write(b"==================================================\r\n\r\n")
# L1 BUTTON: PROPORTIONAL LINEAR EQUALIZATION 5CM PUSH
elif "TOOL ADVANCE" in cmd_upper:
if last_processed_command != "ADVANCE":
last_processed_command = "ADVANCE"
win7.write(b"[FROM GAMEPAD] -> L1: TOOL ADVANCE DETECTED\r\n")
if not is_ready_state_set:
win7.write(b"[ABORT] Operator must press SELECT first to lock baseline.\r\n\r\n")
continue
win7.write(b"[KINEMATICS] Executing Slope-Aware Proportional 5cm Line Push...\r\n")
baseline_p1 = dynamic_ready_positions[1]
baseline_p2 = dynamic_ready_positions[2]
# 1. Base calculations on your confirmed 30mm/130 pulse ground truth
# Scaled up to a 50mm forward run = 217 total pulses for Joint 1
total_sh_pulses = 217
# 2. Determine compensation slope for Joint 2 based on your starting footprint
# If starting at Home (1500), it's flat; slope is 0.
# If starting at Tuck (1842), it's steep; it needs to open outward to cancel rise.
if baseline_p1 > 1600:
# Tuck Profile: Joint 2 tracks downward proportionally with Joint 1
total_el_pulses = -215
else:
# Home Profile: Flat top of arc, minimal elbow adjustments required
total_el_pulses = 45
win7.write(b" -------------------------------------------------------------\r\n")
win7.write(b" SEGMENT SHOULDER (P1) ELBOW (P2) WRIST (P3) (LEVEL)\r\n")
win7.write(b" -------------------------------------------------------------\r\n")
# 3. Process and stream the 5 distinct 1cm steps
for step in [1, 2, 3, 4, 5]:
fraction = step / 5.0
# Apply the safe physical outward decrement to Joint 1
target_p1 = constraint_safety_clip(int(round(baseline_p1 - (total_sh_pulses * fraction))))
# Apply the mapped slope adjustment to Joint 2
target_p2 = constraint_safety_clip(int(round(baseline_p2 + (total_el_pulses * fraction))))
# Apply your equal-and-opposite wrist leveling logic
el_delta = target_p2 - baseline_p2
target_p3 = constraint_safety_clip(dynamic_ready_positions[3] - el_delta)
current_arm_positions[1] = target_p1
current_arm_positions[2] = target_p2
current_arm_positions[3] = target_p3
packet = f"#1P{target_p1}#2P{target_p2}#3P{target_p3}T1000\r"
lynx.write(packet.encode('utf-8'))
win7.write(f" {step} cm {target_p1} {target_p2} {target_p3} -> {packet.strip()} [\\r]\r\n".encode('utf-8'))
time.sleep(1.0)
win7.write(b"[PUSH COMPLETE] Target line reached cleanly.\r\n\r\n")
# L2 BUTTON: SNAP DIRECTLY BACK TO READY
elif "TOOL RETRACT" in cmd_upper:
if last_processed_command != "RETRACT":
last_processed_command = "RETRACT"
win7.write(b"[FROM GAMEPAD] -> L2: TOOL RETRACT DETECTED\r\n")
if not is_ready_state_set:
win7.write(b"[ABORT] No footprint locked.\r\n\r\n")
continue
win7.write(b"[RESET] Snapping back to Ready matrix...\r\n")
macro_packet = "".join(f"#{j}P{dynamic_ready_positions[j]}" for j in range(6)) + f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
time.sleep(0.01)
except KeyboardInterrupt:
print("\nBridge safely terminated.")
except Exception as e:
print(f"\nBridge Error: {e}")(th)
Offline
Like button can go here
This is Version 38 with a sign reversed from Version 37...
# bridgeV38.py Prepared by Gemini Supervised by Tom Hanson
# Staged Kinematics Isolation: Phase 2 (Proportional Linear Equalization Fixed)
import serial
import serial.tools.list_ports
import time
import math
import xml.etree.ElementTree as ET
print("==================================================")
print("Initializing Robot Junction Bridge V38...")
print("XML Hardware Abstraction Layer (HAL) Integrated")
print("Corrected Proportional 5cm Horizontal Push")
print("==================================================\n")
# ==============================================================================
# STEP 0: UNIFIED ROBOT DESCRIPTION FORMAT (URDF) - XML PARAMETERS
# ==============================================================================
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>
<pulses_per_degree>11.11</pulses_per_degree>
</controller_settings>
<link name="link_1_shoulder">
<length_mm>147.0</length_mm>
</link>
<link name="link_2_elbow">
<length_mm>187.5</length_mm>
</link>
<link name="link_3_tool_head">
<length_mm>0.0</length_mm>
</link>
<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>
"""
# --- DYNAMIC XML PARSER ---
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)
P_PER_DEG = float(root.find(".//pulses_per_degree").text)
L1 = float(root.find(".//link[@name='link_1_shoulder']/length_mm").text)
def constraint_safety_clip(pulse):
return max(MIN_PULSE, min(MAX_PULSE, pulse))
# --- STEP 1: SCAN & FILTER FOR PHYSICAL USB DEVICES ---
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)
print("Detected Physical USB Serial Devices:")
for idx, port in enumerate(ports):
print(f" [{idx}] {port.device} - {port.description}")
print("")
# --- STEP 2: INTERACTIVE MENU ---
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")
# --- STEP 3: ROBOT CONTEXT QUESTIONNAIRE ---
current_arm_positions = [CENTER_PULSE] * 6
HOME_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 500, 500, 1500]
dynamic_ready_positions = list(HOME_TARGET)
is_ready_state_set = False
print("\n==================================================")
print(" ROBOT ARM POSITION INITIALIZATION ")
print("==================================================")
print(" [T] Secured in TUCK position")
print(" [H] Standing upright at HOME position")
print(" [O] OTHER (Manual estimates)")
print("--------------------------------------------------")
while True:
choice = input("Enter alignment choice (T/H/O): ").strip().upper()
if choice == 'T':
current_arm_positions = list(TUCK_TARGET)
break
elif choice == 'H':
current_arm_positions = list(HOME_TARGET)
break
elif choice == 'O':
break
# --- STEP 4: RUNTIME ENGAGEMENT ---
BAUD_RATE = 9600
TRANSIT_TIME_MS = 3000
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")
system_state = 0
binary_counter = 0
win7.write(b"==================================================\r\n")
win7.write(b"--- SYSTEM BOOT: LOCKOUT ACTIVE (SAFE MODE) ---\r\n")
win7.write(b"==================================================\r\n\r\n")
last_processed_command = ""
while True:
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)
if cokoino.in_waiting > 0:
data = cokoino.readline()
cmd = data.decode('utf-8', errors='ignore').strip()
if cmd and not cmd.startswith("LED:"):
cmd_upper = cmd.upper()
if cmd != last_processed_command:
last_processed_command = ""
if "RELEASE" in cmd_upper:
last_processed_command = ""
continue
if system_state == 0:
if "START" in cmd_upper:
system_state = 1
cokoino.write(b"LED:STATE:GREEN\n")
win7.write(b"[!!!] START DETECTED [!!!]\r\n\r\n")
elif system_state == 1:
if "CROSS" in cmd_upper:
if last_processed_command != "CROSS":
last_processed_command = "CROSS"
win7.write(b"[FROM GAMEPAD] -> CROSS\r\n")
lynx.write(b"VER\r")
time.sleep(0.1)
if lynx.in_waiting > 0:
response = lynx.readline().decode('utf-8', errors='ignore').strip()
win7.write(f"==> LYNXMOTION RESPONSE: '{response}'\r\n\r\n".encode('utf-8'))
elif "TRIANGLE" in cmd_upper:
if last_processed_command != "TRIANGLE":
last_processed_command = "TRIANGLE"
win7.write(b"[FROM GAMEPAD] -> TRIANGLE\r\n")
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" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
elif "CIRCLE" in cmd_upper:
if last_processed_command != "CIRCLE":
last_processed_command = "CIRCLE"
win7.write(b"[FROM GAMEPAD] -> CIRCLE\r\n")
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" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
elif "SELECT" in cmd_upper:
if last_processed_command != "SELECT":
last_processed_command = "SELECT"
dynamic_ready_positions = list(current_arm_positions)
is_ready_state_set = True
win7.write(b"==================================================\r\n")
win7.write(b"[MEMORY LOCK] Dynamic READY Position Established!\r\n")
win7.write(f"Captured Hardware Footprint: {dynamic_ready_positions}\r\n".encode('utf-8'))
win7.write(b"==================================================\r\n\r\n")
# L1 BUTTON: PROPORTIONAL LINEAR EQUALIZATION 5CM PUSH
elif "TOOL ADVANCE" in cmd_upper:
if last_processed_command != "ADVANCE":
last_processed_command = "ADVANCE"
win7.write(b"[FROM GAMEPAD] -> L1: TOOL ADVANCE DETECTED\r\n")
if not is_ready_state_set:
win7.write(b"[ABORT] Operator must press SELECT first to lock baseline.\r\n\r\n")
continue
win7.write(b"[KINEMATICS] Executing Slope-Aware Proportional 5cm Line Push...\r\n")
baseline_p1 = dynamic_ready_positions[1]
baseline_p2 = dynamic_ready_positions[2]
# 1. Total Shoulder Pulse Delta for 50mm Run
total_sh_pulses = 217
# 2. Determine compensation direction for Joint 2 based on footprint
if baseline_p1 > 1600:
# Tuck Profile: Joint 2 must drop DOWN towards 1500 to lower arm
total_el_pulses = 215
else:
# Home Profile: Minimal adjustments from flat top
total_el_pulses = -45
win7.write(b" -------------------------------------------------------------\r\n")
win7.write(b" SEGMENT SHOULDER (P1) ELBOW (P2) WRIST (P3) (LEVEL)\r\n")
win7.write(b" -------------------------------------------------------------\r\n")
# 3. Process and stream the 5 distinct 1cm steps
for step in [1, 2, 3, 4, 5]:
fraction = step / 5.0
# Move Shoulder Forward/Outward
target_p1 = constraint_safety_clip(int(round(baseline_p1 - (total_sh_pulses * fraction))))
# SIGN FIXED: Subtract the pulse delta to drive Elbow DOWNWARD
target_p2 = constraint_safety_clip(int(round(baseline_p2 - (total_el_pulses * fraction))))
# Track Wrist leveler with the new Elbow direction
el_delta = target_p2 - baseline_p2
target_p3 = constraint_safety_clip(dynamic_ready_positions[3] - el_delta)
current_arm_positions[1] = target_p1
current_arm_positions[2] = target_p2
current_arm_positions[3] = target_p3
packet = f"#1P{target_p1}#2P{target_p2}#3P{target_p3}T1000\r"
lynx.write(packet.encode('utf-8'))
win7.write(f" {step} cm {target_p1} {target_p2} {target_p3} -> {packet.strip()} [\\r]\r\n".encode('utf-8'))
time.sleep(1.0)
win7.write(b"[PUSH COMPLETE] Target line reached cleanly.\r\n\r\n")
# L2 BUTTON: SNAP DIRECTLY BACK TO READY
elif "TOOL RETRACT" in cmd_upper:
if last_processed_command != "RETRACT":
last_processed_command = "RETRACT"
win7.write(b"[FROM GAMEPAD] -> L2: TOOL RETRACT DETECTED\r\n")
if not is_ready_state_set:
win7.write(b"[ABORT] No footprint locked.\r\n\r\n")
continue
win7.write(b"[RESET] Snapping back to Ready matrix...\r\n")
macro_packet = "".join(f"#{j}P{dynamic_ready_positions[j]}" for j in range(6)) + f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
time.sleep(0.01)
except KeyboardInterrupt:
print("\nBridge safely terminated.")
except Exception as e:
print(f"\nBridge Error: {e}")(th)
Offline
Like button can go here
This post contains Version 39 of a Python program to interface a Cokoino controller to a LynxMotion robot arm.
In this version, we are activating the Square button to hold a set of values chosen by the operator to specify a position that is to be stored and recalled by the Square button. Thus, we will have three buttons working to set configurations.
Home is set to 1500 for all servo's. This value is in the middle of the range for each servo, from 500 to 2500 fully clockwise for 180 degrees of movement.
Tuck is set to a set of values discovered through experiment to prepare the robot arm for transport to another location.
the new Square button setting is for "Other" values that are entered at program start time.
The forward movement command still needs work, but I've decided to let that rest for a while. We have 17 buttons in all to configure, and we've only worked with 8 to this point.
# bridgeV39_Final.py Prepared by Gemini Supervised by Tom Hanson
# Hard Hardware Alignment: Active Command Transmission on SQUARE Press
import serial
import serial.tools.list_ports
import time
import xml.etree.ElementTree as ET
print("==================================================")
print("Initializing Robot Junction Bridge V39...")
print("Active Execution & Hardware Alignment Engine Active")
print("==================================================\n")
# ==============================================================================
# STEP 0: UNIFIED ROBOT DESCRIPTION FORMAT (URDF) - XML PARAMETERS
# ==============================================================================
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))
# --- STEP 1: SERIAL CONNECTIONS ---
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")
# --- STEP 2: POSITION MANAGEMENT CONFIGURATION ---
current_arm_positions = [CENTER_PULSE] * 6
HOME_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 500, 500, 1500]
OTHER_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]
print("\n==================================================")
print(" ROBOT ARM CONFIGURATION MATRIX ")
print("==================================================")
print(" [T] Secured in TUCK position")
print(" [H] Standing upright at HOME position")
print(" [O] MANUALLY SPECIFY 'OTHER' VALUES (Field Override)")
print("--------------------------------------------------")
while True:
choice = input("Enter alignment choice (T/H/O): ").strip().upper()
if choice == 'T':
current_arm_positions = list(TUCK_TARGET)
break
elif choice == 'H':
current_arm_positions = list(HOME_TARGET)
break
elif choice == 'O':
print("\n--- OPERATOR FIELD SPECIFICATION INJECTION ---")
print("Please enter raw pulse widths (500 - 2500) for the current setup:")
try:
p0 = constraint_safety_clip(int(input(" Channel 0 (Base) [1500]: ") or 1500))
p1 = constraint_safety_clip(int(input(" Channel 1 (Shoulder) [1500]: ") or 1500))
p2 = constraint_safety_clip(int(input(" Channel 2 (Elbow) [1500]: ") or 1500))
p3 = constraint_safety_clip(int(input(" Channel 3 (Wrist) [1500]: ") or 1500))
p4 = constraint_safety_clip(int(input(" Channel 4 (Gripper) [1500]: ") or 1500))
p5 = constraint_safety_clip(int(input(" Channel 5 (Wrist Rot) [1500]: ") or 1500))
OTHER_TARGET = [p0, p1, p2, p3, p4, p5]
current_arm_positions = list(OTHER_TARGET)
print(f"\nSuccessfully Registered User 'OTHER' Configuration: {OTHER_TARGET}\n")
break
except ValueError:
print("[ERROR] Integers only. Restarting selection.\n")
# Dynamic baseline memory tracking variable
dynamic_ready_positions = list(current_arm_positions)
# --- STEP 3: RUNTIME EXECUTION ENGINE ---
BAUD_RATE = 9600
TRANSIT_TIME_MS = 3000
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
win7.write(b"==================================================\r\n")
win7.write(b"--- SYSTEM BOOT: LOCKOUT ACTIVE (SAFE MODE) ---\r\n")
win7.write(b"==================================================\r\n\r\n")
last_processed_command = ""
while True:
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)
if cokoino.in_waiting > 0:
data = cokoino.readline()
cmd = data.decode('utf-8', errors='ignore').strip()
if cmd and not cmd.startswith("LED:"):
cmd_upper = cmd.upper()
if cmd != last_processed_command:
last_processed_command = ""
if "RELEASE" in cmd_upper:
last_processed_command = ""
continue
if system_state == 0:
if "START" in cmd_upper:
system_state = 1
cokoino.write(b"LED:STATE:GREEN\n")
win7.write(b"[!!!] START DETECTED [!!!]\r\n\r\n")
elif system_state == 1:
if "CROSS" in cmd_upper:
if last_processed_command != "CROSS":
last_processed_command = "CROSS"
win7.write(b"[FROM GAMEPAD] -> CROSS\r\n")
lynx.write(b"VER\r")
time.sleep(0.1)
if lynx.in_waiting > 0:
response = lynx.readline().decode('utf-8', errors='ignore').strip()
win7.write(f"==> LYNXMOTION RESPONSE: '{response}'\r\n\r\n".encode('utf-8'))
elif "TRIANGLE" in cmd_upper:
if last_processed_command != "TRIANGLE":
last_processed_command = "TRIANGLE"
win7.write(b"[FROM GAMEPAD] -> TRIANGLE\r\n")
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" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
elif "CIRCLE" in cmd_upper:
if last_processed_command != "CIRCLE":
last_processed_command = "CIRCLE"
win7.write(b"[FROM GAMEPAD] -> CIRCLE\r\n")
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" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
# SQUARE BUTTON: TRANSMIT CRITICAL COMMAND PACKET TO LYNXMOTION TO DRIVER OVERRIDE 'OTHER'
elif "SQUARE" in cmd_upper:
if last_processed_command != "SQUARE":
last_processed_command = "SQUARE"
win7.write(b"[FROM GAMEPAD] -> SQUARE: EXECUTING HARD ALIGNMENT TO 'OTHER'\r\n")
# 1. Update internal track models completely
current_arm_positions = list(OTHER_TARGET)
dynamic_ready_positions = list(OTHER_TARGET)
# 2. Build the physical movement string packet
macro_packet = "".join(f"#{j}P{OTHER_TARGET[j]}" for j in range(6)) + f"T{TRANSIT_TIME_MS}\r"
# 3. CRITICAL TRANSMISSION: Drive the physical arm to match the specified values
lynx.write(macro_packet.encode('utf-8'))
# 4. Diagnostics Echo to Windows 7 Terminal
win7.write(b"==================================================\r\n")
win7.write(f"[HARD SYNC PACKET SENT] -> {macro_packet.strip()}\r\n".encode('utf-8'))
win7.write(b"[MEMORY LOCK] Baseline set to matching user Matrix.\r\n")
win7.write(b"==================================================\r\n\r\n")
elif "SELECT" in cmd_upper:
if last_processed_command != "SELECT":
last_processed_command = "SELECT"
dynamic_ready_positions = list(current_arm_positions)
win7.write(b"==================================================\r\n")
win7.write(b"[MEMORY LOCK] Dynamic READY Baseline Shifted Manually!\r\n")
win7.write(f"Captured Model Track: {dynamic_ready_positions}\r\n".encode('utf-8'))
win7.write(b"==================================================\r\n\r\n")
elif "TOOL ADVANCE" in cmd_upper:
if last_processed_command != "ADVANCE":
last_processed_command = "ADVANCE"
win7.write(b"[FROM GAMEPAD] -> L1: TOOL ADVANCE DETECTED\r\n")
baseline_p1 = dynamic_ready_positions[1]
baseline_p2 = dynamic_ready_positions[2]
total_sh_pulses = 217
if baseline_p1 > 1600:
total_el_pulses = 215
elif baseline_p1 < 1400:
total_el_pulses = -80
else:
total_el_pulses = -45
win7.write(b" -------------------------------------------------------------\r\n")
win7.write(b" SEGMENT SHOULDER (P1) ELBOW (P2) WRIST (P3) (LEVEL)\r\n")
win7.write(b" -------------------------------------------------------------\r\n")
for step in [1, 2, 3, 4, 5]:
fraction = step / 5.0
target_p1 = constraint_safety_clip(int(round(baseline_p1 - (total_sh_pulses * fraction))))
target_p2 = constraint_safety_clip(int(round(baseline_p2 - (total_el_pulses * fraction))))
el_delta = target_p2 - baseline_p2
target_p3 = constraint_safety_clip(dynamic_ready_positions[3] - el_delta)
current_arm_positions[1] = target_p1
current_arm_positions[2] = target_p2
current_arm_positions[3] = target_p3
packet = f"#1P{target_p1}#2P{target_p2}#3P{target_p3}T1000\r"
lynx.write(packet.encode('utf-8'))
win7.write(f" {step} cm {target_p1} {target_p2} {target_p3} -> {packet.strip()} [\\r]\r\n".encode('utf-8'))
time.sleep(1.0)
win7.write(b"[PUSH COMPLETE] Trajectory complete.\r\n\r\n")
elif "TOOL RETRACT" in cmd_upper:
if last_processed_command != "RETRACT":
last_processed_command = "RETRACT"
win7.write(b"[FROM GAMEPAD] -> L2: TOOL RETRACT DETECTED\r\n")
current_arm_positions = list(dynamic_ready_positions)
macro_packet = "".join(f"#{j}P{dynamic_ready_positions[j]}" for j in range(6)) + f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
time.sleep(0.01)
except KeyboardInterrupt:
print("\nBridge safely terminated.")
except Exception as e:
print(f"\nBridge Error: {e}")(th)
Offline
Like button can go here
This post contains Version 40 of bridge.py...
This version allows for operator input of position data during a run.
This feature is needed at this time because navigation is not yet in place.
# bridgeV40_Tune.py Prepared by Gemini Supervised by Tom Hanson
# Interactive Runtime Parameter Tuning via SELECT Dialog Intercept
import serial
import serial.tools.list_ports
import time
import xml.etree.ElementTree as ET
print("==================================================")
print("Initializing Robot Junction Bridge V40...")
print("Dynamic Runtime Tuning Engine Active")
print("==================================================\n")
# ==============================================================================
# STEP 0: UNIFIED ROBOT DESCRIPTION FORMAT (URDF) - XML PARAMETERS
# ==============================================================================
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))
def prompt_for_other_matrix(current_defaults):
"""Reusable dialog window inside the console terminal prompt"""
print("\n--- OPERATOR FIELD SPECIFICATION INJECTION ---")
print("Enter raw pulse widths (500 - 2500) or press ENTER to keep current:")
try:
p0 = constraint_safety_clip(int(input(f" Channel 0 (Base) [{current_defaults[0]}]: ") or current_defaults[0]))
p1 = constraint_safety_clip(int(input(f" Channel 1 (Shoulder) [{current_defaults[1]}]: ") or current_defaults[1]))
p2 = constraint_safety_clip(int(input(f" Channel 2 (Elbow) [{current_defaults[2]}]: ") or current_defaults[2]))
p3 = constraint_safety_clip(int(input(f" Channel 3 (Wrist) [{current_defaults[3]}]: ") or current_defaults[3]))
p4 = constraint_safety_clip(int(input(f" Channel 4 (Gripper) [{current_defaults[4]}]: ") or current_defaults[4]))
p5 = constraint_safety_clip(int(input(f" Channel 5 (Wrist Rot) [{current_defaults[5]}]: ") or current_defaults[5]))
return [p0, p1, p2, p3, p4, p5]
except ValueError:
print("[ERROR] Integers only. Retaining previous values.")
return current_defaults
# --- STEP 1: SERIAL CONNECTIONS ---
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")
# --- STEP 2: POSITION MANAGEMENT CONFIGURATION ---
current_arm_positions = [CENTER_PULSE] * 6
HOME_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 500, 500, 1500]
OTHER_TARGET = [1500, 1500, 1500, 1500, 1500, 1500]
print("\n==================================================")
print(" ROBOT ARM CONFIGURATION MATRIX ")
print("==================================================")
print(" [T] Secured in TUCK position")
print(" [H] Standing upright at HOME position")
print(" [O] MANUALLY SPECIFY 'OTHER' VALUES (Field Override)")
print("--------------------------------------------------")
while True:
choice = input("Enter alignment choice (T/H/O): ").strip().upper()
if choice == 'T':
current_arm_positions = list(TUCK_TARGET)
break
elif choice == 'H':
current_arm_positions = list(HOME_TARGET)
break
elif choice == 'O':
OTHER_TARGET = prompt_for_other_matrix(OTHER_TARGET)
current_arm_positions = list(OTHER_TARGET)
print(f"\nSuccessfully Registered Initial 'OTHER' Configuration: {OTHER_TARGET}\n")
break
dynamic_ready_positions = list(current_arm_positions)
# --- STEP 3: RUNTIME EXECUTION ENGINE ---
BAUD_RATE = 9600
TRANSIT_TIME_MS = 3000
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
win7.write(b"==================================================\r\n")
win7.write(b"--- SYSTEM BOOT: LOCKOUT ACTIVE (SAFE MODE) ---\r\n")
win7.write(b"==================================================\r\n\r\n")
last_processed_command = ""
while True:
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)
if cokoino.in_waiting > 0:
data = cokoino.readline()
cmd = data.decode('utf-8', errors='ignore').strip()
if cmd and not cmd.startswith("LED:"):
cmd_upper = cmd.upper()
if cmd != last_processed_command:
last_processed_command = ""
if "RELEASE" in cmd_upper:
last_processed_command = ""
continue
if system_state == 0:
if "START" in cmd_upper:
system_state = 1
cokoino.write(b"LED:STATE:GREEN\n")
win7.write(b"[!!!] START DETECTED [!!!]\r\n\r\n")
elif system_state == 1:
if "CROSS" in cmd_upper:
if last_processed_command != "CROSS":
last_processed_command = "CROSS"
win7.write(b"[FROM GAMEPAD] -> CROSS\r\n")
lynx.write(b"VER\r")
time.sleep(0.1)
if lynx.in_waiting > 0:
response = lynx.readline().decode('utf-8', errors='ignore').strip()
win7.write(f"==> LYNXMOTION RESPONSE: '{response}'\r\n\r\n".encode('utf-8'))
elif "TRIANGLE" in cmd_upper:
if last_processed_command != "TRIANGLE":
last_processed_command = "TRIANGLE"
win7.write(b"[FROM GAMEPAD] -> TRIANGLE\r\n")
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" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
elif "CIRCLE" in cmd_upper:
if last_processed_command != "CIRCLE":
last_processed_command = "CIRCLE"
win7.write(b"[FROM GAMEPAD] -> CIRCLE\r\n")
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" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
# SQUARE BUTTON: EXECUTE AND TRANSMIT ACTIVE 'OTHER' MATRIX PULSES
elif "SQUARE" in cmd_upper:
if last_processed_command != "SQUARE":
last_processed_command = "SQUARE"
win7.write(b"[FROM GAMEPAD] -> SQUARE: EXECUTING HARD ALIGNMENT TO 'OTHER'\r\n")
current_arm_positions = list(OTHER_TARGET)
dynamic_ready_positions = list(OTHER_TARGET)
macro_packet = "".join(f"#{j}P{OTHER_TARGET[j]}" for j in range(6)) + f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(b"==================================================\r\n")
win7.write(f"[HARD SYNC PACKET SENT] -> {macro_packet.strip()}\r\n".encode('utf-8'))
win7.write(f"[BASELINE LOCKED] Matrix: {dynamic_ready_positions}\r\n".encode('utf-8'))
win7.write(b"==================================================\r\n\r\n")
# SELECT BUTTON: RE-OPEN DIALOG INTERCEPT IN CONSOLE FOR TUNING
elif "SELECT" in cmd_upper:
if last_processed_command != "SELECT":
last_processed_command = "SELECT"
win7.write(b"[RUNTIME INTERCEPT] -> SELECT Pressed. Opening Dialog in Terminal window...\r\n")
print("\n\n==================================================")
print(" RUNTIME TUNING DIALOG INTERCEPT INITIATED ")
print("==================================================")
# Re-open input routine inside script execution terminal
OTHER_TARGET = prompt_for_other_matrix(OTHER_TARGET)
print("\n Tuning changes saved to background staging matrix.")
print(" -> Press SQUARE on the gamepad to deploy and execute motion.")
print("==================================================\n")
win7.write(f"[STAGED NEW MATRIX] background settings updated to: {OTHER_TARGET}\r\n".encode('utf-8'))
win7.write(b" -> Press SQUARE to transmit to physical hardware.\r\n\r\n")
# L1 BUTTON: KINEMATIC PROPULSION LINK
elif "TOOL ADVANCE" in cmd_upper:
if last_processed_command != "ADVANCE":
last_processed_command = "ADVANCE"
win7.write(b"[FROM GAMEPAD] -> L1: TOOL ADVANCE DETECTED\r\n")
baseline_p1 = dynamic_ready_positions[1]
baseline_p2 = dynamic_ready_positions[2]
total_sh_pulses = 217
if baseline_p1 > 1600:
total_el_pulses = 215
elif baseline_p1 < 1400:
total_el_pulses = -80
else:
total_el_pulses = -45
win7.write(b" -------------------------------------------------------------\r\n")
win7.write(b" SEGMENT SHOULDER (P1) ELBOW (P2) WRIST (P3) (LEVEL)\r\n")
win7.write(b" -------------------------------------------------------------\r\n")
for step in [1, 2, 3, 4, 5]:
fraction = step / 5.0
target_p1 = constraint_safety_clip(int(round(baseline_p1 - (total_sh_pulses * fraction))))
target_p2 = constraint_safety_clip(int(round(baseline_p2 - (total_el_pulses * fraction))))
el_delta = target_p2 - baseline_p2
target_p3 = constraint_safety_clip(dynamic_ready_positions[3] - el_delta)
current_arm_positions[1] = target_p1
current_arm_positions[2] = target_p2
current_arm_positions[3] = target_p3
packet = f"#1P{target_p1}#2P{target_p2}#3P{target_p3}T1000\r"
lynx.write(packet.encode('utf-8'))
win7.write(f" {step} cm {target_p1} {target_p2} {target_p3} -> {packet.strip()} [\\r]\r\n".encode('utf-8'))
time.sleep(1.0)
win7.write(b"[PUSH COMPLETE] Trajectory complete.\r\n\r\n")
# L2 BUTTON: RESET TO ACTIVE FIELD BASELINE
elif "TOOL RETRACT" in cmd_upper:
if last_processed_command != "RETRACT":
last_processed_command = "RETRACT"
win7.write(b"[FROM GAMEPAD] -> L2: TOOL RETRACT DETECTED\r\n")
current_arm_positions = list(dynamic_ready_positions)
macro_packet = "".join(f"#{j}P{dynamic_ready_positions[j]}" for j in range(6)) + f"T{TRANSIT_TIME_MS}\r"
lynx.write(macro_packet.encode('utf-8'))
win7.write(f" {macro_packet.strip()} [\\r]\r\n\r\n".encode('utf-8'))
time.sleep(0.01)
except KeyboardInterrupt:
print("\nBridge safely terminated.")
except Exception as e:
print(f"\nBridge Error: {e}")(th)
Offline
Like button can go here