import phantomController as pc
import time

def main() -> None:
    ser = pc.connect()
    w = [130, 120, 18, 0, 0, -1];
    print(pc.move_pose(ser, w, 0, 2000));
    print("moving to pose")
    time.sleep(2000);
    print("done")
    #pc.set_joint_angles(ser,1.07, 0.21, -0.15, -1.6, -1, 0, 2000)
    pc.sleep(ser)
    pc.disconnect(ser)


if __name__ == "__main__":
    main()