import numpy as np
from calculate.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

hd = np.pi / 180
u1 = 120 * hd
u4 = 60 * hd

omega1 = 500
omega4 = 500
alpha1 = 0
alpha4 = 0

# 计算正向运动学
xc, yc, u2, u3, omega2, omega3, alpha2, alpha3 = forwardF(u1, u4, omega1, omega4, l1, l2, l3, l4, l5, alpha1, alpha4)

# 存储中间变量
theta2 = [u2]
theta3 = [u3]
omega2_list = [omega2]
omega3_list = [omega3]
alpha2_list = [alpha2]
alpha3_list = [alpha3]
xcd = [xc]
ycd = [yc]

# 绘图部分
fig = plt.figure(figsize=(8, 8))
ax = fig.add_subplot(111)
ax.set_title('并联SCARA')
ax.set_xlabel('mm')
ax.set_ylabel('mm')
ax.set_xlim(-200, 600)
ax.set_ylim(-200, 600)
ax.grid(True)

x = [0] * 5
y = [0] * 5

# 基础点
x[0] = 0
y[0] = 0

# 第一个连杆末端
x[1] = l1 * np.cos(u1)
y[1] = l1 * np.sin(u1)

# 末端执行器位置
x[2] = xcd[0]
y[2] = ycd[0]

# 第四个连杆末端
x[3] = l4 * np.cos(u4) + l5
y[3] = l4 * np.sin(u4)

# 第五个点（第二个电机位置）
x[4] = l5
y[4] = 0

# 绘制结构线和关键点
ax.plot(x, y, 'k-', linewidth=2)
ax.plot(x[0], y[0], 'or')  # 基础点
ax.plot(x[1], y[1], 'or')  # 第一连杆末端
ax.plot(x[2], y[2], 'og')  # 末端执行器
ax.plot(x[3], y[3], 'om')  # 第四连杆末端
ax.plot(x[4], y[4], 'oc')  # 第二个电机

plt.show()