def test5():
for i in range(10):
x = randint(50, 120)
y = randint(50, 120)
p3 = Point2D(x, y)
cross(trans_point(p3), WHITE)
alfa, beta = invkin2(p3)
print(x, y, alfa, beta)
arm_angles(alfa, beta)
def move_servo2(p1, p2, delay=5):
steps = move_2d_line(p1, p2)
for step in steps:
alfa, beta = invkin2(step, L1, L2)
print(step, alfa, beta)
s1.set_degree(alfa)
s2.set_degree(beta)
sleep_ms(delay)
def test1():
angles = [(0, 0), (60, 0), (90, 0), (150, 0), (0, 45), (60, 45), (90, 45),
(150, 45), (0, 90), (60, 90), (90, 90), (150, 90)]
for angle in angles:
alfa = angle[0]
beta = angle[1]
bod2 = arm_angles(alfa, beta)
print(alfa, beta, bod2.x, bod2.y)
alfa, beta = invkin2(p2)
sleep(1)
def test6():
points = [(10, 10), (30, 30), (50, 100), (75, 100), (50, 50), (75, 75),
(100, 50), (100, 75), (90, 90)]
for p in points:
p2 = Point2D(p[0], p[1])
cross(trans_point(p2), WHITE)
alfa, beta = invkin2(p2)
print(p[0], p[1], alfa, beta)
arm_angles(alfa, beta)
sleep(1)
def test7():
for j in range(6):
for i in range(6):
x = j * 15
y = i * 15
p2 = Point2D(x, y)
cross(trans_point(p2), WHITE)
alfa, beta = invkin2(p2)
print(x, y, alfa, beta)
arm_angles(alfa, beta)
sleep(0.5)
def run_sqare():
"""
p1 = 0, 0 # strart point
p2 = 50, 50 # stop point
move_servo2(p1, p2)
"""
move_servo2((xa, ya), (xa, ya + aa))
move_servo2((xa, ya + aa), (xa + aa, ya + aa))
move_servo2((xa + aa, ya + aa), (xa + aa, ya))
move_servo2((xa + aa, ya), (xa, ya))
for _ in range(12):
run_sqare()
while True:
y = int(input("x? "))
x = int(input("y? "))
point2d = x, y
alfa, beta = invkin2(point2d, L1, L2)
print(alfa, beta)
s1.set_degree(alfa)
s2.set_degree(beta)
print("-" * 12)