import numpy as np

from fk import forwardF
import matplotlib.pyplot as plt

# 设置中文字体和解决负号显示问题
plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong']  # 按优先级选择字体
plt.rcParams['axes.unicode_minus'] = False  # 显示负号 -


# 杆长参数
l1 = 50
l2 = 50
l3 = 50
l4 = 50
l5 = 50

# 初始角度（弧度）
u1_base = np.deg2rad(120)  # 左侧电机初始角
u4_base = np.deg2rad(120)  # 右侧电机初始角

# 设置绘图区域
plt.figure(figsize=(8, 8))
ax = plt.gca()
ax.set_xlim(-100, l5 + 100)
ax.set_ylim(-100, 100)
ax.set_aspect('equal')
ax.grid(True)
ax.set_title("五连杆机构运动仿真（调用FK函数）")

for i in range(1, 61):
    # 更新两个驱动臂的角度
    angle1 = u1_base - np.deg2rad(1.75 * i)   # 左侧角度变化
    angle4 = u4_base + np.deg2rad(1.75 * i)   # 右侧角度变化

    #正向运动学函数获取末端位置和中间角度
    result = forwardF(angle1, angle4, 0, 0, l1, l2, l3, l4, l5, 0, 0)
    xc, yc, u2, u3 = result[:4]

    # 构建各点坐标
    x = [0, l1*np.cos(angle1), xc, l4*np.cos(angle4)+l5, l5]
    y = [0, l1*np.sin(angle1), yc, l4*np.sin(angle4), 0]

    # 清除上一帧并绘制新图形
    ax.cla()
    ax.set_xlim(-100, l5 + 100)
    ax.set_ylim(-100, 100)
    ax.set_aspect('equal')
    ax.grid(True)
    ax.set_title("五连杆机构运动仿真（调用FK函数）")

    # 绘制结构线和关键点
    ax.plot(x, y, 'r-o', linewidth=2, markersize=6, markerfacecolor='red')
    ax.plot(x[0], y[0], 'go')   # 原点
    ax.plot(x[1], y[1], 'bo')   # 第二个点
    ax.plot(x[2], y[2], 'mo')   # 中间点
    ax.plot(x[3], y[3], 'co')   # 第四个点
    ax.plot(x[4], y[4], 'yo')   # 最后一个点

    plt.pause(0.1)

plt.close()