まじかる☆マリオネット資料室

MSPインタプリタの仕様

20070119時点
- インタプリタコマンドのXが「サーボPWM反映」なのは間違いだと思う

持っていくものリスト

C言語関連

http://www.nurs.or.jp/~sug/soft/super/

PIC関連

時間とステップ数の対応表

L-Card関連

L-Card:GPIOドライバの注意点(20050804-1)
- /dev/led[0-9]は負論理なので注意(echo 0 > /dev/led で点灯、echo 1 > で消灯)
L-Card:GPIO関連のコネクタピン配置(20050807-2)

無線関連

RS232-STR関連

運動学まわり

基礎
- Radian(ラジアン角) vs Degree (度)
  - X(Rad) = X(Deg) * PI / 180
- 三角関数の定理
  - Wikipediaの「三角関数」参照

- 脚のピッチ軸の関係式(20040226の日記)
  - (Rubyの式は最後のRad/Deg変換が抜けているので注意)

図から
θ4 = θ3 + θ5
L34 * sinθ3 = L45 * sinθ5
(θ4の回転方向が逆だが符号は気にしないことにする)

式1)
∴ θ4 = θ3 + asin( sin(θ3) * L34 / L45 )

また、L34を延長し、∠J3,J4',J5が直角をなす点 J4'を仮定すると
L35^2 = L34'^2 + L4'5^2
L44' = L45 * cosθ4
L4'5 = L45 * sinθ4

式2)
∴  L35^2 = ( L34 + L45 * cosθ4 )^2 + ( L45 * sinθ4 )^2

これをJ4に対する式に変換する

右辺第１項を展開すると
L35^2 = L34^2 + 2 * (L34 * L45 * cosθ4) + (L45 * cosθ4)^2 +  (L45 * sinθ4)^2
      = L34^2 + 2 * (L34 * L45 * cosθ4) + L45^2  ∵ cosθ^2 + sinθ^2 = 1

左辺がθ4 になるように変換すると
2*(L34*L45*cosθ4) = L35^2 - L34^2 - L45^2
            cosθ4 = ( L35^2 - L34^2 - L45^2 )/(2 * L34 * L45)
式3) 
               θ4 = acos ( ( L35^2 - L34^2 - L45^2 )/( 2 * L34 * L45) )

さらにθ4の値からθ3を求めるため
θ4 = θ3 + asin(sinθ3 * L34/L45)
をθ3を左辺とした式に変換する。
 
sin(θ4-θ3) = sinθ3 * L34/L45
加法定理より
( sinθ4 * cosθ3 - cosθ4 * sinθ3 ) = sinθ3 * L34/L45
両辺をsinθ3で割ると
 sinθ4/tanθ3 - cosθ4 = L34/L45
          sinθ4/tanθ3 = L34/L45 + cosθ4
                 tanθ3 = sinθ4/(L34/L45 + cosθ4)
式4)
                    θ3 = atan( sinθ4 / (L34/L45 + cosθ4) )

Ruby で計算する場合 (Jx は θx のDegree値を持つ変数とする)

式1')
J4 = J3 + Math.asin(Math.sin(J3 * Math::PI/180) * L34/L45 ) / Math::PI*180

式2')
L35 = Math.hypot( L34 + L45 * Math.cos( J4 * Math::PI/180 ),
                   L45 * Math.sin( J4 * Math::PI/180 ) )
  ※) Math.hypot(x,y) は sqrt(x*x, y*y) を返すモジュール関数

式3')
J4 = Math.acos( ( L35**2 - L34**2 - L45**2 ) / ( 2 * L34 * L45) ) / Math::PI*180

式4')
J3  = Math.atan( Math.sin(J4 * Math::PI/180) / (L34/L45 + Math.cos(J4 * Math::PI/180)) ) / Math::PI*180

ServoCreationのDXFデータからサイズを拾うと
L34 = 39.0 mm + 30.0 mm = 69.0 mm
L45 = 39.0 mm + 39.0 mm = 78.0 mm 

腰と足首を同一平面上にする J3,J4,J5 を求める式

J5 = Math.asin(Math.sin(J3 * Math::PI/180) * L34/L45 ) / Math::PI*180
J4 = J3 + J5

L35が与えられたとき、J3,J4,J5 を求める式
J4  = Math.acos( ( L35**2 - 10845.0 ) / 10764.0 ) / Math::PI*180
J3  = Math.atan( Math.sin(J4 * Math::PI/180) / (L34/L45 + Math.cos(J4 * Math::PI/180) )