You are not logged in.
We skipped V82 in this series. V82 was devoted to solving a problem with code to compute movement of the robot arm in a distance X without change of Z. V83 is able to perform the maneuver, but all the diagnostic printing has been consolidated in a single function which can be activated if needed. We are now moving toward configuration of hardware and software to operate as a stand-alone system without keyboard, mouse or monitor. We are not quite ready for that. V83 is a first step toward field operation.
# bridgeV83.py Prepared by ChatGPT Supervised by Tom Hanson
# ==============================================================================
# Version 83: CENTRALIZED OPTIONAL WINDOWS 7 / HYPERTERMINAL REPORTING
# All Win7 serial output passes through report_win7().
# Enter N or X for the Windows 7 port during setup to disable it.
# Robot calculations, safety audits, Cokoino traffic, and SSC-32U
# control remain active when Win7 reporting is disabled.
# ==============================================================================
# Version 82: CORRECTED JOINT/WORLD GEOMETRY + L1 ALTITUDE AUDIT
#
# PURPOSE:
# V82 corrects the mathematical relationship between the robot's local joint
# angles and the fixed X-Z world coordinate system used by the L1 Tool Advance.
#
# PHYSICAL OBSERVATION LEADING TO V82:
# Bench tests established that the Shoulder, Elbow, and Wrist motors are mounted
# with their power hubs on the PORT side of the arm. Viewed from STARBOARD,
# the observer sees the aft/end side of all three motor shafts. Increasing PWM
# produces the same observed motor rotation sense at these motors.
#
# The important distinction is that identical motor rotation does NOT mean that
# every joint angle is added in the world-coordinate geometry. The Shoulder
# angle establishes the absolute world orientation of Link AB. The Elbow angle
# is a LOCAL bend of Link BC relative to Link AB.
#
# V81 modeled the absolute orientation of BC as:
#
# BC_world_angle = shoulder_angle + elbow_bend
#
# Physical bench observations indicate that, for the installed AL5D geometry
# and the angle conventions used by this program, the relationship should be:
#
# BC_world_angle = shoulder_angle - elbow_bend
#
# V82 will therefore revise Forward Kinematics, Inverse Kinematics, and wrist
# counter-rotation together so that all calculations use one consistent joint-
# to-world convention. The change will NOT be made as an isolated sign change.
#
# L1 MOTION REQUIREMENT:
# Pivot C (the wrist pivot) is commanded to advance exactly +50 mm in world X
# while maintaining its starting world Z coordinate:
#
# C_target_X = C_start_X + 50.0 mm
# C_target_Z = C_start_Z
#
# V82 will make this Z-altitude requirement conspicuous in the HyperTerminal
# transcript. The audit will report:
#
# - Starting X and Z coordinates of Pivots B and C
# - Starting world orientation of AB and BC
# - Starting local elbow bend
# - Requested C target X and target Z
# - Solved final Shoulder angle, Elbow bend, and BC world angle
# - Calculated final B and C coordinates
# - Starting C Z altitude
# - Requested C Z altitude
# - Calculated final C Z altitude
# - Calculated Delta-Z and Z error
# - Expected physical Shoulder, Elbow, and Wrist rotation directions
# - Starting and final physical PWM values
# - Final Forward-Kinematics verification of Delta-X and Delta-Z
# - Exact SSC-32U synchronized group-move command
#
# TEST POLICY:
# V81 remains the frozen known reference version.
#
# Initial V82 testing will preserve the real SSC-32U command-generation and
# serial-write path. After normal startup/verification and establishment of
# the READY starting pose, servo power will be removed before L1 is invoked.
# V82 can then execute the complete production calculation and report the exact
# command it would send without permitting physical arm movement.
#
# Only after the V82 transcript demonstrates correct geometry, correct motor
# direction, and constant calculated C altitude will servo power be restored
# for a live L1 test. During that test, the physical altitude of Pivot C will
# be measured before and after L1 and compared with the mathematical Delta-Z.
#
# DESIGN PRINCIPLE:
#
# GEOMETRY FIRST. MOTORS SECOND.
#
# World geometry, local joint geometry, and PWM/motor coordinates are related
# but distinct. V82 keeps those distinctions explicit so that mathematical
# self-consistency cannot conceal an incorrect physical coordinate convention.
# ==============================================================================
# # Version 81: LIVE +50 MM L1 GROUP-MOVE VERSION.
# Retains V80 geometry, envelope, PWM, and path audits.
# After all checks pass, one synchronized S1/S2/S3
# endpoint command will be sent to the SSC-32U.
# # Version 80: SSC-32U GROUP-MOVE TRAJECTORY AUDIT.
# Calculates one complete +50 mm +X endpoint.
# Simulates the intermediate Cartesian path produced
# by a synchronized S1/S2/S3 group move.
# NO PHYSICAL L1 MOTION IS SENT TO LYNXMOTION.
# # Version 79: FIRST LIVE L1 TEST.
# Executes one validated +10 mm +X (bowward) wrist advance only.
# Retains V78 305 mm operational envelope and IK safety checks.
# Version 76: Adding code to clear Cokoino LED's after Halt
# Verions prior to 76 were prepared by Gemini as supervised by Tom Hanson
# Version 75: CALIBRATED PHYSICAL ANGLE ENGINE & START LED RESET RECTIFIED.
# Corrected pwm_to_physical_radians() to remove trim offsets before angle conversion.
# Ensured S1 (1386 PWM) = 90.0 deg and S2 (633 PWM) = 0.0 deg physical baseline.
# Updated START handler to send LED:LOCK:0 clear before LED:LOCK:1 to flush red lockout buffer.
# Version 74: KINEMATIC MATH & BOUNDARY AUDIT RECTIFIED.
# Corrected 2-DOF Inverse Kinematics (IK) motor-frame alignment.
# Enforced strict Cartesian altitude (Z-drift) boundary checks in Step 12.
# Version 73: FULL DIAGNOSTIC TELEMETRY RESTORED.
# Version 69: Baseline physical forward kinematics engine with trim offset mapping.
# ==============================================================================
# STARBOARD PHYSICAL VIEW vs PORT MOTOR FRAME MAP
# ==============================================================================
# [STERN / 180 deg / 2500us] <- VERTICAL (90 deg / 1500us) -> [BOW / 0 deg / 500us]
# - INCREASING PWM moves joint toward STERN (Aft / Backward / Down in Tuck)
# - DECREASING PWM moves joint toward BOW (Forward / Up in Tuck)
# ==============================================================================
# ==============================================================================
# OFFICIAL FIELD OPERATOR CARD MATRIX
# ==============================================================================
# STEP 0 | Hex x'0' | LED: ROTATING RED LOOP | Power Up: Lockout Safe Mode
# STEP 1 | Hex x'1' | LED: BLUE/WHITE BIT 1 | START Pressed: Runtime Engine Live
# STEP 2 | Hex x'2' | LED: BLUE/WHITE BIT 2 | CROSS Pressed: Query LynxMotion VER
# STEP 3 | Hex x'3' | LED: BLUE/WHITE BIT 3 | CIRCLE Pressed: Transit to TUCK
# STEP 4 | Hex x'4' | LED: BLUE/WHITE BIT 4 | TRIANGLE Pressed: Transit to HOME
# STEP 5 | Hex x'5' | LED: BLUE/WHITE BIT 5 | SQUARE Pressed: Transit to READY (Tuned)
# STEP 6 | Hex x'6' | LED: FLASH/SOLID BLUE | L3 Click: Tune Base/Shoulder (Lock & Save)
# STEP 7 | Hex x'7' | LED: FLASH/SOLID BLUE | R3 Click: Tune Elbow Reach (Lock & Save)
# STEP 8 | Hex x'8' | LED: BLUE/WHITE BIT 8 | D-Pad: Tune Wrist Pitch & Rotate (Live)
# STEP 9 | Hex x'9' | LED: BLUE/WHITE BIT 9 | L2 Pressed: Tool Retract (Transit to READY)
# STEP 10 | Hex x'A' | LED: BLUE/WHITE BIT 10 | R1 Pressed: Ready Position (Transit to READY)
# STEP 11 | Hex x'B' | LED: BLUE/WHITE BIT 11 | R2 Pressed: System Home (Transit to HOME)
# STEP 12 | Hex x'C' | LED: BLUE/WHITE BIT 12 | L1 Pressed: Tool Advance (FK Calculation Audit)
# ==============================================================================
import math
import serial
import serial.tools.list_ports
import time
import xml.etree.ElementTree as ET
# --- PHYSICAL LINK LENGTH PARAMETERS ---
L1_SHOULDER_MM = 145.0 # Link 1 (Shoulder pivot A to Elbow pivot B)
L2_ELBOW_MM = 185.0 # Link 2 (Elbow pivot B to Wrist pivot C)
L3_WRIST_MM = 120.0 # Link 3 (Wrist pivot C to Tool Tip T)
# --- PHYSICAL HARDWARE CALIBRATION OFFSETS (BENCH MEASURED) ---
# Maps ideal target PWM to actual physical PWM required to achieve true alignment.
PHYSICAL_TRIM_OFFSETS = {
0: 0, # Base
1: -114, # Shoulder (Trims nominal 1500 -> 1386 PWM forward toward Bow for true 90 deg)
2: 133, # Elbow (Trims nominal 500 -> 633 PWM for true 0 deg physical extension)
3: 0, # Wrist Pitch
4: 0, # Wrist Rotate
5: 0, # Gripper
}
# Safe operational limits for servo pulse widths (PWM)
SAFE_PWM_MIN = 600
SAFE_PWM_MAX = 2400
# Deliberate operational reach envelope for Step 12 L1 motion.
# Geometric maximum wrist reach is 330 mm, but normal operation
# is intentionally kept inside that limit.
MAX_WRIST_REACH_MM = 305.0
REACH_POLICY_TOL_MM = 1.0e-9
# --- PWM & ANGLE CONVERSION HELPERS ---
def constraint_safety_clip(pulse):
return max(500, min(2500, pulse))
def get_physical_pwm(channel, raw_pwm):
"""Converts a nominal/commanded PWM to the true physical PWM acting on hardware."""
calibrated_pwm = raw_pwm + PHYSICAL_TRIM_OFFSETS.get(channel, 0)
return constraint_safety_clip(calibrated_pwm)
def get_nominal_pwm(channel, physical_pwm):
"""Converts a physical PWM back to the program's nominal/reference PWM."""
nominal_pwm = physical_pwm - PHYSICAL_TRIM_OFFSETS.get(channel, 0)
return int(round(nominal_pwm))
def pwm_to_physical_radians(channel, phys_pwm):
"""Converts actual physical PWM to true geometric joint angles (radians).
Strips hardware calibration trim before angle scaling so bench positions evaluate accurately.
"""
untrimmed_pwm = phys_pwm - PHYSICAL_TRIM_OFFSETS.get(channel, 0)
degrees = (untrimmed_pwm - 500) * (180.0 / 2000.0)
return math.radians(degrees)
def physical_radians_to_pwm(channel, rad):
"""Converts physical joint angle in radians back to calibrated physical PWM."""
degrees = math.degrees(rad)
untrimmed_pwm = 500 + (degrees * (2000.0 / 180.0))
phys_pwm = untrimmed_pwm + PHYSICAL_TRIM_OFFSETS.get(channel, 0)
return int(round(phys_pwm))
# --- FULL FORWARD KINEMATICS ENGINE (3 JOINTS + TIP) ---
def compute_full_kinematics(theta1_rad, theta2_rad, theta3_rad):
"""Computes Cartesian positions (X, Z) for all arm pivots.
Origin (0,0) is fixed at Shoulder Pivot A.
Returns:
(x_b, z_b) -> Elbow Pivot B
(x_c, z_c) -> Wrist Pivot C
(x_tip, z_tip) -> Tool Tip Position T
"""
# 1. Shoulder Pivot A (Fixed Baseline Foundation)
x_a = 0.0
z_a = 0.0
# 2. Elbow Joint B relative to Shoulder Pivot A
x_b = L1_SHOULDER_MM * math.cos(theta1_rad)
z_b = L1_SHOULDER_MM * math.sin(theta1_rad)
# 3. Absolute world angle of Link 2 (Elbow link BC)
#
# V82 PHYSICAL JOINT CONVENTION:
# theta1_rad is the absolute world angle of Link AB.
# theta2_rad is the LOCAL elbow bend measured in the installed motor's
# positive direction. Bench testing established that increasing this
# elbow bend rotates BC clockwise relative to AB when viewed from
# starboard.
#
# The world mathematical frame uses counterclockwise-positive angles.
# Therefore a positive local elbow bend SUBTRACTS from the absolute
# world angle established by the shoulder:
#
# BC_world_angle = theta1_rad - theta2_rad
#
absolute_elbow_angle = theta1_rad - theta2_rad
# Wrist Joint C relative to Elbow Joint B
x_c = x_b + L2_ELBOW_MM * math.cos(absolute_elbow_angle)
z_c = z_b + L2_ELBOW_MM * math.sin(absolute_elbow_angle)
# 4. Absolute world angle of Link 3 (Wrist/tool link)
#
# Wrist pitch remains a local joint angle referenced to its 90-degree
# centered condition. V82 retains this expression here; Step 12 wrist
# counter-rotation will be revised separately to preserve the desired
# absolute tool orientation under the corrected BC convention.
absolute_wrist_angle = absolute_elbow_angle + (
theta3_rad - math.pi / 2.0
)
# Gripper Tool Tip T relative to Wrist Joint C
x_tip = x_c + L3_WRIST_MM * math.cos(absolute_wrist_angle)
z_tip = z_c + L3_WRIST_MM * math.sin(absolute_wrist_angle)
return (x_b, z_b), (x_c, z_c), (x_tip, z_tip)
# --- 2-DOF ANALYTICAL INVERSE KINEMATICS ENGINE ---
def solve_inverse_kinematics_2d(target_x, target_z):
"""Calculates required Shoulder (theta1) and Elbow (theta2) joint angles.
theta1 and theta2 are returned as physical joint angles in radians matching
the SSC-32 motor frame.
"""
r_sq = target_x**2 + target_z**2
r = math.sqrt(r_sq)
# Reachability Check (Triangle Inequality)
if r > (L1_SHOULDER_MM + L2_ELBOW_MM) or r < abs(
L1_SHOULDER_MM - L2_ELBOW_MM
):
return None, None
# Law of Cosines for Interior Elbow Angle
cos_theta2_int = (r_sq - L1_SHOULDER_MM**2 - L2_ELBOW_MM**2) / (
2.0 * L1_SHOULDER_MM * L2_ELBOW_MM
)
cos_theta2_int = max(-1.0, min(1.0, cos_theta2_int))
phi_elbow = math.acos(cos_theta2_int)
# Base Angle to Target Vector + Angle Offset via Law of Cosines
gamma = math.atan2(target_z, target_x)
cos_alpha = (L1_SHOULDER_MM**2 + r_sq - L2_ELBOW_MM**2) / (
2.0 * L1_SHOULDER_MM * r
)
cos_alpha = max(-1.0, min(1.0, cos_alpha))
alpha = math.acos(cos_alpha)
# V82 JOINT/WORLD FRAME TRANSFORM
#
# The Law of Cosines gives phi_elbow as the positive MAGNITUDE of the
# local angle between AB and BC.
#
# V82 defines positive elbow bend as clockwise from AB when viewed from
# starboard. In the counterclockwise-positive world frame:
#
# BC_world_angle = theta1_rad - theta2_rad
#
# For this physical branch of triangle A-B-C, AB lies alpha
# counterclockwise from the radial vector AC. Therefore:
#
# shoulder_world = gamma + alpha
# elbow_bend = phi_elbow
#
# These equations are the inverse counterpart of the V82 Forward
# Kinematics relationship above. They must remain paired.
theta1_rad = gamma + alpha
theta2_rad = phi_elbow
return theta1_rad, theta2_rad
def build_calibrated_macro_packet(target_array, transit_ms):
"""Formats outbound serial strings by applying physical offsets to each joint."""
packet_parts = []
for ch in range(6):
phys_pwm = get_physical_pwm(ch, target_array[ch])
packet_parts.append(f"#{ch}P{phys_pwm}")
return "".join(packet_parts) + f"T{transit_ms}\r"
# --- URDF COMPONENT DESCRIPTION PARSING ---
urdf_configuration = """<?xml version="1.0" ?>
<robot name="junction_arm">
<controller_settings>
<min_pulse_width>500</min_pulse_width>
<max_pulse_width>2500</max_pulse_width>
<center_pulse_width>1500</center_pulse_width>
</controller_settings>
<joint channel="0" name="Base"></joint>
<joint channel="1" name="Shoulder"></joint>
<joint channel="2" name="Elbow"></joint>
<joint channel="3" name="Wrist"></joint>
<joint channel="4" name="Gripper"></joint>
<joint channel="5" name="Wrist Rot"></joint>
</robot>
"""
root = ET.fromstring(urdf_configuration)
# --- PORT CONFIGURATION ---
all_found_ports = serial.tools.list_ports.comports()
ports = [p for p in all_found_ports if "USB" in p.device.upper()]
ports = sorted(ports, key=lambda x: x.device)
# V83 permits operation with only the two required robot USB devices.
# Windows 7 / HyperTerminal is optional and may be disabled with N or X.
if len(ports) < 2:
print(f"[ERROR] Found only {len(ports)} physical USB devices.")
exit(1)
for idx, port in enumerate(ports):
print(f" [{idx}] {port.device}")
while True:
try:
win7_choice = input(
"Enter index number for WINDOWS 7 (HyperTrm), or N/X for none: "
).strip()
win7_reporting_enabled = win7_choice.upper() not in {"N", "X"}
if win7_reporting_enabled:
win7_idx = int(win7_choice)
if win7_idx < 0 or win7_idx >= len(ports):
print("[INVALID WINDOWS 7 INDEX] Try again.\n")
continue
else:
win7_idx = None
cokoino_idx = int(input("Enter index number for COKOINO (Arduino): "))
if cokoino_idx < 0 or cokoino_idx >= len(ports):
print("[INVALID COKOINO INDEX] Try again.\n")
continue
if win7_idx is not None and cokoino_idx == win7_idx:
print("\n[CONFLICT DETECTED] Re-enter assignments.\n")
continue
used_indices = {cokoino_idx}
if win7_idx is not None:
used_indices.add(win7_idx)
lynx_candidates = [i for i in range(len(ports)) if i not in used_indices]
if len(lynx_candidates) != 1:
print(
"[PORT ASSIGNMENT ERROR] Exactly one unassigned USB port must "
"remain for LynxMotion. Try again.\n"
)
continue
lynx_idx = lynx_candidates[0]
WIN7_PORT = ports[win7_idx].device if win7_idx is not None else None
COKOINO_PORT = ports[cokoino_idx].device
LYNX_PORT = ports[lynx_idx].device
break
except ValueError:
print("[INVALID] Try again.\n")
# --- DATA STATE MEMORY SPACE ---
HOME_TARGET = [1500, 1500, 500, 1500, 1500, 1500]
TUCK_TARGET = [1500, 1821, 1842, 2500, 500, 1500]
READY_TARGET = [1500, 1931, 1274, 1500, 500, 1500] # leaning back
current_arm_positions = list(TUCK_TARGET)
LEFT_STEER_LIVE = False
RIGHT_STEER_LIVE = False
BAUD_RATE = 9600
TRANSIT_TIME_MS = 3000
WRIST_STEP_SIZE = 25
def send_led_binary_pattern(ser_conn, count_val):
if count_val == 0:
ser_conn.write(b"LED:LOCK:0\n")
else:
hex_val = format(count_val, "X")
ser_conn.write(f"LED:LOCK:{hex_val}\n".encode("utf-8"))
# --- V83 OPTIONAL WINDOWS 7 REPORTING GATEWAY ---
win7 = None
def report_win7(message):
"""Send diagnostic/reporting output to Windows 7 only when enabled.
All Win7/HyperTerminal traffic must pass through this function.
Disabling reporting must never alter robot calculations, safety checks,
Cokoino communication, or LynxMotion commands.
"""
if not win7_reporting_enabled or win7 is None:
return
if isinstance(message, str):
message = message.encode("utf-8")
win7.write(message)
try:
if win7_reporting_enabled:
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)
report_win7(b"\x1b[2J\x1b[H") # Clear HyperTerminal Screen
report_win7(b"==================================================\r\n")
report_win7(b"Initializing Robot Junction Bridge V83...\r\n")
report_win7(
b"Phase 1 Forward & Inverse Kinematic Engines Active (Fully"
b" Logged)\r\n"
)
report_win7(b"==================================================\r\n\r\n")
system_state = 0
binary_counter = 0
last_processed_command = ""
flash_state = False
last_flash_time = time.time()
report_win7(b"==================================================\r\n")
report_win7(b"--- SYSTEM BOOT: STEP 0 (LOCKOUT SAFE MODE) ---\r\n")
report_win7(b"==================================================\r\n\r\n")
while True:
current_time = time.time()
# Step 0 Lockout Pulse Loop
if system_state == 0:
hex_val = format(binary_counter, "X")
cokoino.write(f"LED:LOCK:{hex_val}\n".encode("utf-8"))
binary_counter = (binary_counter + 1) % 16
time.sleep(0.2)
# Steps 6 & 7 Dynamic LED Flashing Manager
elif system_state == 6 and LEFT_STEER_LIVE:
if current_time - last_flash_time > 0.3:
flash_state = not flash_state
if flash_state:
send_led_binary_pattern(cokoino, 6)
else:
cokoino.write(b"LED:LOCK:0\n")
last_flash_time = current_time
elif system_state == 7 and RIGHT_STEER_LIVE:
if current_time - last_flash_time > 0.3:
flash_state = not flash_state
if flash_state:
send_led_binary_pattern(cokoino, 7)
else:
cokoino.write(b"LED:LOCK:0\n")
last_flash_time = current_time
if cokoino.in_waiting > 0:
data = cokoino.readline()
cmd = data.decode("utf-8", errors="ignore").strip()
if cmd and not cmd.startswith("LED:"):
report_win7(f" [COKOINO -> BRDG]: {cmd}\r\n".encode("utf-8"))
cmd_upper = cmd.upper()
# --- STEP 1 INITIATION ---
if system_state == 0:
if "START" in cmd_upper:
system_state = 1
# Flush red lockout loop explicitly before setting Blue Bit 1
cokoino.write(b"LED:LOCK:0\n")
time.sleep(0.05)
send_led_binary_pattern(cokoino, 1)
report_win7(
b"[OPERATOR CARD] -> Step 1: START Detected."
b" System Online.\r\n\r\n"
)
# --- RUNTIME CONTROLLER STATE ENGINE ---
elif system_state >= 1:
# --- RUNTIME START RESET TO STEP 0 ---
if "START" in cmd_upper:
system_state = 0
binary_counter = 0
LEFT_STEER_LIVE = False
RIGHT_STEER_LIVE = False
last_processed_command = ""
cokoino.write(b"ANALOG:DISABLE\n")
cokoino.write(b"LED:LOCK:0\n")
report_win7(
b"[OPERATOR CARD] -> START Reset Detected. "
b"Returning to Step 0 Lockout Safe Mode.\r\n\r\n"
)
continue
# Real-time Analog Stream Processing
if cmd_upper.startswith("ANALOG:"):
parts = cmd_upper.split(":")
if len(parts) == 3:
axis = parts[1]
try:
val = int(parts[2])
offset = val - 128
if LEFT_STEER_LIVE:
if axis == "LX":
step = int(offset * 0.15)
current_arm_positions[0] = (
constraint_safety_clip(
current_arm_positions[0] + step
)
)
elif axis == "LY":
step = int(offset * 0.15)
current_arm_positions[1] = (
constraint_safety_clip(
current_arm_positions[1] + step
)
)
p0 = get_physical_pwm(
0, current_arm_positions[0]
)
p1 = get_physical_pwm(
1, current_arm_positions[1]
)
motion_packet = f"#0P{p0}#1P{p1}T100\r"
lynx.write(motion_packet.encode("utf-8"))
report_win7(
f" [TX -> LYNXMOTION]:"
f" {motion_packet.strip()}\r\n".encode(
"utf-8"
)
)
elif RIGHT_STEER_LIVE:
if axis == "RY":
step = int(offset * 0.15)
current_arm_positions[2] = (
constraint_safety_clip(
current_arm_positions[2] - step
)
)
p2 = get_physical_pwm(
2, current_arm_positions[2]
)
motion_packet = f"#2P{p2}T100\r"
lynx.write(
motion_packet.encode("utf-8")
)
report_win7(
f" [TX -> LYNXMOTION]:"
f" {motion_packet.strip()}\r\n".encode(
"utf-8"
)
)
except ValueError:
pass
continue
# --- STEP 6: L3 HANDSHAKE (BASE & SHOULDER TUNING) ---
elif (
"STICK CLICK LEFT" in cmd_upper
and "RELEASED" not in cmd_upper
):
if LEFT_STEER_LIVE:
LEFT_STEER_LIVE = False
cokoino.write(b"ANALOG:DISABLE\n")
READY_TARGET = list(current_arm_positions)
send_led_binary_pattern(cokoino, 6)
report_win7(
b"[OPERATOR CARD] -> Step 6 Locked."
b" READY_TARGET Matrix Updated.\r\n"
)
report_win7(
f" -> Snapshot Committed to READY:"
f" {READY_TARGET}\r\n\r\n".encode("utf-8")
)
else:
LEFT_STEER_LIVE = True
RIGHT_STEER_LIVE = False
cokoino.write(b"ANALOG:ENABLE\n")
system_state = 6
report_win7(
b"[OPERATOR CARD] -> Step 6: Base & Shoulder"
b" Steering Live.\r\n\r\n"
)
continue
# --- STEP 7: R3 HANDSHAKE (ELBOW AXIS TUNING) ---
elif (
"STICK CLICK RIGHT" in cmd_upper
and "RELEASED" not in cmd_upper
):
if RIGHT_STEER_LIVE:
RIGHT_STEER_LIVE = False
cokoino.write(b"ANALOG:DISABLE\n")
READY_TARGET = list(current_arm_positions)
send_led_binary_pattern(cokoino, 7)
report_win7(
b"[OPERATOR CARD] -> Step 7 Locked."
b" READY_TARGET Matrix Updated.\r\n"
)
report_win7(
f" -> Snapshot Committed to READY:"
f" {READY_TARGET}\r\n\r\n".encode("utf-8")
)
else:
RIGHT_STEER_LIVE = True
LEFT_STEER_LIVE = False
cokoino.write(b"ANALOG:ENABLE\n")
system_state = 7
report_win7(
b"[OPERATOR CARD] -> Step 7: Elbow Axis"
b" Steering Live.\r\n\r\n"
)
continue
# --- STEP 8: D-PAD WRIST FINE-TUNING ---
elif cmd_upper in [
"PAD UP",
"PAD DOWN",
"PAD LEFT",
"PAD RIGHT",
]:
system_state = 8
send_led_binary_pattern(cokoino, 8)
if cmd_upper == "PAD UP":
current_arm_positions[3] = constraint_safety_clip(
current_arm_positions[3] + WRIST_STEP_SIZE
)
elif cmd_upper == "PAD DOWN":
current_arm_positions[3] = constraint_safety_clip(
current_arm_positions[3] - WRIST_STEP_SIZE
)
elif cmd_upper == "PAD LEFT":
current_arm_positions[5] = constraint_safety_clip(
current_arm_positions[5] - WRIST_STEP_SIZE
)
elif cmd_upper == "PAD RIGHT":
current_arm_positions[5] = constraint_safety_clip(
current_arm_positions[5] + WRIST_STEP_SIZE
)
READY_TARGET[3] = current_arm_positions[3]
READY_TARGET[5] = current_arm_positions[5]
p3 = get_physical_pwm(3, current_arm_positions[3])
p5 = get_physical_pwm(5, current_arm_positions[5])
motion_packet = f"#3P{p3}#5P{p5}T150\r"
lynx.write(motion_packet.encode("utf-8"))
report_win7(
f" [TX -> LYNXMOTION]:"
f" {motion_packet.strip()}\r\n".encode("utf-8")
)
report_win7(
f"[OPERATOR CARD] -> Step 8: Wrist Adjusted ->"
f" Pitch(S3): {current_arm_positions[3]} | Rot(S5):"
f" {current_arm_positions[5]}\r\n\r\n".encode(
"utf-8"
)
)
continue
# --- STEP 9: L2 (TOOL RETRACT -> READY TARGET) ---
elif "TOOL RETRACT" in cmd_upper:
if "RELEASED" in cmd_upper:
if last_processed_command == "TOOL RETRACT":
last_processed_command = ""
elif last_processed_command != "TOOL RETRACT":
last_processed_command = "TOOL RETRACT"
system_state = 9
send_led_binary_pattern(cokoino, 9)
current_arm_positions = list(READY_TARGET)
macro_packet = build_calibrated_macro_packet(
READY_TARGET, TRANSIT_TIME_MS
)
lynx.write(macro_packet.encode("utf-8"))
report_win7(
f" [TX -> LYNXMOTION]:"
f" {macro_packet.strip()}\r\n".encode("utf-8")
)
report_win7(
b"[OPERATOR CARD] -> Step 9: Tool Retracted."
b" Returned cleanly to READY Target.\r\n\r\n"
)
# --- STEP 10: R1 (READY POSITION -> READY TARGET) ---
elif "READY POSITION" in cmd_upper:
if "RELEASED" in cmd_upper:
if last_processed_command == "READY POSITION":
last_processed_command = ""
elif last_processed_command != "READY POSITION":
last_processed_command = "READY POSITION"
system_state = 10
send_led_binary_pattern(cokoino, 10)
current_arm_positions = list(READY_TARGET)
macro_packet = build_calibrated_macro_packet(
READY_TARGET, TRANSIT_TIME_MS
)
lynx.write(macro_packet.encode("utf-8"))
report_win7(
f" [TX -> LYNXMOTION]:"
f" {macro_packet.strip()}\r\n".encode("utf-8")
)
report_win7(
b"[OPERATOR CARD] -> Step 10: Executing"
b" transit to READY Target.\r\n\r\n"
)
# --- STEP 11: R2 (SYSTEM HOME -> HOME TARGET) ---
elif "SYSTEM HOME" in cmd_upper:
if "RELEASED" in cmd_upper:
if last_processed_command == "SYSTEM HOME":
last_processed_command = ""
elif last_processed_command != "SYSTEM HOME":
last_processed_command = "SYSTEM HOME"
system_state = 11
send_led_binary_pattern(cokoino, 11)
current_arm_positions = list(HOME_TARGET)
macro_packet = build_calibrated_macro_packet(
HOME_TARGET, TRANSIT_TIME_MS
)
lynx.write(macro_packet.encode("utf-8"))
report_win7(
f" [TX -> LYNXMOTION]:"
f" {macro_packet.strip()}\r\n".encode("utf-8")
)
report_win7(
b"[OPERATOR CARD] -> Step 11: Executing"
b" transit to HOME Target.\r\n\r\n"
)
# --- STEP 12: L1 (V83 LIVE +50 MM GROUP MOVE) ---
elif "TOOL ADVANCE" in cmd_upper:
if "RELEASED" in cmd_upper:
if last_processed_command == "TOOL ADVANCE":
last_processed_command = ""
elif last_processed_command != "TOOL ADVANCE":
last_processed_command = "TOOL ADVANCE"
system_state = 12
send_led_binary_pattern(cokoino, 12) # Hex 'C'
report_win7(
b"\r\n==================================================\r\n"
)
report_win7(
b"--- STEP 12: L1 TOOL ADVANCE (V83 LIVE GROUP MOVE) ---\r\n"
)
report_win7(
b"--- +50 MM ENDPOINT / LIVE MOTION AFTER AUDIT ---\r\n"
)
report_win7(
b"==================================================\r\n"
)
# --------------------------------------------------
# 1. CAPTURE CURRENT PHYSICAL ARM GEOMETRY
# --------------------------------------------------
s1_phys_pwm = get_physical_pwm(
1, current_arm_positions[1]
)
s2_phys_pwm = get_physical_pwm(
2, current_arm_positions[2]
)
s3_phys_pwm = get_physical_pwm(
3, current_arm_positions[3]
)
th1_start = pwm_to_physical_radians(
1, s1_phys_pwm
)
th2_start = pwm_to_physical_radians(
2, s2_phys_pwm
)
th3_start = pwm_to_physical_radians(
3, s3_phys_pwm
)
(
(x_b0, z_b0),
(x_c0, z_c0),
(x_tip0, z_tip0),
) = compute_full_kinematics(
th1_start,
th2_start,
th3_start,
)
start_reach_mm = math.hypot(x_c0, z_c0)
# Absolute orientation of Link 3 / tool.
# V81 will preserve this orientation by
# counter-rotating the wrist.
tool_angle_start = (
th1_start
- th2_start
+ (th3_start - math.pi / 2.0)
)
report_win7(
f" TARGET PWMs -> "
f"S1(Shoulder):{current_arm_positions[1]} | "
f"S2(Elbow):{current_arm_positions[2]} | "
f"S3(Wrist):{current_arm_positions[3]}\r\n".encode(
"utf-8"
)
)
report_win7(
f" PHYSICAL PWMs -> "
f"S1(Shoulder):{s1_phys_pwm} | "
f"S2(Elbow):{s2_phys_pwm} | "
f"S3(Wrist):{s3_phys_pwm}\r\n".encode(
"utf-8"
)
)
report_win7(
f" PHYSICAL ANGLES -> "
f"S1:{th1_start:.4f} rad "
f"({math.degrees(th1_start):.1f}deg) | "
f"S2:{th2_start:.4f} rad "
f"({math.degrees(th2_start):.1f}deg) | "
f"S3:{th3_start:.4f} rad "
f"({math.degrees(th3_start):.1f}deg)\r\n".encode(
"utf-8"
)
)
report_win7(
f" TOOL ABS ANGLE -> "
f"{math.degrees(tool_angle_start):.1f}deg\r\n".encode(
"utf-8"
)
)
report_win7(
f" PHYSICAL COORDS "
f"(relative to Pivot A (0,0)):\r\n"
f" - Pivot A (Shoulder) : "
f"X: 0.0 mm | Z: 0.0 mm\r\n"
f" - Pivot B (Elbow) : "
f"X: {x_b0:+6.1f} mm | "
f"Z: {z_b0:+6.1f} mm\r\n"
f" - Pivot C (Wrist) : "
f"X: {x_c0:+6.1f} mm | "
f"Z: {z_c0:+6.1f} mm\r\n"
f" - Tip T (Tool) : "
f"X: {x_tip0:+6.1f} mm | "
f"Z: {z_tip0:+6.1f} mm\r\n".encode(
"utf-8"
)
)
report_win7(
f" OPERATIONAL REACH -> "
f"Current C radius:{start_reach_mm:.2f} mm | "
f"Limit:{MAX_WRIST_REACH_MM:.2f} mm\r\n".encode(
"utf-8"
)
)
# --------------------------------------------------
# V82 L1 WRIST-PIVOT ALTITUDE REFERENCE
#
# Pivot C is the controlled point for L1.
# Its starting mathematical Z coordinate becomes the
# altitude that L1 must preserve while advancing C
# +50 mm in world X.
#
# NOTE:
# This Z coordinate is relative to Shoulder Pivot A
# at mathematical Z=0. During the later live test,
# the operator may measure C above the physical deck.
# The absolute numbers need not share the same zero;
# the critical comparison is Delta-Z before/after L1.
# --------------------------------------------------
l1_target_x = x_c0 + 50.0
l1_target_z = z_c0
report_win7(
b"\r\n"
b"==================================================\r\n"
)
report_win7(
b" V82 L1 PIVOT-C ALTITUDE REFERENCE\r\n"
)
report_win7(
b"==================================================\r\n"
)
report_win7(
f" START C POSITION -> "
f"X:{x_c0:+.2f} mm | "
f"Z:{z_c0:+.2f} mm\r\n".encode("utf-8")
)
report_win7(
f" TARGET C POSITION -> "
f"X:{l1_target_x:+.2f} mm | "
f"Z:{l1_target_z:+.2f} mm\r\n".encode("utf-8")
)
report_win7(
b" L1 REQUIREMENT -> "
b"Delta-X:+50.00 mm | Delta-Z:+0.00 mm\r\n"
)
report_win7(
f" ALTITUDE TO HOLD -> "
f"C Z = {l1_target_z:+.2f} mm "
f"(relative to Pivot A)\r\n".encode("utf-8")
)
# --------------------------------------------------
# 2. V81 OPERATIONAL ENVELOPE + BASELINE IK SELF-CHECK
#
# Feed the CURRENT wrist coordinates back through
# inverse kinematics. A correct solver should
# reconstruct the current shoulder/elbow angles.
# --------------------------------------------------
report_win7(
b"--------------------------------------------------\r\n"
)
report_win7(
b" V83 BASELINE IK SELF-CHECK:\r\n"
)
audit_failed = False
# V81 policy check: the operator may establish READY anywhere
# appropriate, but L1 is permitted only inside the selected
# operational wrist-reach envelope.
if (
start_reach_mm
> MAX_WRIST_REACH_MM + REACH_POLICY_TOL_MM
):
report_win7(
f" [OPERATING ENVELOPE] REJECTED: "
f"Current wrist radius "
f"{start_reach_mm:.2f} mm exceeds "
f"{MAX_WRIST_REACH_MM:.2f} mm limit.\r\n".encode(
"utf-8"
)
)
audit_failed = True
if not audit_failed:
check_th1, check_th2 = (
solve_inverse_kinematics_2d(
x_c0, z_c0
)
)
if check_th1 is None or check_th2 is None:
report_win7(
b" [SELF-CHECK] REJECTED: "
b"Current arm geometry could not be "
b"reconstructed by IK.\r\n"
)
audit_failed = True
else:
# Normalize shoulder angle into 0..360 degrees.
# Example from TUCK:
# -241.1 degrees becomes +118.9 degrees.
check_th1 = check_th1 % (
2.0 * math.pi
)
delta_th1_deg = (
math.degrees(
check_th1 - th1_start
)
)
delta_th2_deg = (
math.degrees(
check_th2 - th2_start
)
)
report_win7(
f" START ACTUAL -> "
f"S1:{math.degrees(th1_start):.2f}deg | "
f"S2:{math.degrees(th2_start):.2f}deg\r\n".encode(
"utf-8"
)
)
report_win7(
f" IK REBUILD -> "
f"S1:{math.degrees(check_th1):.2f}deg | "
f"S2:{math.degrees(check_th2):.2f}deg\r\n".encode(
"utf-8"
)
)
report_win7(
f" ERROR -> "
f"dS1:{delta_th1_deg:+.3f}deg | "
f"dS2:{delta_th2_deg:+.3f}deg\r\n".encode(
"utf-8"
)
)
if (
abs(delta_th1_deg) > 0.5
or abs(delta_th2_deg) > 0.5
):
report_win7(
b" [SELF-CHECK] FAILED: "
b"IK does not reproduce current geometry.\r\n"
)
audit_failed = True
else:
report_win7(
b" [SELF-CHECK] PASSED: "
b"IK reproduces current geometry.\r\n"
)
# --------------------------------------------------
# 3. TEST COMPLETE +50 MM HORIZONTAL WRIST ADVANCE
#
# V81 validates five 10 mm mathematical waypoints
# to establish a safe +50 mm final endpoint.
# Physical movement occurs only after the full audit passes.
#
# --------------------------------------------------
if not audit_failed:
report_win7(
b"--------------------------------------------------\r\n"
)
report_win7(
b" EVALUATING +50 MM (+X) ADVANCE "
b"AT FIXED WRIST ALTITUDE Z:\r\n"
)
report_win7(
b" WRIST COUNTER-ROTATION ENABLED "
b"(CALCULATION ONLY):\r\n"
)
dz_tolerance_mm = 1.0
# Existing program intentionally uses S3=2500
# in TUCK, so V81 permits the established
# 500..2500 physical wrist range.
WRIST_PWM_MIN = 500
WRIST_PWM_MAX = 2500
for step_i in range(1, 6):
req_x_c = (
x_c0 + step_i * 10.0
)
req_z_c = z_c0
req_reach_mm = math.hypot(
req_x_c,
req_z_c,
)
# V81 operational envelope check.
if (
req_reach_mm
> MAX_WRIST_REACH_MM + REACH_POLICY_TOL_MM
):
report_win7(
f"\r\n [CALC STEP "
f"{step_i}/5 "
f"(s=+{step_i}cm)] "
f"REJECTED: "
f"Outside Operational Reach Envelope "
f"(Radius:{req_reach_mm:.2f}mm, "
f"Limit:{MAX_WRIST_REACH_MM:.2f}mm)\r\n".encode(
"utf-8"
)
)
audit_failed = True
break
ik_th1, ik_th2 = (
solve_inverse_kinematics_2d(
req_x_c,
req_z_c,
)
)
if (
ik_th1 is None
or ik_th2 is None
):
report_win7(
f"\r\n [CALC STEP "
f"{step_i}/5 "
f"(s=+{step_i}cm)] "
f"REJECTED: "
f"Geometric Singularity / "
f"Out of Reach "
f"(Target X:"
f"{req_x_c:.1f}mm, "
f"Z:{req_z_c:.1f}mm)\r\n".encode(
"utf-8"
)
)
audit_failed = True
break
# Normalize shoulder angle into the
# physical positive representation.
ik_th1 = ik_th1 % (
2.0 * math.pi
)
# V82 WRIST COUNTER-ROTATION:
# Preserve the starting absolute tool orientation while Shoulder and
# Elbow change during L1.
#
# V82 world geometry:
#
# tool_world = shoulder - elbow + (wrist - 90 deg)
#
# Solving for wrist:
#
# wrist = tool_world - shoulder + elbow + 90 deg
#
# This sign convention must remain paired with the V82 Forward
# Kinematics relationship BC_world = shoulder - elbow.
# Counter-rotate wrist so Link 3 / tool
# maintains its original absolute angle.
ik_th3 = (
tool_angle_start
- ik_th1
+ ik_th2
+ math.pi / 2.0
)
s1_req_pwm = (
physical_radians_to_pwm(
1, ik_th1
)
)
s2_req_pwm = (
physical_radians_to_pwm(
2, ik_th2
)
)
s3_req_pwm = (
physical_radians_to_pwm(
3, ik_th3
)
)
(
(x_b_cur, z_b_cur),
(x_c_cur, z_c_cur),
(x_tip_cur, z_tip_cur),
) = compute_full_kinematics(
ik_th1,
ik_th2,
ik_th3,
)
dx_c = x_c_cur - x_c0
dz_c = z_c_cur - z_c0
dx_tip = x_tip_cur - x_tip0
dz_tip = z_tip_cur - z_tip0
# Verify requested geometry and
# servo envelopes.
if (
not (
SAFE_PWM_MIN
<= s1_req_pwm
<= SAFE_PWM_MAX
)
or not (
SAFE_PWM_MIN
<= s2_req_pwm
<= SAFE_PWM_MAX
)
or not (
WRIST_PWM_MIN
<= s3_req_pwm
<= WRIST_PWM_MAX
)
or abs(dz_c)
> dz_tolerance_mm
or abs(dz_tip)
> dz_tolerance_mm
):
report_win7(
f"\r\n [CALC STEP "
f"{step_i}/5 "
f"(s=+{step_i}cm)] "
f"REJECTED: "
f"Trajectory or PWM "
f"Boundary Breach!\r\n"
f" S1:{s1_req_pwm} | "
f"S2:{s2_req_pwm} | "
f"S3:{s3_req_pwm} | "
f"dZ(C):{dz_c:+.2f}mm | "
f"dZ(Tip):"
f"{dz_tip:+.2f}mm\r\n".encode(
"utf-8"
)
)
audit_failed = True
break
deg1 = math.degrees(
ik_th1
)
deg2 = math.degrees(
ik_th2
)
deg3 = math.degrees(
ik_th3
)
report_win7(
f"\r\n [CALC STEP "
f"{step_i}/5 "
f"(s=+{step_i}cm)] "
f"VALID\r\n".encode(
"utf-8"
)
)
report_win7(
f" ANGLES -> "
f"S1:{deg1:.2f}deg | "
f"S2:{deg2:.2f}deg | "
f"S3:{deg3:.2f}deg\r\n".encode(
"utf-8"
)
)
report_win7(
f" PWMs -> "
f"S1:{s1_req_pwm} | "
f"S2:{s2_req_pwm} | "
f"S3:{s3_req_pwm}\r\n".encode(
"utf-8"
)
)
report_win7(
f" Pivot B -> "
f"X:{x_b_cur:+7.2f} mm | "
f"Z:{z_b_cur:+7.2f} mm\r\n".encode(
"utf-8"
)
)
report_win7(
f" Pivot C -> "
f"X:{x_c_cur:+7.2f} mm | "
f"Z:{z_c_cur:+7.2f} mm | "
f"dX:{dx_c:+6.2f} mm | "
f"dZ:{dz_c:+6.2f} mm\r\n".encode(
"utf-8"
)
)
report_win7(
f" Tool T -> "
f"X:{x_tip_cur:+7.2f} mm | "
f"Z:{z_tip_cur:+7.2f} mm | "
f"dX:{dx_tip:+6.2f} mm | "
f"dZ:{dz_tip:+6.2f} mm\r\n".encode(
"utf-8"
)
)
# ----------------------------------------------
# V82 FINAL PIVOT-C ALTITUDE AUDIT
#
# The fifth mathematical waypoint is the full
# +50 mm L1 endpoint. Report its Z altitude
# separately and conspicuously before any
# physical command can be authorized.
# ----------------------------------------------
if not audit_failed:
final_z_error = z_c_cur - l1_target_z
final_x_error = x_c_cur - l1_target_x
report_win7(
b"\r\n"
b"==================================================\r\n"
)
report_win7(
b" V82 FINAL L1 ALTITUDE AUDIT\r\n"
)
report_win7(
b"==================================================\r\n"
)
report_win7(
f" C STARTING Z -> "
f"{z_c0:+.3f} mm\r\n".encode("utf-8")
)
report_win7(
f" C REQUESTED Z -> "
f"{l1_target_z:+.3f} mm\r\n".encode(
"utf-8"
)
)
report_win7(
f" C CALCULATED FINAL Z -> "
f"{z_c_cur:+.3f} mm\r\n".encode(
"utf-8"
)
)
report_win7(
f" C CALCULATED DELTA-Z -> "
f"{dz_c:+.3f} mm\r\n".encode("utf-8")
)
report_win7(
f" C FINAL Z ERROR -> "
f"{final_z_error:+.6f} mm\r\n".encode(
"utf-8"
)
)
report_win7(
b"--------------------------------------------------\r\n"
)
report_win7(
f" C REQUESTED DELTA-X -> "
f"+50.000 mm\r\n"
f" C CALCULATED DELTA-X -> "
f"{dx_c:+.3f} mm\r\n"
f" C FINAL X ERROR -> "
f"{final_x_error:+.6f} mm\r\n".encode(
"utf-8"
)
)
if abs(final_z_error) <= dz_tolerance_mm:
report_win7(
b" [ALTITUDE AUDIT] PASSED: "
b"Calculated Pivot C altitude "
b"is maintained.\r\n"
)
else:
report_win7(
b" [ALTITUDE AUDIT] FAILED: "
b"Calculated Pivot C altitude "
b"is not maintained.\r\n"
)
audit_failed = True
report_win7(
b"==================================================\r\n"
)
# ------------------------------------------
# 4. V83 SSC-32U GROUP-MOVE PATH PREDICTION
#
# The five Cartesian waypoints above validate the
# requested +50 mm endpoint. V81 now asks a
# different question:
#
# If SSC-32U moves directly from the starting
# S1/S2/S3 pulse widths to the final +50 mm pulse
# widths as one synchronized group move, what
# Cartesian path would Pivot C and Tool T follow?
#
# NO COMMAND IS SENT DURING THIS PREDICTION PHASE.
# --------------------------------------------------
if not audit_failed:
report_win7(
b"\r\n"
b"==================================================\r\n"
)
report_win7(
b" V83 SSC-32U GROUP-MOVE PATH PREDICTION\r\n"
)
report_win7(
b" PATH PREDICTION ONLY - LIVE MOVE PENDING FINAL AUDIT\r\n"
)
report_win7(
b"==================================================\r\n"
)
report_win7(
f" START PHYSICAL PWM -> "
f"S1:{s1_phys_pwm} | "
f"S2:{s2_phys_pwm} | "
f"S3:{s3_phys_pwm}\r\n".encode(
"utf-8"
)
)
report_win7(
f" FINAL PHYSICAL PWM -> "
f"S1:{s1_req_pwm} | "
f"S2:{s2_req_pwm} | "
f"S3:{s3_req_pwm}\r\n".encode(
"utf-8"
)
)
report_win7(
b"\r\n"
b" Percent |"
b" X(C) |"
b" Z(C) |"
b" dZ(C) |"
b" dZ(Tip)\r\n"
)
report_win7(
b"---------+-----------+-----------+-----------+----------\r\n"
)
max_positive_dz_c = 0.0
max_negative_dz_c = 0.0
max_abs_dz_c = 0.0
max_abs_dz_tip = 0.0
# 21 samples: 0%, 5%, 10% ... 100%.
for sample_i in range(21):
fraction = sample_i / 20.0
percent = sample_i * 5
# Approximate SSC-32U synchronized group
# interpolation by linearly interpolating
# each physical servo pulse width between
# the common start and finish positions.
sim_s1_pwm = (
s1_phys_pwm
+ fraction
* (
s1_req_pwm
- s1_phys_pwm
)
)
sim_s2_pwm = (
s2_phys_pwm
+ fraction
* (
s2_req_pwm
- s2_phys_pwm
)
)
sim_s3_pwm = (
s3_phys_pwm
+ fraction
* (
s3_req_pwm
- s3_phys_pwm
)
)
sim_th1 = pwm_to_physical_radians(
1, sim_s1_pwm
)
sim_th2 = pwm_to_physical_radians(
2, sim_s2_pwm
)
sim_th3 = pwm_to_physical_radians(
3, sim_s3_pwm
)
(
(sim_x_b, sim_z_b),
(sim_x_c, sim_z_c),
(sim_x_tip, sim_z_tip),
) = compute_full_kinematics(
sim_th1,
sim_th2,
sim_th3,
)
sim_dz_c = sim_z_c - z_c0
sim_dz_tip = (
sim_z_tip - z_tip0
)
max_positive_dz_c = max(
max_positive_dz_c,
sim_dz_c,
)
max_negative_dz_c = min(
max_negative_dz_c,
sim_dz_c,
)
max_abs_dz_c = max(
max_abs_dz_c,
abs(sim_dz_c),
)
max_abs_dz_tip = max(
max_abs_dz_tip,
abs(sim_dz_tip),
)
report_win7(
f" {percent:3d}% | "
f"{sim_x_c:+8.2f} | "
f"{sim_z_c:+8.2f} | "
f"{sim_dz_c:+8.2f} | "
f"{sim_dz_tip:+8.2f}\r\n".encode(
"utf-8"
)
)
report_win7(
b"--------------------------------------------------\r\n"
)
report_win7(
f" PREDICTED PIVOT C Z RANGE -> "
f"{max_negative_dz_c:+.2f} mm to "
f"{max_positive_dz_c:+.2f} mm\r\n".encode(
"utf-8"
)
)
report_win7(
f" MAXIMUM ABS PIVOT C Z EXCURSION -> "
f"{max_abs_dz_c:.2f} mm\r\n".encode(
"utf-8"
)
)
report_win7(
f" MAXIMUM ABS TOOL-TIP Z EXCURSION -> "
f"{max_abs_dz_tip:.2f} mm\r\n".encode(
"utf-8"
)
)
report_win7(
b" [V83 AUDIT] "
b"All calculations complete; live command pending.\r\n"
)
report_win7(
b" [V83 AUDIT] "
b"PATH PREDICTION COMPLETE - LIVE COMMAND NOT YET SENT.\r\n"
)
# V81 code added after for oop and inside if not audit failed
# --------------------------------------------------
# 5. FINAL V81 AUDIT RESULT
# --------------------------------------------------
if audit_failed:
cokoino.write(
b"ERROR:OUT_OF_BOUNDS\n"
)
report_win7(
b"\r\n[OPERATOR CARD] -> "
b"Step 12 V83 GROUP-MOVE Audit "
b"REJECTED "
b"(Boundary Alarm Sent to Cokoino).\r\n\r\n"
)
else:
report_win7(
b"\r\n[OPERATOR CARD] -> "
b"Step 12 V83 GROUP-MOVE AUDIT PASSED.\r\n"
)
report_win7(
b"+50 mm endpoint is mathematically valid "
b"and the predicted SSC-32U path has been "
b"reported above.\r\n"
)
# ----------------------------------------------
# V81 LIVE +50 MM GROUP MOVE
#
# All geometry, operational-envelope, PWM,
# constant-Z, and predicted-path checks have
# passed before execution reaches this point.
#
# s1_req_pwm, s2_req_pwm, and s3_req_pwm are
# PHYSICAL PWM values. Send them directly.
# Do NOT apply trim again.
# ----------------------------------------------
motion_packet = (
f"#1P{s1_req_pwm}"
f"#2P{s2_req_pwm}"
f"#3P{s3_req_pwm}"
f"T{TRANSIT_TIME_MS}\r"
)
report_win7(
b"\r\n==================================================\r\n"
)
report_win7(
b" V83 LIVE L1 GROUP MOVE AUTHORIZED\r\n"
)
report_win7(
b"==================================================\r\n"
)
report_win7(
f" [TX -> LYNXMOTION]: "
f"{motion_packet.strip()}\r\n".encode(
"utf-8"
)
)
lynx.write(
motion_packet.encode("utf-8")
)
# Convert the physical endpoint back into the
# program's nominal/reference PWM system.
# READY_TARGET remains unchanged so L2 can
# return the arm to the stored READY position.
current_arm_positions[1] = get_nominal_pwm(
1, s1_req_pwm
)
current_arm_positions[2] = get_nominal_pwm(
2, s2_req_pwm
)
current_arm_positions[3] = get_nominal_pwm(
3, s3_req_pwm
)
report_win7(
b"[V83 LIVE] One synchronized +50 mm "
b"group-move command has been sent.\r\n"
)
report_win7(
b"[V83 LIVE] No Q or VER query will be "
b"sent during the movement.\r\n\r\n"
)
# --- OTHER SEQUENCED BUTTON EXECUTIONS ---
# STEP 2: CROSS -> Query LynxMotion VER
if "CROSS" in cmd_upper:
if "RELEASED" in cmd_upper:
if last_processed_command == "CROSS":
last_processed_command = ""
elif last_processed_command != "CROSS":
last_processed_command = "CROSS"
system_state = 2
send_led_binary_pattern(cokoino, 2)
lynx.write(b"VER\r")
report_win7(b" [TX -> LYNXMOTION]: VER\r\n")
time.sleep(0.1)
ver_response = ""
if lynx.in_waiting > 0:
ver_response = (
lynx.readline()
.decode("utf-8", errors="ignore")
.strip()
)
if ver_response:
report_win7(
f" [LYNXMOTION -> BRDG]:"
f" {ver_response}\r\n".encode("utf-8")
)
report_win7(
b"[OPERATOR CARD] -> Step 2: Firmware"
b" Version Verified Successfully.\r\n\r\n"
)
else:
report_win7(
b"[OPERATOR CARD] -> Step 2: VER Query"
b" Sent (No direct response"
b" received).\r\n\r\n"
)
# STEP 3: CIRCLE -> Travel to TUCK Position
elif "CIRCLE" in cmd_upper:
if "RELEASED" in cmd_upper:
if last_processed_command == "CIRCLE":
last_processed_command = ""
elif last_processed_command != "CIRCLE":
last_processed_command = "CIRCLE"
system_state = 3
send_led_binary_pattern(cokoino, 3)
current_arm_positions = list(TUCK_TARGET)
macro_packet = build_calibrated_macro_packet(
TUCK_TARGET, TRANSIT_TIME_MS
)
lynx.write(macro_packet.encode("utf-8"))
report_win7(
f" [TX -> LYNXMOTION]:"
f" {macro_packet.strip()}\r\n".encode("utf-8")
)
report_win7(
b"[OPERATOR CARD] -> Step 3: Traveling cleanly"
b" to TUCK configuration.\r\n\r\n"
)
# STEP 4: TRIANGLE -> Travel to HOME Configuration
elif "TRIANGLE" in cmd_upper:
if "RELEASED" in cmd_upper:
if last_processed_command == "TRIANGLE":
last_processed_command = ""
elif last_processed_command != "TRIANGLE":
last_processed_command = "TRIANGLE"
system_state = 4
send_led_binary_pattern(cokoino, 4)
current_arm_positions = list(HOME_TARGET)
macro_packet = build_calibrated_macro_packet(
HOME_TARGET, TRANSIT_TIME_MS
)
lynx.write(macro_packet.encode("utf-8"))
report_win7(
f" [TX -> LYNXMOTION]:"
f" {macro_packet.strip()}\r\n".encode("utf-8")
)
report_win7(
b"[OPERATOR CARD] -> Step 4: Traveling cleanly"
b" to HOME configuration.\r\n\r\n"
)
# STEP 5: SQUARE -> Travel to READY Configuration
elif "SQUARE" in cmd_upper:
if "RELEASED" in cmd_upper:
if last_processed_command == "SQUARE":
last_processed_command = ""
elif last_processed_command != "SQUARE":
last_processed_command = "SQUARE"
system_state = 5
send_led_binary_pattern(cokoino, 5)
current_arm_positions = list(READY_TARGET)
macro_packet = build_calibrated_macro_packet(
READY_TARGET, TRANSIT_TIME_MS
)
lynx.write(macro_packet.encode("utf-8"))
report_win7(
f" [TX -> LYNXMOTION]:"
f" {macro_packet.strip()}\r\n".encode("utf-8")
)
report_win7(
b"[OPERATOR CARD] -> Step 5: Traveling to READY"
b" configuration.\r\n\r\n"
)
time.sleep(0.01)
except KeyboardInterrupt:
print("\nBridge safely terminated.")
except Exception as e:
print(f"\nFatal Runtime Intercept: {e}")(th)
Online
Like button can go here