スポンサーサイト

上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。


にほんブログ村 その他趣味ブログ LEGO・ブロックへ

お絵描きロボ - プログラミング編 (1/4)

お絵かきロボの構成は、ハンド部分、まんなか部分、付け根部分の3パートありまして、それぞれ1つのモーターを持っています。また、それぞれの部分はターンテーブルで接続されています。この2つのターンテーブルの回転する角度によって、描く座標が決まります。

xy座標, rt座標, uv座標の図

以下では高校数学+逆三角関数で、その回転する角度を求めてみます。ここでは図のように変数を定義します。

ターンテーブル1 = ハンド部分 と まんなか部分 の間のターンテーブル
ターンテーブル2 = まんなか部分 と 付け根部分 の間のターンテーブル
原点 = ターンテーブル2の中心
R1 = ターンテーブル1の中心 と ターンテーブル2の中心 の間の長さ
R2 = ペン先 と ターンテーブル1の中心 の間の長さ
(x, y) = ペン先の座標
(r, t) = ペン先の極座標
(u, v) = (まんなか部分とx軸のなす角度, まんなか部分の延長線とハンド部分のなす角度)
A, B, C = それぞれ三角形の内角の角度

解くべき問題は「(x, y) が与えられた時に、(u, v)を(x, y)を使って表す」ことです。
以下の記述では、「sqrt」はルート、「^」は階乗を表します、googleで「sqrt(6^2+12^2)」と打ち込んで検索してみてください。

まず、R1, R2は組み立て完了時に分かっていて、三平方の定理より、
R1 = sqrt(6ポッチ^2+12ポッチ^2) ≒ 13.41641ポッチ = 13.41641 * 5/16 inch ≒ 106.4927 mm
R2 = sqrt((2.5+ε)ポッチ^2 + 13ポッチ^2) ≒ 105.0782 mm + 0.2998 * ε mm
ここで、εは2.5ポッチからペン先が少しだけズレていることを表す量で、だいたい2mm以内(≒0.25ポッチ以内)です。テイラー展開を使ってεを外に出しています。ペンがマッキーとかでなければ、R2 = 105.0782 mm として十分です。

次に、(x, y)を極座標の(r, t)に変換します。これは簡単で、r = sqrt(x^2 + y^2), t = atan(y/x) です。(r, t)を(u, v)に変換するには、いくつか満たすべき式から変数を消していきます。

定義から: ①式: t = B + u
余弦定理: ②式: r^2 = R1^2 + R2^2 - 2*R1*R2*cos(PI-v)
正弦定理: ③式: R1/sin(A) = R2/sin(B)

まず②の中で分かっていないのは v だけです。また、cos(PI-v) = -cos(v) なので、普通に解いて、
v = acos((r^2-R1^2-R2^2)/(2*R1*R2)) となります。

次に、A + B = v から A = v - B であり、これを③式に代入すると、
R1/sin(v-B) = R2/sin(B)
⇔ R1*sin(B) = R2*sin(v-B)
⇔ R1*sin(B) = R2*(sin(v)cos(B) - cos(v)sin(B))
⇔ (R1 + R2*cos(v)) * sin(B) = R2 * sin(v)cos(B)
⇔ (R1 + R2*cos(v)) * tan(B) = R2 * sin(v)
⇔ B = atan(R2*sin(v)/(R1 + R2*cos(v)))

①式より、u = t - B = t - atan(R2*sin(v)/(R1 + R2*cos(v))) となります。

これらをプログラミングする時に気をつけたいのは、各種逆関数(acos, atanなど)が [-PI/2, PI/2]の範囲で値を返す時があるので、負の場合はPIを足して (u, v)を意図した範囲にします。
スポンサーサイト


にほんブログ村 その他趣味ブログ LEGO・ブロックへ

コメントの投稿

非公開コメント

リンク
フリーエリア
プロフィール

tsukuba.lego

Author:tsukuba.lego
●主な登場人物
koba : 会長. RCX, WeDo, NXT1.0, NXT2.0を持つ.
matk : 副会長. 2010年夏にNXT2.0を購入, 20年来のレゴ熱復活.
●会員募集中です!小学生から歓迎. 近日月1ぐらいで集まる予定.
mail:tsukuba.lego@gmail.com
YouTube (tsukubalego)
Twitter(tsukubalego)

カテゴリ
最新記事
最新コメント
月別アーカイブ
検索フォーム
RSSリンクの表示
ブロとも申請フォーム

この人とブロともになる

QRコード
QR
最新トラックバック
上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。