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)
Offline
Like button can go here
Version 85 of the Python bridge program we've been working on is the first of a series to convert the program from "bench only" operation to bench or field.
This installment introduces the ability to operate in the field without a monitor, keyboard or mouse, or with all three in "bench" mode.
The signal to select "bench" mode is to press the space bar during boot. If no space is detected then the program will assume field mode.
# bridgeV85.py Prepared by ChatGPT Supervised by Tom Hanson
# ==============================================================================
# Version 85: AUTOMATIC USB DEVICE IDENTIFICATION + FIELD/BENCH MODE SELECTION
#
# PURPOSE:
# V85 begins the transition from bench operation toward stand-alone field use.
# The proven V84 robot-control, safety, kinematic, READY, and L1/L2 motion code
# remains unchanged. V85 modifies startup and USB Port Configuration only.
#
# OPERATING MODE SELECTION:
# At program startup, V85 provides a short, non-blocking keyboard detection
# window.
#
# SPACE detected -> BENCH MODE
# No SPACE -> FIELD MODE
#
# FIELD MODE is the default so that future stand-alone operation will not
# require a monitor, keyboard, mouse, or Windows 7 reporting computer.
#
# BENCH MODE retains operator-visible startup information and the optional
# Windows 7 / HyperTerminal reporting capability used during development.
#
# AUTOMATIC USB DEVICE IDENTIFICATION:
# V85 no longer identifies the two required robot controllers by USB port
# number, connection order, or USB converter type.
#
# Each required device must positively identify itself:
#
# Cokoino:
# Bridge -> "WHOAREYOU"
# Expected response begins with:
# "COKOINO CKK0017"
#
# LynxMotion SSC-32U:
# Bridge -> "VER"
# Expected response begins with:
# "SSC32-"
#
# Only positively identified devices may be assigned as robot controllers.
# Unrecognized USB serial devices are ignored.
#
# STARTUP SAFETY POLICY:
# Both the Cokoino controller and LynxMotion SSC-32U are required for normal
# robot operation.
#
# If both are positively identified:
# Startup may continue into the proven V84 operational code.
#
# If Cokoino is identified but LynxMotion is not:
# Use the established Cokoino red lockout/error indication and terminate.
# This condition may occur if the separately powered LynxMotion controller
# has not been powered on.
#
# If Cokoino is not identified:
# Do not attempt robot operation. In FIELD MODE there may be no available
# operator display, so terminate safely without requiring user input.
#
# If identification is ambiguous:
# Do not guess. Terminate safely.
#
# No startup identification failure may result in a robot motion command.
#
# FUTURE FIELD DIAGNOSTICS:
# A later update may add timestamped startup diagnostic logs with limited file
# retention. Logging is deliberately NOT introduced in V85 so this version
# remains focused on mode selection and positive USB device identification.
#
# DESIGN PRINCIPLE:
#
# POSITIVE IDENTIFICATION OR NO OPERATION.
#
# ==============================================================================
# Version 84: FIELD USB PORT-COUNT FIX
# Retains V83 centralized optional Windows 7 reporting.
# N/X mode permits two robot USB ports: Cokoino + LynxMotion.
# 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 select
import sys
import termios
import tty
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)
# --- V85 FIELD / BENCH MODE SELECTION ---
BENCH_MODE_WINDOW_SECONDS = 5.0
def select_operating_mode():
"""Select BENCH with SPACE; otherwise default safely to FIELD.
The startup window is deliberately non-blocking. If no keyboard is
available, or standard input is not an interactive terminal, V85
automatically selects FIELD mode.
"""
print()
print("========================================")
print(" COKOINO / LYNXMOTION BRIDGE V85")
print(" Press SPACE now for BENCH MODE")
print(" Default after 5 seconds: FIELD MODE")
print("========================================")
# Future headless operation may have no keyboard terminal at all.
if not sys.stdin.isatty():
print("[MODE] No interactive keyboard detected.")
print("[MODE] FIELD")
return "FIELD"
old_terminal_settings = termios.tcgetattr(sys.stdin)
try:
# cbreak permits individual keystrokes to be detected immediately
# without waiting for ENTER.
tty.setcbreak(sys.stdin.fileno())
deadline = time.monotonic() + BENCH_MODE_WINDOW_SECONDS
while time.monotonic() < deadline:
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
if readable:
key = sys.stdin.read(1)
if key == " ":
print()
print("[MODE] SPACE detected.")
print("[MODE] BENCH")
return "BENCH"
# All other keystrokes are deliberately ignored.
finally:
# Always restore normal terminal behavior before continuing.
termios.tcsetattr(
sys.stdin,
termios.TCSADRAIN,
old_terminal_settings,
)
print()
print("[MODE] Startup window expired.")
print("[MODE] FIELD")
return "FIELD"
operating_mode = select_operating_mode()
# --- 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"}
# --------------------------------------------------------------
# WINDOWS 7 REPORTING ENABLED
#
# Preserve the familiar bench procedure:
# 1. Operator identifies Windows 7.
# 2. Operator identifies Cokoino.
# 3. The one remaining USB port is assumed to be LynxMotion.
# --------------------------------------------------------------
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
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 cokoino_idx == win7_idx:
print("\n[CONFLICT DETECTED] Re-enter assignments.\n")
continue
used_indices = {win7_idx, cokoino_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]
# --------------------------------------------------------------
# WINDOWS 7 REPORTING DISABLED -- V84 FIELD TEST MODE
#
# N or X means Windows 7 is logically absent. Other USB serial
# devices may still be physically connected, so do NOT infer
# LynxMotion from the number of remaining ports.
#
# Until automatic device identification is implemented, the
# operator explicitly identifies both required robot devices.
# --------------------------------------------------------------
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
lynx_idx = int(
input("Enter index number for LYNXMOTION (SSC-32U): ")
)
if lynx_idx < 0 or lynx_idx >= len(ports):
print("[INVALID LYNXMOTION INDEX] Try again.\n")
continue
if lynx_idx == cokoino_idx:
print(
"\n[CONFLICT DETECTED] Cokoino and LynxMotion "
"cannot use the same port. Re-enter assignments.\n"
)
continue
# --------------------------------------------------------------
# ASSIGN THE SELECTED DEVICE PATHS
# --------------------------------------------------------------
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 V84...\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 (V84 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" V84 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. V84 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" V84 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" [V84 AUDIT] "
b"All calculations complete; live command pending.\r\n"
)
report_win7(
b" [V84 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 V84 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 V84 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" V84 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"[V84 LIVE] One synchronized +50 mm "
b"group-move command has been sent.\r\n"
)
report_win7(
b"[V84 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)
Offline
Like button can go here
This Python program was created by Gemini in support of the Business Calendar for Mars.
from datetime import datetime, timedelta
# Constants for Mars Timekeeping
SOL_IN_EARTH_SECONDS = 88775.244 # 24h 39m 35.244s
SOL_IN_DAYS = SOL_IN_EARTH_SECONDS / 86400.0
# MY 39 Epoch (MTC Midnight anchor for Ls = 0°)
# Mars Year 39 begins on September 30, 2026 (approx 09:12 UTC)
MY39_EPOCH_UTC = datetime(2026, 9, 30, 9, 12, 0)
def generate_mars_calendar_sol(sol_number):
"""
Computes the Earth UTC start and end bounds for a given Mars Business Sol.
"""
# Offset from Sol 1 in Earth seconds
seconds_offset_start = (sol_number - 1) * SOL_IN_EARTH_SECONDS
seconds_offset_end = sol_number * SOL_IN_EARTH_SECONDS
sol_start_utc = MY39_EPOCH_UTC + timedelta(seconds=seconds_offset_start)
sol_end_utc = MY39_EPOCH_UTC + timedelta(seconds=seconds_offset_end)
# Calculate Earth date overlap percentages
day1_str = sol_start_utc.strftime("%Y-%m-%d")
# Midnight boundary of the primary Earth day
next_earth_midnight = datetime(sol_start_utc.year, sol_start_utc.month, sol_start_utc.day) + timedelta(days=1)
seconds_in_day1 = (next_earth_midnight - sol_start_utc).total_seconds()
percent_day1 = min(100.0, max(0.0, (seconds_in_day1 / SOL_IN_EARTH_SECONDS) * 100))
percent_day2 = 100.0 - percent_day1
return {
"Sol": sol_number,
"MTC_Start_UTC": sol_start_utc.strftime("%Y-%m-%d %H:%M:%S UTC"),
"MTC_End_UTC": sol_end_utc.strftime("%Y-%m-%d %H:%M:%S UTC"),
"Primary_Earth_Date": day1_str,
"Overlap_Split": f"{percent_day1:.1f}% / {percent_day2:.1f}%"
}
# Generate First 5 Sols of Mars Year 39
print(f"{'Sol':<6} | {'MTC Start (Earth UTC)':<22} | {'MTC End (Earth UTC)':<22} | {'Primary Earth Date':<18} | {'Overlap Split'}")
print("-" * 88)
for s in range(1, 6):
data = generate_mars_calendar_sol(s)
print(f"Sol {data['Sol']:<2} | {data['MTC_Start_UTC']} | {data['MTC_End_UTC']} | {data['Primary_Earth_Date']:<18} | {data['Overlap_Split']}")This is a first attempt to see if we might be able to calculate the numbers that would go into a printed calendar for the coming Mars year 39.
I am thinking of using images of the sky above Mars, but since they do not exist at present, Gemini suggested using images created by planetarium software. The images would be of the sky above I Airy Crater which (I just learned) is accepted as the site of Longitude 0 on Mars, based upon Astronomical history.
(th)
Offline
Like button can go here