import os
import sys
import time

sys.path.append(os.path.join(os.path.dirname(__file__), '../../..'))

from xarm.wrapper import XArmAPI

arm = XArmAPI('192.168.1.135')
arm.motion_enable(enable=True)
arm.set_mode(0)
arm.set_state(state=0)
arm.set_tcp_jerk(10000)
arm.set_joint_jerk(500,is_radian=True)
arm.save_conf ()

arm.reset(wait=True)
speed1 = 1000
mvacc1=50000

# arm.set_pause_time(1)
while True:
    arm.set_position(x=400, y=-100, z=250, roll=180, pitch=0, yaw=0, radius=50, speed=speed1, mvacc=mvacc1, wait=False)
    print(arm.get_position(), arm.get_position(is_radian=True))
    arm.set_position(x=400, y=100, z=250, roll=180, pitch=0, yaw=0, radius=50, speed=speed1, mvacc=mvacc1, wait=False)
    print(arm.get_position(), arm.get_position(is_radian=True))
    arm.set_position(x=300, y=0, z=250, roll=-180, pitch=0, yaw=0, radius=50, speed=speed1, mvacc=mvacc1, wait=False)
    print(arm.get_position(), arm.get_position(is_radian=True))