import math
from ik import inverseF
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 = 250
l2 = 300
l3 = 300
l4 = 250
l5 = 250
x = 125
#y = 382.338
y = 300
omega1 = omega4 = 500
alpha1 = alpha4 = 0

# 逆解
u1, u4 = inverseF(x, y, l1, l2, l3, l4, l5)

# 正解验证
xc, yc, u2, u3, omega, alpha, _, _ = forwardF(u1, u4, omega1, omega4, l1, l2, l3, l4, l5, alpha1, alpha4)

# 左侧链路
x1, y1 = 0, 0
x2 = l1 * math.cos(u1)
y2 = l1 * math.sin(u1)

# 右侧链路
x5, y5 = l5, 0
x4 = l4 * math.cos(u4)+l5 # 注意方向
y4 = l4 * math.sin(u4)

# 绘图
plt.figure(figsize=(8, 8))

# 左侧链路
plt.plot([x1, x2, xc], [y1, y2, yc], 'b-o', label='左侧链路')

# 右侧链路
plt.plot([x5, x4, xc], [y5, y4, yc], 'r-o', label='右侧链路')

# 标记关键点
plt.plot(x1, y1, 'ro')  # O1
plt.plot(x5, y5, 'go')  # O2
plt.plot(x2, y2, 'yo')  # B
plt.plot(x4, y4, 'mo')  # D
plt.plot(xc, yc, 'ko', markersize=10)  # C（末端）

# 设置图形
plt.grid(True)
plt.axis('equal')
plt.xlim([-200, l5 + 200])
plt.ylim([-200, 600])
plt.title('SCARA 五连杆逆解结构图')
plt.xlabel('X (mm)')
plt.ylabel('Y (mm)')
plt.legend()
plt.show()