import serial
import time
from robot_driver import RobotDriver



# === 主程序逻辑 ===
if __name__ == "__main__":
    # 请修改端口号
    robot = RobotDriver(port='COM9')

    print("\n=== 全能控制面板 ===")
    print(" [1] 变构型: Mode 1 (30, 150)")
    print(" [2] 变构型: Mode 2 (120, 60)")
    print(" [0] 变构型: Reset (90, 90)")
    print(" [o] 直线电机: 张开")
    print(" [c] 直线电机: 闭合")
    print(" [s] 直线电机: 停止")
    print(" [q] 退出")

    try:
        while True:
            cmd = input("指令 > ").strip().lower()

            if cmd == 'q':
                break

            # 直线电机
            elif cmd == 'o':
                robot.motor_open()
            elif cmd == 'c':
                robot.motor_close()
            elif cmd == 's':
                robot.motor_stop()

            # 舵机构型
            elif cmd == '1':
                robot.set_config(1)
            elif cmd == '2':
                robot.set_config(2)
            elif cmd == '0':
                robot.set_config(0)

    except KeyboardInterrupt:
        pass
    finally:
        robot.close()
        print("System Shutdown.")