def moveToEuclidean(position, time):
lengths = (1.75, 4, 5, 0.5)
angles = Finesse.inverse_pack(lengths, position)
if angles is not None:
servo1.set_target(angles[0])
servo2.set_target(angles[1])
servo3.set_target(angles[2])
maestro.end_together(servos, update=True, t=time)
else:
print('Unable to reach position (%s, %s, %s)!' % position)
goHome()
return t1, t2, t3
def invkinematics(lengths, angles):
t1, t2, t3 = angles
l1, l2, l3 = lengths
i1 = (l2 * cos(t2)) + (l3 * cos(t2 + t3))
x = (i1 + l1) * cos(t1)
y = (i1 + l1) * sin(t1)
z = (l2 * sin(t2)) + (l3 * sin(t2 + t3))
return x, y, z
usb = serial.Serial("COM5")
lengths = (1.75, 4, 5, 0.5)
x, y, z = (4, -4, -5)
t = 0
k = 5
r = 3
a = 4
while True:
target = (x + r * sin(k * t), y, z + (cos(k * t) / abs(cos(k * t))) * (1 - sin(k * t) ** a) ** (1 / a))
t1, t2, t3 = Finesse.inverse_pack(lengths, target)
# print(t1, t2, t3)
data = struct.pack("<fff", t1, t2, t3)
usb.write(data)
time.sleep(0.05)
t += 0.05
