1. プログラム！
Dim PI As Variant, Ox As Variant, Oy As Variant, Z(3) As Variant, θ As Variant
Sub JansenLinkage()
'NOBのArduino日記！4脚Z軸回転(20170905作成)
'入力要求
Dim buf(2) As Variant
buf(0) = 720 '動作角度を入力
buf(1) = 3 '足の数-1を入力
buf(2) = 10 'ステップ角度を入力
'定数の宣言
Const a As Variant = 77.2, b As Variant = 79.1, c As Variant = 77.2, d As Variant = 77.2, e As Variant = 110#, f As Variant = 77.2, g As Variant = 71.4, h As Variant = 127.4, i As Variant = 92.7, j As Variant = 98.5, k As Variant = 121.6, l As Variant = 15.4, m As Variant = 29#
PI = 3.14159: Ox = 263: Oy = 215: Z(0) = -60: Z(1) = -20: Z(2) = 20: Z(3) = 60:
'変数の宣言
Dim Ax(20) As Variant, Bx(20) As Variant, Cx(20) As Variant, Dx(20) As Variant, Ex(20) As Variant, Fx(20) As Variant, Gx(20) As Variant, Ay(20) As Variant, By(20) As Variant, Cy(20) As Variant, Dy(20) As Variant, Ey(20) As Variant, Fy(20) As Variant, Gy(20) As Variant, Cθ(20) As Variant, Eθ(20) As Variant, Fθ(20) As Variant, Gθ(20) As Variant, Cxc(20) As Variant, Dxc(20) As Variant, Exc(20) As Variant, Fxc(20) As Variant, Gxc(20) As Variant, Cyc(20) As Variant, Dyc(20) As Variant, Eyc(20) As Variant, Fyc(20) As Variant, Gyc(20) As Variant, AB(20) As Variant, DE(20) As Variant
'処理
For θ = 0 To buf(0) Step buf(2)
'オートシェイプ削除
For Each ee In ActiveSheet.Shapes
ActiveSheet.Shapes(1).Delete
Next
'足描画
For n = 0 To buf(1)
Dim ARZ As Single: ARZ = Application.RoundUp(n Mod 2, 0) * 2
'座標A計算
Ax(n) = Ox + m * Cos(Radians(θ + 360 / (buf(1) + 1) * n))
Ay(n) = Oy + m * Sin(Radians(θ + 360 / (buf(1) + 1) * n))
'座標B計算
Bx(n) = Ox - a + ARZ * a
By(n) = Oy + l
'座標C計算
AB(n) = ( (Ax(n) - Bx(n)) ^ 2 + (By(n) - Ay(n)) ^ 2) ^ 0.5
Cθ(n) = Atn( (Ay(n) - By(n)) / (Ax(n) - Bx(n)))
Cxc(n) = (AB(n) ^ 2 + b ^ 2 - j ^ 2) / (2 * AB(n))
Cyc(n) = (b ^ 2 - Cxc(n) ^ 2) ^ 0.5
Cx(n) = Bx(n) + Cxc(n) * Cos(Cθ(n)) + Cyc(n) * Cos(Cθ(n) - PI / 2) - ARZ * Cxc(n) * Cos(Cθ(n))
Cy(n) = By(n) + Cxc(n) * Sin(Cθ(n)) + Cyc(n) * Sin(Cθ(n) - PI / 2) - ARZ * Cxc(n) * Sin(Cθ(n))
'座標D計算
Dxc(n) = (AB(n) ^ 2 + c ^ 2 - k ^ 2) / (2 * AB(n))
Dyc(n) = (c ^ 2 - Dxc(n) ^ 2) ^ 0.5
Dx(n) = Bx(n) + Dxc(n) * Cos(Cθ(n)) + Dyc(n) * Cos(Cθ(n) + PI / 2) - ARZ * Dxc(n) * Cos(Cθ(n))
Dy(n) = By(n) + Dxc(n) * Sin(Cθ(n)) + Dyc(n) * Sin(Cθ(n) + PI / 2) - ARZ * Dxc(n) * Sin(Cθ(n))
'座標E計算
Eθ(n) = Acos( (Bx(n) - Cx(n)) / b) + PI
Exc(n) = (b ^ 2 + d ^ 2 - e ^ 2) / (2 * b)
Eyc(n) = (d ^ 2 - Exc(n) ^ 2) ^ 0.5
Ex(n) = Bx(n) + Exc(n) * Cos(Eθ(n)) + Eyc(n) * Cos(Eθ(n) - PI / 2) - ARZ * Eyc(n) * Cos(Eθ(n) - PI / 2)
Ey(n) = By(n) + Exc(n) * Sin(Eθ(n)) + Eyc(n) * Sin(Eθ(n) - PI / 2) - ARZ * Eyc(n) * Sin(Eθ(n) - PI / 2)
'座標F計算
DE(n) = ( (Dx(n) - Ex(n)) ^ 2 + (Dy(n) - Ey(n)) ^ 2) ^ 0.5
Fθ(n) = Atn( (Ey(n) - Dy(n)) / (Ex(n) - Dx(n)))
Fxc(n) = (DE(n) ^ 2 + g ^ 2 - f ^ 2) / (2 * DE(n))
Fyc(n) = Abs(g ^ 2 - Fxc(n) ^ 2) ^ 0.5
Fx(n) = Dx(n) - Fxc(n) * Cos(Fθ(n)) - Fyc(n) * Cos(Fθ(n) - PI / 2) + ARZ * Fxc(n) * Cos(Fθ(n))
Fy(n) = Dy(n) - Fxc(n) * Sin(Fθ(n)) - Fyc(n) * Sin(Fθ(n) - PI / 2) + ARZ * Fxc(n) * Sin(Fθ(n))
'座標G計算
Gθ(n) = Atn( (Fy(n) - Dy(n)) / (Fx(n) - Dx(n)))
Gxc(n) = (g ^ 2 + i ^ 2 - h ^ 2) / (2 * g)
Gyc(n) = (i ^ 2 - Gxc(n) ^ 2) ^ 0.5
Gx(n) = Dx(n) - Gxc(n) * Cos(Gθ(n)) - Gyc(n) * Cos(Gθ(n) - PI / 2) + ARZ * Gxc(n) * Cos(Gθ(n))
Gy(n) = Dy(n) - Gxc(n) * Sin(Gθ(n)) - Gyc(n) * Sin(Gθ(n) - PI / 2) + ARZ * Gxc(n) * Sin(Gθ(n))
Dim axis() As Variant, AOx As Variant, AOy As Variant, link As Variant, AOx1 As Variant, AOy1 As Variant, AOx2 As Variant, AOy2 As Variant
'リンク軸座標格納
axis = Array("A", "B", "C", "D", "E", "F", "G", "O")
AOx = Array(Ax(n), Bx(n), Cx(n), Dx(n), Ex(n), Fx(n), Gx(n), Ox)
AOy = Array(Ay(n), By(n), Cy(n), Dy(n), Ey(n), Fy(n), Gy(n), Oy)
'リンク座標格納 b c d e f g h i j k m
link = Array("b", "c", "d", "e", "f", "g", "h", "i", "j", "k", "m")
AOx1 = Array(Bx(n), Bx(n), Bx(n), Cx(n), Ex(n), Dx(n), Fx(n), Dx(n), Ax(n), Ax(n), Ox)
AOy1 = Array(By(n), By(n), By(n), Cy(n), Ey(n), Dy(n), Fy(n), Dy(n), Ay(n), Ay(n), Oy)
AOx2 = Array(Cx(n), Dx(n), Ex(n), Ex(n), Fx(n), Fx(n), Gx(n), Gx(n), Cx(n), Dx(n), Ax(n))
AOy2 = Array(Cy(n), Dy(n), Ey(n), Ey(n), Fy(n), Fy(n), Gy(n), Gy(n), Cy(n), Dy(n), Ay(n))
'b～mのリンク・記号を描画
For jj = 0 To 10
'リンク
With ActiveSheet.Shapes.AddLine(ZRotX(AOx1(jj), AOy1(jj)), ZRotY(AOx1(jj), AOy1(jj)), ZRotX(AOx2(jj), AOy2(jj)), ZRotY(AOx2(jj), AOy2(jj))).Line
.ForeColor.RGB = RGB(Abs(Application.RoundUp( ( (n + 4) Mod 4) / 10, 0) - 1) * 255, Abs(Application.RoundUp( ( (n + 4) Mod 6) / 10, 0) - 1) * 255, Abs(Application.RoundUp( ( (n + 4) Mod 5) / 10, 0) - 1) * 255)
.Weight = 0.75
End With
'リンク記号
ActiveSheet.Shapes.AddShape(msoShapeRectangle, (ZRotX(AOx1(jj), AOy1(jj)) + ZRotX(AOx2(jj), AOy2(jj))) / 2 - 9, (ZRotY(AOx1(jj), AOy1(jj)) + ZRotY(AOx2(jj), AOy2(jj))) / 2 - 8, 48.75, 43.5).Select
With Selection.ShapeRange
.Fill.Visible = msoFalse
.Line.Visible = msoFalse
.TextFrame2.TextRange.Characters.Text = link(jj)
.TextFrame2.TextRange.Characters.Font.Fill.ForeColor.ObjectThemeColor = msoThemeColorText1
.TextFrame2.TextRange.Characters.Font.Size = 8
End With
Next jj
'軸ポイント描画
With ActiveSheet.Shapes
.AddShape msoShapeOval, Ox - 4, ZRotY(Ox, Oy) - 5, 8, 8 'O
.AddShape msoShapeOval, ZRotX(Bx(n), By(n)) - 4, ZRotY(Bx(n), By(n)) - 5, 8, 8 'B
.AddShape msoShapeOval, ZRotX(Gx(n), Gy(n)) - 4, ZRotY(Gx(n), Gy(n)) - 5, 8, 8 'G
End With
'軸記号描画
For kk = 0 To 7
ActiveSheet.Shapes.AddShape(msoShapeRectangle, ZRotX(AOx(kk), AOy(kk)) - 9, ZRotY(AOx(kk), AOy(kk)) - 8, 48.75, 43.5).Select
With Selection.ShapeRange
.Fill.Visible = msoFalse
.Line.Visible = msoFalse
.TextFrame2.TextRange.Characters.Text = axis(kk)
.TextFrame2.TextRange.Characters.Font.Fill.ForeColor.ObjectThemeColor = msoThemeColorText1
.TextFrame2.TextRange.Characters.Font.Size = 8
End With
Next kk
Next n
Range("B2") = "θ=" & θ & "°"
Range("A24").Select
DoEvents
Next θ
End Sub
Function Radians(Degrees As Variant) As Variant
'角度からラジアンを求る
Radians = (PI / 180) * Degrees
End Function
Function Acos(x As Variant) As Variant
'アークコサインを求める
Select Case x
Case Is = -1
Acos = PI
Case Is = 1
Acos = 0
Case Else
Acos = PI / 2 - Atn(x / Sqr(1 - x ^ 2))
End Select
End Function
Function ZRotX(x As Variant, y As Variant) As Variant
'Z軸を中心としてθ回転させた時のX軸座標を計算する
ZRotX = Cos(Radians(θ / 2)) * (x - Ox) - Sin(Radians(θ / 2)) * (y - Oy) + Ox
End Function
Function ZRotY(x As Variant, y As Variant) As Variant
'Z軸を中心としてθ回転させた時のY軸座標を計算する
ZRotY = Sin(Radians(θ / 2)) * (x - Ox) + Cos(Radians(θ / 2)) * (y - Oy) + Oy
End Function
Sub JansenLinkage()
'NOBのArduino日記！4脚Z軸回転(20170905作成)
'入力要求
Dim buf(2) As Variant
buf(0) = 720 '動作角度を入力
buf(1) = 3 '足の数-1を入力
buf(2) = 10 'ステップ角度を入力
'定数の宣言
Const a As Variant = 77.2, b As Variant = 79.1, c As Variant = 77.2, d As Variant = 77.2, e As Variant = 110#, f As Variant = 77.2, g As Variant = 71.4, h As Variant = 127.4, i As Variant = 92.7, j As Variant = 98.5, k As Variant = 121.6, l As Variant = 15.4, m As Variant = 29#
PI = 3.14159: Ox = 263: Oy = 215: Z(0) = -60: Z(1) = -20: Z(2) = 20: Z(3) = 60:
'変数の宣言
Dim Ax(20) As Variant, Bx(20) As Variant, Cx(20) As Variant, Dx(20) As Variant, Ex(20) As Variant, Fx(20) As Variant, Gx(20) As Variant, Ay(20) As Variant, By(20) As Variant, Cy(20) As Variant, Dy(20) As Variant, Ey(20) As Variant, Fy(20) As Variant, Gy(20) As Variant, Cθ(20) As Variant, Eθ(20) As Variant, Fθ(20) As Variant, Gθ(20) As Variant, Cxc(20) As Variant, Dxc(20) As Variant, Exc(20) As Variant, Fxc(20) As Variant, Gxc(20) As Variant, Cyc(20) As Variant, Dyc(20) As Variant, Eyc(20) As Variant, Fyc(20) As Variant, Gyc(20) As Variant, AB(20) As Variant, DE(20) As Variant
'処理
For θ = 0 To buf(0) Step buf(2)
'オートシェイプ削除
For Each ee In ActiveSheet.Shapes
ActiveSheet.Shapes(1).Delete
Next
'足描画
For n = 0 To buf(1)
Dim ARZ As Single: ARZ = Application.RoundUp(n Mod 2, 0) * 2
'座標A計算
Ax(n) = Ox + m * Cos(Radians(θ + 360 / (buf(1) + 1) * n))
Ay(n) = Oy + m * Sin(Radians(θ + 360 / (buf(1) + 1) * n))
'座標B計算
Bx(n) = Ox - a + ARZ * a
By(n) = Oy + l
'座標C計算
AB(n) = ( (Ax(n) - Bx(n)) ^ 2 + (By(n) - Ay(n)) ^ 2) ^ 0.5
Cθ(n) = Atn( (Ay(n) - By(n)) / (Ax(n) - Bx(n)))
Cxc(n) = (AB(n) ^ 2 + b ^ 2 - j ^ 2) / (2 * AB(n))
Cyc(n) = (b ^ 2 - Cxc(n) ^ 2) ^ 0.5
Cx(n) = Bx(n) + Cxc(n) * Cos(Cθ(n)) + Cyc(n) * Cos(Cθ(n) - PI / 2) - ARZ * Cxc(n) * Cos(Cθ(n))
Cy(n) = By(n) + Cxc(n) * Sin(Cθ(n)) + Cyc(n) * Sin(Cθ(n) - PI / 2) - ARZ * Cxc(n) * Sin(Cθ(n))
'座標D計算
Dxc(n) = (AB(n) ^ 2 + c ^ 2 - k ^ 2) / (2 * AB(n))
Dyc(n) = (c ^ 2 - Dxc(n) ^ 2) ^ 0.5
Dx(n) = Bx(n) + Dxc(n) * Cos(Cθ(n)) + Dyc(n) * Cos(Cθ(n) + PI / 2) - ARZ * Dxc(n) * Cos(Cθ(n))
Dy(n) = By(n) + Dxc(n) * Sin(Cθ(n)) + Dyc(n) * Sin(Cθ(n) + PI / 2) - ARZ * Dxc(n) * Sin(Cθ(n))
'座標E計算
Eθ(n) = Acos( (Bx(n) - Cx(n)) / b) + PI
Exc(n) = (b ^ 2 + d ^ 2 - e ^ 2) / (2 * b)
Eyc(n) = (d ^ 2 - Exc(n) ^ 2) ^ 0.5
Ex(n) = Bx(n) + Exc(n) * Cos(Eθ(n)) + Eyc(n) * Cos(Eθ(n) - PI / 2) - ARZ * Eyc(n) * Cos(Eθ(n) - PI / 2)
Ey(n) = By(n) + Exc(n) * Sin(Eθ(n)) + Eyc(n) * Sin(Eθ(n) - PI / 2) - ARZ * Eyc(n) * Sin(Eθ(n) - PI / 2)
'座標F計算
DE(n) = ( (Dx(n) - Ex(n)) ^ 2 + (Dy(n) - Ey(n)) ^ 2) ^ 0.5
Fθ(n) = Atn( (Ey(n) - Dy(n)) / (Ex(n) - Dx(n)))
Fxc(n) = (DE(n) ^ 2 + g ^ 2 - f ^ 2) / (2 * DE(n))
Fyc(n) = Abs(g ^ 2 - Fxc(n) ^ 2) ^ 0.5
Fx(n) = Dx(n) - Fxc(n) * Cos(Fθ(n)) - Fyc(n) * Cos(Fθ(n) - PI / 2) + ARZ * Fxc(n) * Cos(Fθ(n))
Fy(n) = Dy(n) - Fxc(n) * Sin(Fθ(n)) - Fyc(n) * Sin(Fθ(n) - PI / 2) + ARZ * Fxc(n) * Sin(Fθ(n))
'座標G計算
Gθ(n) = Atn( (Fy(n) - Dy(n)) / (Fx(n) - Dx(n)))
Gxc(n) = (g ^ 2 + i ^ 2 - h ^ 2) / (2 * g)
Gyc(n) = (i ^ 2 - Gxc(n) ^ 2) ^ 0.5
Gx(n) = Dx(n) - Gxc(n) * Cos(Gθ(n)) - Gyc(n) * Cos(Gθ(n) - PI / 2) + ARZ * Gxc(n) * Cos(Gθ(n))
Gy(n) = Dy(n) - Gxc(n) * Sin(Gθ(n)) - Gyc(n) * Sin(Gθ(n) - PI / 2) + ARZ * Gxc(n) * Sin(Gθ(n))
Dim axis() As Variant, AOx As Variant, AOy As Variant, link As Variant, AOx1 As Variant, AOy1 As Variant, AOx2 As Variant, AOy2 As Variant
'リンク軸座標格納
axis = Array("A", "B", "C", "D", "E", "F", "G", "O")
AOx = Array(Ax(n), Bx(n), Cx(n), Dx(n), Ex(n), Fx(n), Gx(n), Ox)
AOy = Array(Ay(n), By(n), Cy(n), Dy(n), Ey(n), Fy(n), Gy(n), Oy)
'リンク座標格納 b c d e f g h i j k m
link = Array("b", "c", "d", "e", "f", "g", "h", "i", "j", "k", "m")
AOx1 = Array(Bx(n), Bx(n), Bx(n), Cx(n), Ex(n), Dx(n), Fx(n), Dx(n), Ax(n), Ax(n), Ox)
AOy1 = Array(By(n), By(n), By(n), Cy(n), Ey(n), Dy(n), Fy(n), Dy(n), Ay(n), Ay(n), Oy)
AOx2 = Array(Cx(n), Dx(n), Ex(n), Ex(n), Fx(n), Fx(n), Gx(n), Gx(n), Cx(n), Dx(n), Ax(n))
AOy2 = Array(Cy(n), Dy(n), Ey(n), Ey(n), Fy(n), Fy(n), Gy(n), Gy(n), Cy(n), Dy(n), Ay(n))
'b～mのリンク・記号を描画
For jj = 0 To 10
'リンク
With ActiveSheet.Shapes.AddLine(ZRotX(AOx1(jj), AOy1(jj)), ZRotY(AOx1(jj), AOy1(jj)), ZRotX(AOx2(jj), AOy2(jj)), ZRotY(AOx2(jj), AOy2(jj))).Line
.ForeColor.RGB = RGB(Abs(Application.RoundUp( ( (n + 4) Mod 4) / 10, 0) - 1) * 255, Abs(Application.RoundUp( ( (n + 4) Mod 6) / 10, 0) - 1) * 255, Abs(Application.RoundUp( ( (n + 4) Mod 5) / 10, 0) - 1) * 255)
.Weight = 0.75
End With
'リンク記号
ActiveSheet.Shapes.AddShape(msoShapeRectangle, (ZRotX(AOx1(jj), AOy1(jj)) + ZRotX(AOx2(jj), AOy2(jj))) / 2 - 9, (ZRotY(AOx1(jj), AOy1(jj)) + ZRotY(AOx2(jj), AOy2(jj))) / 2 - 8, 48.75, 43.5).Select
With Selection.ShapeRange
.Fill.Visible = msoFalse
.Line.Visible = msoFalse
.TextFrame2.TextRange.Characters.Text = link(jj)
.TextFrame2.TextRange.Characters.Font.Fill.ForeColor.ObjectThemeColor = msoThemeColorText1
.TextFrame2.TextRange.Characters.Font.Size = 8
End With
Next jj
'軸ポイント描画
With ActiveSheet.Shapes
.AddShape msoShapeOval, Ox - 4, ZRotY(Ox, Oy) - 5, 8, 8 'O
.AddShape msoShapeOval, ZRotX(Bx(n), By(n)) - 4, ZRotY(Bx(n), By(n)) - 5, 8, 8 'B
.AddShape msoShapeOval, ZRotX(Gx(n), Gy(n)) - 4, ZRotY(Gx(n), Gy(n)) - 5, 8, 8 'G
End With
'軸記号描画
For kk = 0 To 7
ActiveSheet.Shapes.AddShape(msoShapeRectangle, ZRotX(AOx(kk), AOy(kk)) - 9, ZRotY(AOx(kk), AOy(kk)) - 8, 48.75, 43.5).Select
With Selection.ShapeRange
.Fill.Visible = msoFalse
.Line.Visible = msoFalse
.TextFrame2.TextRange.Characters.Text = axis(kk)
.TextFrame2.TextRange.Characters.Font.Fill.ForeColor.ObjectThemeColor = msoThemeColorText1
.TextFrame2.TextRange.Characters.Font.Size = 8
End With
Next kk
Next n
Range("B2") = "θ=" & θ & "°"
Range("A24").Select
DoEvents
Next θ
End Sub
Function Radians(Degrees As Variant) As Variant
'角度からラジアンを求る
Radians = (PI / 180) * Degrees
End Function
Function Acos(x As Variant) As Variant
'アークコサインを求める
Select Case x
Case Is = -1
Acos = PI
Case Is = 1
Acos = 0
Case Else
Acos = PI / 2 - Atn(x / Sqr(1 - x ^ 2))
End Select
End Function
Function ZRotX(x As Variant, y As Variant) As Variant
'Z軸を中心としてθ回転させた時のX軸座標を計算する
ZRotX = Cos(Radians(θ / 2)) * (x - Ox) - Sin(Radians(θ / 2)) * (y - Oy) + Ox
End Function
Function ZRotY(x As Variant, y As Variant) As Variant
'Z軸を中心としてθ回転させた時のY軸座標を計算する
ZRotY = Sin(Radians(θ / 2)) * (x - Ox) + Cos(Radians(θ / 2)) * (y - Oy) + Oy
End Function
2. まとめ
X軸とY軸の3D回転は片軸だけの計算で良かったので楽でしたがZ軸を回転させるとX,Y軸共に計算せねばならず計算負荷が倍増してしまいました
励みになりますのでよければクリック下さい(^o^)/
