« エクセルで歩行モーションの作成 | トップページ | エクセルで歩行モーション作り »

2016年12月26日 (月)

エクセルで歩行モーションを作るためのVBAソースコード

マクロ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                    '作画用
        'データ転記開始行-1
        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

« エクセルで歩行モーションの作成 | トップページ | エクセルで歩行モーション作り »

コメント

こんにちは。こちらのプログラムを膝ダブルサーボのロボットに適用するには理論式のどこを変更したらよろしいでしょうか...?また、理論で参考にした文献など教えて頂けるとありがたいです。もしよろしければご返答お願いします。

こんにちは、膝ダブルサーボへの適用ですが、脚の関節角度は位置Px,長さLx,高さZxから求めています。
みっちーさんの http://dream-drive.net が参考になります。
参考にした文献はたくさんあって・・・足位置の計算は「ヒューマノイドロボット」の4章かな・・・。

コメントを書く

(ウェブ上には掲載しません)

« エクセルで歩行モーションの作成 | トップページ | エクセルで歩行モーション作り »