import hiwonder
import time
# from utils.PixelToRealworld.predefinedconstants import *
from utils.pixel_convert import pixel2robot

jetmax = hiwonder.JetMax()
sucker = hiwonder.Sucker()

jetmax.go_home(1)
time.sleep(1)

ort_point = (0, -162.94, 212.8 - 28)

new_point = ort_point
new_point = (new_point[0] , new_point[1] - 80, new_point[2] - 130)

ele_point = (-110, -50.94, 75)

# px, py = input("请输入像素坐标：").split()
# px, py = int(px), int(py)
# target = pixel2robot(877, 628)
# print(target)

# jetmax.set_position((target[0], target[1], -30), 1)
# jetmax.set_position((-75.41654829, -156.86319459, 70), 1)
jetmax.set_position(ele_point, 1)
time.sleep(1)

print(jetmax.position)

def move_to_pixel_position(x, y):
    target = pixel2robot(x, y)
    print(target)
    jetmax.set_position((target[0], target[1], -30), 1)

if __name__ == "__main__":
    pix_pos = input("分别输入 pixspy 上测得的横坐标和纵坐标，用空格分开：")
    print(pix_pos)
    x, y = pix_pos.split()
    move_to_pixel_position(int(x), int(y))