import time

# 假设jetmax是机械臂的控制模块
import hiwonder


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


import sys
import tty
import termios
import time

# 假设jetmax是机械臂的控制模块
# from your_jetmax_library import jetmax

# 设置初始位置
position = [-100, -265.94, 80]
step = 5  # 每次移动的步长
time_to_move = 0.1  # 运动时间

# 控制机械臂的位置函数
def set_position(position, time_to_move):
    print(f"Setting position to {position} in {time_to_move} seconds")
    # 在这里调用机械臂的接口
    jetmax.set_position(tuple(position), time_to_move)

# 获取单个字符输入的函数
def getch():
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(sys.stdin.fileno())
        ch = sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    return ch

print("Use WASD to move in the XY plane, R and F to move along Z. Press 'q' to quit.")

try:
    while True:
        char = getch()
        if char == 'w':
            position[1] += step  # Y 轴增加
        elif char == 's':
            position[1] -= step  # Y 轴减少
        elif char == 'a':
            position[0] -= step  # X 轴减少
        elif char == 'd':
            position[0] += step  # X 轴增加
        elif char == 'r':
            position[2] += step  # Z 轴增加
        elif char == 'f':
            position[2] -= step  # Z 轴减少
        elif char == 'q':
            break

        set_position(position, time_to_move)
        time.sleep(0.1)

except KeyboardInterrupt:
    print("Program interrupted.")
finally:
    print("Program exited.")