from calculate.calculate_angle import main_of_5dof

# control_with_serial.py


# 📡 初始化串口连接
def init_serial(port=SERIAL_PORT, baud_rate=BAUD_RATE):
    try:
        ser = serial.Serial(port, baud_rate, timeout=TIMEOUT)
        print(f"串口已连接: {port} @ {baud_rate}")
        return ser
    except Exception as e:
        print(f"无法打开串口 {port}: {e}")
        return None

# 📤 发送角度到串口
def send_angles(ser, theta1_deg, theta4_deg):
    # 格式：theta1_deg,theta4_deg\n
    data = f"{int(theta1_deg)},{int(theta4_deg)}\n"
    try:
        ser.write(data.encode())
        print(f"已发送角度: theta1 = {theta1_deg}°, theta4 = {theta4_deg}°")
    except Exception as e:
        print(f"发送失败: {e}")

# 🚀 主程序入口
if __name__ == "__main__":
    # 调用你的轨迹规划函数
    angles_list = main_of_5dof(
        trajectory_type='line',
        start=(0, 0),
        end=(200, 300),
        show_animation=False
    )

    # 初始化串口
    ser = init_serial()
    if ser is None:
        exit(1)

    # 遍历角度列表并发送
    for i, (theta1, theta4) in enumerate(angles_list):
        theta1_deg = np.degrees(theta1)
        theta4_deg = np.degrees(theta4)

        print(f"第 {i} 帧角度: theta1 = {theta1_deg:.2f}°, theta4 = {theta4_deg:.2f}°")
        send_angles(ser, theta1_deg, theta4_deg)

        # 可选：每帧之间加延迟，控制发送速度
        import time
        time.sleep(0.05)  # 每帧间隔 50ms

    # 关闭串口
    ser.close()
    print("串口已关闭")