added epc placement
This commit is contained in:
Salijoghli
parent
de309a613d
commit
da78207b9b
104
add_devices.py
104
add_devices.py
@ -8,7 +8,7 @@ import re
|
||||
# -----------------------
|
||||
SCALE = 0.0254
|
||||
FIXED_Y = 2.4
|
||||
BUTTON_Y = 2.3
|
||||
BUTTON_Y = 2.2
|
||||
CONVEYOR_WIDTH = 1.524
|
||||
EDGE_CLEARANCE = 0.45
|
||||
BEAM_RANGE_ADJUSTMENT = 0.25
|
||||
@ -41,6 +41,7 @@ BUTTON_RES_ID = "auto_button"
|
||||
tpe_devices = []
|
||||
btn_devices = []
|
||||
ss_devices = []
|
||||
epc_devices = []
|
||||
|
||||
with open(CSV_PATH, newline="", encoding="utf-8") as f:
|
||||
reader = csv.DictReader(f)
|
||||
@ -52,6 +53,8 @@ with open(CSV_PATH, newline="", encoding="utf-8") as f:
|
||||
btn_devices.append(row)
|
||||
elif rt == "SS":
|
||||
ss_devices.append(row)
|
||||
elif rt == "EPC":
|
||||
epc_devices.append(row)
|
||||
|
||||
if not tpe_devices:
|
||||
raise RuntimeError("No TPE records found in CSV.")
|
||||
@ -74,6 +77,12 @@ for d in ss_devices:
|
||||
if key:
|
||||
ss_by_conveyor.setdefault(key, []).append(d)
|
||||
|
||||
epc_by_conveyor = {}
|
||||
for d in epc_devices:
|
||||
key = (d.get("conveyor_key") or "").strip()
|
||||
if key:
|
||||
epc_by_conveyor.setdefault(key, []).append(d)
|
||||
|
||||
# -----------------------
|
||||
# SELECT SCENE
|
||||
# -----------------------
|
||||
@ -98,14 +107,16 @@ print("[1] All")
|
||||
print("[2] Sensors only")
|
||||
print("[3] Buttons only (S)")
|
||||
print("[4] SS only")
|
||||
print("[5] EPC only")
|
||||
|
||||
device_choice = int(input("\nSelect option: "))
|
||||
if device_choice < 1 or device_choice > 4:
|
||||
if device_choice < 1 or device_choice > 5:
|
||||
raise RuntimeError("Invalid selection.")
|
||||
|
||||
place_sensors = device_choice in (1, 2)
|
||||
place_buttons = device_choice in (1, 3)
|
||||
place_ss = device_choice in (1, 4)
|
||||
place_epc = device_choice in (1, 5)
|
||||
|
||||
# -----------------------
|
||||
# READ SCENE
|
||||
@ -177,7 +188,7 @@ if place_sensors and SENSOR_SCENE_PATH not in scene_text:
|
||||
f'[ext_resource type="PackedScene" path="{SENSOR_SCENE_PATH}" id="{SENSOR_RES_ID}"]'
|
||||
)
|
||||
|
||||
if (place_buttons or place_ss) and BUTTON_SCENE_PATH not in scene_text:
|
||||
if (place_buttons or place_ss or place_epc) and BUTTON_SCENE_PATH not in scene_text:
|
||||
idx = max(i for i, l in enumerate(lines) if l.startswith("[ext_resource"))
|
||||
lines.insert(idx + 1,
|
||||
f'[ext_resource type="PackedScene" path="{BUTTON_SCENE_PATH}" id="{BUTTON_RES_ID}"]'
|
||||
@ -510,6 +521,93 @@ if place_ss:
|
||||
f'pushbutton_tag_name = "{station}_STPB_OIP"\n'
|
||||
)
|
||||
|
||||
# -------- EPC DEVICES --------
|
||||
if place_epc:
|
||||
for key in sorted(epc_by_conveyor.keys()):
|
||||
devs = epc_by_conveyor[key]
|
||||
|
||||
if key not in conveyors:
|
||||
print(f"⚠ Conveyor not found for EPC: {key}")
|
||||
continue
|
||||
|
||||
info = conveyors[key]
|
||||
xx, xz, zx, zz = info["basis"]
|
||||
width = info["width"]
|
||||
|
||||
yaw = yaw_from_x_axis(xx, xz) if info["has_size"] else yaw_from_z_axis(zx, zz)
|
||||
|
||||
for i, d in enumerate(devs):
|
||||
name = d.get("dev_name") or f"{key}_EPC{i+1}"
|
||||
|
||||
csv_x = float(d["dev_x"]) * SCALE
|
||||
csv_z = -float(d["dev_y"]) * SCALE
|
||||
|
||||
# Same side logic as S buttons
|
||||
x, z = project_to_conveyor(info, yaw, csv_x, csv_z)
|
||||
side = side_of_conveyor(info, yaw, csv_x, csv_z)
|
||||
|
||||
side_sign = 1.0 if side >= 0 else -1.0
|
||||
|
||||
px = math.cos(yaw - math.pi / 2)
|
||||
pz = math.sin(yaw - math.pi / 2)
|
||||
|
||||
EPC_CLEARANCE = 0.1
|
||||
d_off = (width / 2) + EPC_CLEARANCE
|
||||
x += px * d_off * side_sign
|
||||
z += pz * d_off * side_sign
|
||||
|
||||
if side_sign >= 0:
|
||||
base_yaw = yaw + math.pi
|
||||
else:
|
||||
base_yaw = yaw
|
||||
|
||||
# Spread along conveyor direction (like SS buttons do)
|
||||
EPC_SPREAD = 0.35
|
||||
fx = math.cos(yaw)
|
||||
fz = math.sin(yaw)
|
||||
|
||||
c, s = math.cos(base_yaw), math.sin(base_yaw)
|
||||
|
||||
# -------- CH1 --------
|
||||
ch1_name = f"{name}_CH1"
|
||||
ch1_x = x + fx * (EPC_SPREAD / 2)
|
||||
ch1_z = z + fz * (EPC_SPREAD / 2)
|
||||
|
||||
ch1_transform = (
|
||||
f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},"
|
||||
f"{ch1_x:.6f},{BUTTON_Y:.6f},{ch1_z:.6f})"
|
||||
)
|
||||
|
||||
node_blocks.append(
|
||||
f'\n[node name="{ch1_name}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n'
|
||||
f"transform = {ch1_transform}\n"
|
||||
f'text = "EPC"\n'
|
||||
f"button_color = Color(1, 0.15, 0.15, 1)\n"
|
||||
f"normally_closed = true\n"
|
||||
f"enable_comms = true\n"
|
||||
f'pushbutton_tag_name = "{name}_CH1_OIP"\n'
|
||||
)
|
||||
|
||||
# -------- CH2 --------
|
||||
ch2_name = f"{name}_CH2"
|
||||
ch2_x = x - fx * (EPC_SPREAD / 2)
|
||||
ch2_z = z - fz * (EPC_SPREAD / 2)
|
||||
|
||||
ch2_transform = (
|
||||
f"Transform3D({c:.6f},0,{-s:.6f},0,1,0,{s:.6f},0,{c:.6f},"
|
||||
f"{ch2_x:.6f},{BUTTON_Y:.6f},{ch2_z:.6f})"
|
||||
)
|
||||
|
||||
node_blocks.append(
|
||||
f'\n[node name="{ch2_name}" parent="." instance=ExtResource("{BUTTON_RES_ID}")]\n'
|
||||
f"transform = {ch2_transform}\n"
|
||||
f'text = "EPC"\n'
|
||||
f"button_color = Color(1, 0.15, 0.15, 1)\n"
|
||||
f"normally_closed = true\n"
|
||||
f"enable_comms = true\n"
|
||||
f'pushbutton_tag_name = "{name}_CH2_OIP"\n'
|
||||
)
|
||||
|
||||
|
||||
scene_text += "".join(node_blocks)
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user