Cleanup helper scripts. Closes #7

2026-07-08 20:35:59 +02:00
parent 75ce38daf7
commit 1a94ca3947
11 changed files with 3 additions and 1 deletions
@@ -0,0 +1,41 @@
+import math
+
+d2 = 12.9
+l1 = 85.0
+l2 = 110.0
+
+t1_deg = 474 / 2048.0 * 360.0
+t2_deg = 530 / 2048.0 * 360.0
+
+th1 = math.radians(t1_deg)
+th2 = math.radians(t2_deg)
+
+e1x = -d2 + l1 * math.cos(th1)
+e1y = l1 * math.sin(th1)
+
+e2x = d2 + l1 * math.cos(th2)
+e2y = l1 * math.sin(th2)
+
+dx = e2x - e1x
+dy = e2y - e1y
+dist = math.sqrt(dx*dx + dy*dy)
+
+a = dist / 2
+h = math.sqrt(l2*l2 - a*a)
+
+mx = (e1x + e2x) / 2
+my = (e1y + e2y) / 2
+
+px1 = mx + h * (dy / dist)
+py1 = my + h * (-dx / dist)
+
+px2 = mx - h * (dy / dist)
+py2 = my - h * (-dx / dist)
+
+ex = px1 if py1 >= py2 else px2
+ey = py1 if py1 >= py2 else py2
+
+with open("output.txt", "w") as f:
+    f.write(f"End Effector: {ex:.1f}, {ey:.1f}\n")
+    f.write(f"Elbow 1: {e1x:.1f}, {e1y:.1f}\n")
+    f.write(f"Elbow 2: {e2x:.1f}, {e2y:.1f}\n")