import numpy as np
from arm.arm_directions import ArmDirections
from arm.move_arm import move_arm
import arm.Arm
default_arm_positions = [np.pi / 2, np.pi / 2, 0]
## create the robot arm
arm = Arm.Arm3Link(q=default_arm_positions, L=np.array([90, 60, 48]))
print('the old angles are:')
print(arm.q)
print(arm.get_xy())
old_angles = default_arm_positions
# x, y = -100, 80
x, y = -70, 0
arm.q = arm.inv_kin([x, y])
new_angles = arm.q
print('new angle is:')
print(arm.q)
print(arm.get_xy())
move_arm(old_angles[0] - new_angles[0], ArmDirections.up(),
ArmDirections.down())
