マクロVBAコードです。(修正しました)
安全のためマクロ無しにしたシートダウンロード WalkExcel_1.2x.xls (268.0K)
(ご自分で以下のマクロをコピペしてください)
Public q1, q2, q3, q4, q5
Sub main()
Worksheets("設定シート").Select
Pi = 3.141592
Deg = 180 / Pi
G = 9.8
'----------モーション時間設定-----------
nc = Int(Range("d04") / 2) 'モーション数(一歩の半分)
Ts = Range("d03") / 2 '一歩の半分
'-----------ロボット設定------------
Hc = Range("e19") / 1000 '重心高さ
kh = Range("e24") '立脚相腰高さ(腰下げ)の設定
Lx = Range("b28") / 1000 'ピッチ側の関節間隔
Ly = Range("j28") / 1000 'ロール側の関節間隔
St = Range("f9") / 2000 '歩幅(半分)
Sw = Range("c9") / 2000 '歩隔(半分)= 両足の間隔(半分)
Sw_offset = Sw '重心～足のオフセット = 歩隔(半分)
Uh = Range("e34") * St * 2 '遊脚相足上げ高さの設定
'-----------------------------------
Tc = Sqr(Hc / G)
C = HCos(Ts / Tc)
S = HSin(Ts / Tc)
Vx = St / (Tc * S) '歩速
Zx = Sqr(Lx * Lx - St * St) * kh '立脚高さピッチ側
Zy = Ly - (Lx - Zx) '立脚高さロール側
'計算用
Dim a(24)
Dim b(24)
For n = 0 To nc
a(n) = n / nc
b(n) = (nc - n) / nc
Next n
'作画用足部
Ax1 = Range("d64") / Deg
Ax2 = Range("d65") / Deg
Ax3 = Range("e64") / 1000 / Hc
Ax4 = Range("e65") / 1000 / Hc
Ay1 = Range("i64") / Deg
Ay2 = Range("i65") / Deg
Ay3 = Range("j64") / 1000 / Hc
Ay4 = Range("j65") / 1000 / Hc
'-----------------モーション作成----------------------------
Worksheets("作成データ").Select
Range("A5:U250").ClearContents
Tmp = 2.356
For stc = 1 To 4
fp = stc And 1 '前半・後半フラグ
fl = Int(stc / 2) And 1 '左足・右足フラグ
fb = Int(stc * 1.5) And 1 '作画用
fc = 1 - fb '作画用
'データ転記開始行－１
If stc < 3 Then offcell = 4 Else offcell = 5 + nc * 2
For n = 1 To nc
'------------位置の計算-----------------------------
If fp = 1 Then
t = Ts / Tc * a(n)
'立脚
Px = Vx * Tc * HSin(t)
Py = Sw / C * HCos(t)
'游脚
Ux = Sin(a(n) * Tmp) / Sin(Tmp) * -St
Uy = -Py
Uref = Sin((a(n) / 2 + 0.5) ^ 0.75 * Pi)
toe = Arcsin(Uref * Uh / 0.12) * a(n) '踵で着地
Else
'立脚
t = -Ts / Tc * b(n)
Px = Vx * Tc * HSin(t)
Py = Sw / C * HCos(t)
'游脚
Ux = Sin(b(n) * Tmp) / Sin(Tmp) * St
Uy = -Py
Uref = Sin((a(n) / 2) ^ 0.75 * Pi)
toe = -Arcsin(Uref * Uh / 0.12) * b(n) '爪先で蹴り
End If
dcell = offcell + n + nc * (1 - fp)
Cells(dcell, "f") = n + nc * (stc - 1)
Cells(dcell, "g") = Ts * (a(n) + stc - 1)
'------------立脚相の関節角度-----------------------
Pl = Sqr(Zx ^ 2 + Px ^ 2)
c1 = Arccos(Pl / Lx)
cf = -Atn(Px / Zx)
q1 = cf + c1
q2 = c1 * 2
q3 = c1 - cf
q4 = Atn((Py - Sw_offset) / Zy)
q5 = q4
q6 = Px / Zx
If fl = 0 Then
'RFoot
dcell = offcell + n + nc * fc
Cells(dcell, "j") = q4 * Deg
Cells(dcell, "l") = q1 * Deg
Cells(dcell, "n") = q2 * Deg
Cells(dcell, "p") = q3 * Deg
Cells(dcell, "r") = q5 * Deg
Cells(dcell, "t") = q6 * Deg
'散布図データ
scell = nc * 6 * (3 * fc)
x = Plot(n, scell, q1, q2, q3, -q4, q5, Hc / Lx, Hc / Ly, -Ay1, -Ay2, Ay3, Ay4, -Sw / Hc, Ax1, Ax2, Ax3, Ax4)
End If
If fl = 1 Then
'LFoot
dcell = offcell + n + nc * fb
Cells(dcell, "k") = -q4 * Deg
Cells(dcell, "m") = q1 * Deg
Cells(dcell, "o") = q2 * Deg
Cells(dcell, "q") = q3 * Deg
Cells(dcell, "s") = -q5 * Deg
Cells(dcell, "u") = q6 * Deg
'散布図データ
scell = nc * 6 * (3 * fb)
x = Plot(n, scell, q1, q2, q3, q4, -q5, Hc / Lx, Hc / Ly, Ay1, Ay2, Ay3, Ay4, Sw / Hc, Ax1, Ax2, Ax3, Ax4)
End If
'------------游脚相の関節角度-----------------------
Zu = Zx - Uref * Uh
Ul = Sqr(Zu ^ 2 + Ux ^ 2)
c1 = Arccos(Ul / Lx)
cf = -Atn(Ux / Zu)
q1 = cf + c1
q2 = c1 * 2
q3 = c1 - cf + toe
q4 = -Atn((Uy + Sw_offset) / (Zy - Uref * Uh))
q5 = q4 * 0.9
q6 = -Px / Zx
If fl = 0 Then
'LFoot
dcell = offcell + n + nc * fc
Cells(dcell, "k") = q4 * Deg
Cells(dcell, "m") = q1 * Deg
Cells(dcell, "o") = q2 * Deg
Cells(dcell, "q") = q3 * Deg
Cells(dcell, "s") = q5 * Deg
Cells(dcell, "u") = q6 * Deg
'散布図データ
scell = nc * 6 * (1 + fb)
x = Plot(n, scell, q1, q2, q3, -q4, q5, Hc / Lx, Hc / Ly, Ay1, Ay2, Ay3, Ay4, Sw / Hc, Ax1, Ax2, Ax3, Ax4)
End If
If fl = 1 Then
'RFoot
dcell = offcell + n + nc * fb
Cells(dcell, "j") = -q4 * Deg
Cells(dcell, "l") = q1 * Deg
Cells(dcell, "n") = q2 * Deg
Cells(dcell, "p") = q3 * Deg
Cells(dcell, "r") = -q5 * Deg
Cells(dcell, "t") = q6 * Deg
'散布図データ
scell = nc * 6 * (1 + fc)
x = Plot(n, scell, q1, q2, q3, q4, -q5, Hc / Lx, Hc / Ly, -Ay1, -Ay2, Ay3, Ay4, -Sw / Hc, Ax1, Ax2, Ax3, Ax4)
End If
Next n
Next stc
Worksheets("設定シート").Select
End Sub
'作画用データ
Function Plot(n, scell, q1, q2, q3, q4, q5, kx, ky, Ay1, Ay2, Ay3, Ay4, xy, Ax1, Ax2, Ax3, Ax4)
rad = 0.0174533
x1 = Sin(q1) / kx / 2
y1 = Cos(q1) / kx / 2
x2 = x1 + Sin(q1 - q2) / kx / 2
y2 = y1 + Cos(q1 - q2) / kx / 2
x3 = x2 + Sin(q1 - q2 + q3 + Ax1) * Ax3
y3 = y2 + Cos(q1 - q2 + q3 + Ax1) * Ax3
x4 = x3 + Sin(q1 - q2 + q3 + Ax2) * Ax4
y4 = y3 + Cos(q1 - q2 + q3 + Ax2) * Ax4
Cells(scell + n * 6 + 0, "a") = 0
Cells(scell + n * 6 + 0, "b") = 0
Cells(scell + n * 6 + 1, "a") = x1
Cells(scell + n * 6 + 1, "b") = -y1
Cells(scell + n * 6 + 2, "a") = x2
Cells(scell + n * 6 + 2, "b") = -y2
Cells(scell + n * 6 + 3, "a") = x3
Cells(scell + n * 6 + 3, "b") = -y3
Cells(scell + n * 6 + 4, "a") = x4
Cells(scell + n * 6 + 4, "b") = -y4
x1 = xy
y1 = 0
x2 = x1 + Sin(q4) / ky
y2 = y1 + Cos(q4) / ky
x3 = x2 + Sin(q4 + q5 + Ay1) * Ay3
y3 = y2 + Cos(q4 + q5 + Ay1) * Ay3
x4 = x2 + Sin(q4 + q5 + Ay2) * Ay4
y4 = y2 + Cos(q4 + q5 + Ay2) * Ay4
Cells(scell + n * 6 + 0, "c") = 0
Cells(scell + n * 6 + 0, "d") = 0
Cells(scell + n * 6 + 1, "c") = x1
Cells(scell + n * 6 + 1, "d") = -y1
Cells(scell + n * 6 + 2, "c") = x2
Cells(scell + n * 6 + 2, "d") = -y2
Cells(scell + n * 6 + 3, "c") = x3
Cells(scell + n * 6 + 3, "d") = -y3
Cells(scell + n * 6 + 4, "c") = x4
Cells(scell + n * 6 + 4, "d") = -y4
Plot = 1
End Function
'
Function HSin(x)
HSin = (Exp(x) - Exp(-x)) / 2
End Function
'
Function HCos(x)
HCos = (Exp(x) + Exp(-x)) / 2
End Function
'
Public Function Arcsin(x)
Arcsin = Atn(x / Sqr(-x * x + 1))
End Function
'
Public Function Arccos(x)
If x < 1 Then
Arccos = Atn(-x / Sqr(-x * x + 1)) + 2 * Atn(1)
Else
Arccos = 0
End If
End Function
最近のコメント