m = [1.0, 1.0, 1.0, 1.0] # 質量
ll = [0.3, 0.3, 0.3, 0.3] # リンク長さ
lg = [ll[0] / 2, ll[1] / 2, ll[2] / 2, ll[3] / 2] # 重心位置
D = 0.1 # リンク粘性
g = 9.8 # 重力加速度
# Prametas of motor
Mm = 34.7 * pow(10, -7) # モータの慣性モーメント
B = 0.0 # モータの粘性
# 慣性モーメント
Inertia = []
Inertia = sl.link_inertia(m, ll, Inertia)
# ゲイン調整
control_gain1 = sl.imput_gain(10.0, 0.004, 5.0)
control_gain2 = sl.imput_gain(10.0, 0.004, 5.0)
control_gain3 = sl.imput_gain(0.0, 0.0, 0.0)
control_gain4 = sl.imput_gain(10.0, 0.004, 5.0)
gain = [control_gain1, control_gain2, control_gain3, control_gain4]
# Link data
q = [radians(45), radians(90), radians(45), radians(90)] # 初期角度
dot_q = [0.0, 0.0, 0.0, 0.0]
ddot_q = [0.0, 0.0, 0.0, 0.0]
sum_q = [0.0, 0.0, 0.0, 0.0]
# Desired Parametas
qd = [radians(0), radians(0), radians(0), radians(0)] # 目標角度
dot_qd = [0.0, 0.0, 0.0, 0.0]
# Gearratio
N = 50
# Prametas of motor
Mm = 34.7 * pow(10, -7) # モータの慣性モーメント
B = 0.0037255872020394314 # モータの粘性
# 慣性モーメント
Inertia = []
Inertia = sl.link_inertia(m, ll, Inertia)
arm1_Inertia = [Inertia[0], Inertia[1]]
arm2_Inertia = [Inertia[2], Inertia[3]]
# ゲイン調整
control_gain1 = sl.imput_gain(0, 0.00, 0.000)
control_gain2 = sl.imput_gain(0, 0.00, 0.000)
control_gain3 = sl.imput_gain(0, 0.00, 0.000)
control_gain4 = sl.imput_gain(0.0, 0.0, 0.0)
gain1 = [control_gain1, control_gain2, control_gain3, control_gain4]
control_gain1 = sl.imput_gain(0, 0.00, 0)
control_gain2 = sl.imput_gain(0, 0.00, 0)
control_gain3 = sl.imput_gain(0, 0.00, 0)
control_gain4 = sl.imput_gain(0.0, 0.0, 0.0)
gain2 = [control_gain1, control_gain2, control_gain3, control_gain4]
# 初期姿勢
x10 = 0.0
y10 = 0.4