PROFERIS - Mi³osz Stocki
42 lines
780 B
Python
42 lines
780 B
Python
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")
|