スポンサーサイト

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


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

お絵かきロボ2 - 動画編

こんにちは、matkです。

動画は以下になります。



出来上がったPCの絵とリアルの絵は以下になります。

nxt-star.jpg SANY0014.jpg

いやー、PC上でちゃんと描けた時は「ヨッシャー」と思ったのですが…。マシンをどけてみて「あれ?」です。始点と終点が4cm弱ズレています。こうなる原因として考えられるのは恐らく二つです。一つ目は動いているうちに、マシンの中にあるマウスのポインタとペン先の位置関係がずれることですが、4cmもズレることはないのでこれは違う。もう一つは、マウスが動いているのに、PC上のカーソルがちゃんと動いていないことです。実はよくよく観察しているうちに、マウスが動いている割にはPC上でカーソルの動きがシブイ時があるのに気づきました。試しに紙の上で手でマウスを何度も動かしたのですが、その時は大丈夫と感じていたのですが…。

これが光学式マウスだから起きる現象なのかは分かりません。レーザーマウスや青色LEDマウスにしたら解決する可能性もありますが、もっと最悪な可能性として、マウスはリアルの移動距離とカーソルの移動距離を精度よく比例させるようにできていないという可能性もあります。この場合は、マウスを買っても金をドブに捨てることになってしまいます。

うーむ。はじめから青色LEDのワイヤレスマウスを買っておけばよかった…。700円ケチって光学式マウスなんぞを買ってしまったばっかりに…。幸い、会社のマウスが気に食わないので、青色LEDマウスをポチッとしてみることにします。

 

最後のリベンジはまた来週に!

●2011.9.7 追記
マウス変えてもダメでした…orz マウスはそーゆー道具ではありませんでした。
いつかGPSの精度が10cmぐらいになったら運動場で白い粉で絵を描くことにします!ちくしょー。

にほんブログ村 その他趣味ブログ LEGO・ブロックへ
スポンサーサイト

お絵かきロボ2 - プログラム編

こんにちは、matkです。

需要の少なさそうなプログラム編です。まずは、プログラミングとは直接関係はないのですが、設計編の目的に沿うようにマウスの設定を変えます。もちろん、プログラムの実行時だけで構いません。具体的には、[コントロールパネル]-[マウス]からマウスの設定を変えます。以下の写真のように、精度のところのチェックをはずして、速度を遅くします。
mouse1.jpg

これで、マウスの動いた距離とカーソルの動いた距離が比例し、なおかつ、大きな絵がかけるようになります。また、動画編では、マウスカーソルを動いた軌跡を残したいために別のソフトを使いますが、その時にクリックしながらドラッグしないと絵をかけないため、以下の写真のようにクリックロックという機能を使いました。
mouse2.jpg

さて、プログラム本体ですが、はじめはMicrosoft Robotics Developer Studio (MRDS) で作っていたのですが、マウスカーソルの位置を定期的に出力するサービスを作るまでは順調だったのですが、それをつかってVPLですすめていくうちに挫折してしまいました。どうも自分は横スクロールが苦手なようです。

そこからMindSquallsをいったんはさんで、結局nxt-pythonを使いました。nxt-pythonもモーターの同期が最近実装されたばっかりで不安、exampleが充実していないなどのとっつきにくさはあるものの、rubyやRに慣れている自分にとっては大変書きやすかったです。

#!/usr/bin/env python

import math
import nxt.locator
from nxt.motor import *
from nxt.sensor import *
from win32api import GetCursorPos as mousepos
from win32api import GetSystemMetrics as screensize

tt_ini       = math.radians(45)  # 初期のタイヤの角度(厳密でなくともよい)
draw_data    = [[0.0,-100.0], [1062.0,384.0], [0.0,-10.0], [376.38,606.77], [800.12,23.55], [800.12,744.45], [376.38,161.23], [1062.0,384.0], [0.0,-100.0]]
turn_power   = 100   # タイヤの方向転換する際のモーターパワー
move_power   = 100   # 移動する際のモーターパワー
h_lim        = 4     # どれぐらい目標方角と近くなったらタイヤの方向転換をやめるか        
r_lim        = 5.1   # どれぐらい目標地点とカーソルの座標が近くなったら移動をやめるか
r_brk        = 30    # どれぐらい目標地点とカーソルの座標が近くなったら、細かく移動するようになるか
move_dst     = 180   # 移動する際の通常時のモーターの回転角度
move_dst_brk = 90    # 移動する際の細かく移動する時のモーターの回転角度
phi_lim      = 0.05  # 進路とどれくらいの角度がずれているか
tf           = 6.0   # タイヤの向きを修正する際の適当な係数

b = nxt.locator.find_one_brick()
compass = HTCompass(b, PORT_1)
mt_turn = Motor(b, PORT_A)
mt_move = Motor(b, PORT_B)
mt_pen  = Motor(b, PORT_C)
screen_w = screensize(0)
screen_h = screensize(1)

def get_heading_ave(rep=4):
    hd_sum = 0.0
    for i in range(rep):
        hd_sum += compass.get_heading()
    hd = hd_sum/rep
    return hd

def get_h_diff(hd_goal):
    hd = get_heading_ave()
    diff = math.fabs(hd_goal - hd)
    print '    * hd, diff: %.2f %.2f' % (hd, diff)
    return diff

def get_xyra(x_to, y_to):
    x, y = mousepos()
    y = screen_h - y
    r = math.sqrt(math.pow(x_to - x, 2) + math.pow(y_to - y, 2))
    a = math.atan2(y_to - y, x_to - x)
    return [x,y,r,a]  # 順に, マウスカーソルの座標x,y(左下が0,0), 目標地点までの距離と角度 r,a


time.sleep(15)
hd_ini = get_heading_ave(12)
hd_ofs = hd_ini
tt_ofs = tt_ini

for x_to, y_to in draw_data:
    if y_to < -11:
        mt_pen.turn(-80, 720) # pen up
        continue
    elif y_to < -1:
        mt_pen.turn( 80, 900) # pen down
        continue
        
    # Rough Phase
    print "---- Rough Phase ----"
    x, y, r, a = get_xyra(x_to, y_to)
    if a > 0:
        tt = a
        move_forward = True  # 移動する際に前進方向にタイヤが回転する
    else:
        tt = a + math.pi
        move_forward = False
    hd_goal = (hd_ofs + math.degrees(tt - tt_ofs)) % 360
    print 'hd (ofs, goal):        %.2f %.2f' % (hd_ofs, hd_goal)
    print 'tt (ofs, goal, angle): %.2f %.2f %.2f' % (math.degrees(tt_ofs), math.degrees(tt), math.degrees(a) )
    mt_turn_power = turn_power if tt - tt_ofs > 0 else -turn_power  # turn left: +, right: -
    mt_turn.run(mt_turn_power)
    while get_h_diff(hd_goal) > h_lim: pass
    mt_turn.idle()

    # Detail Phase
    print "---- Detail Phase ----"
    x, y, r, a = get_xyra(x_to, y_to)
    print '(x,y), (r,a): (%d, %d), (%.2f, %.2f)' % (x, y, r, math.degrees(a))
    mt_move_power = move_power if move_forward else -move_power
    while r > r_lim:
        x_old, y_old, r_old, a_old = x, y, r, a
        mt_move_dst = move_dst_brk if r < r_brk else move_dst
        moved = 0.0
        while moved < 1.5:
            mt_move.turn(mt_move_power, mt_move_dst)  # 180 ~ moved:18
            time.sleep(0.7)
            x, y, r, a = get_xyra(x_to, y_to)
            print '(x,y), (r,a): (%d, %d), (%.2f, %.2f)' % (x, y, r, math.degrees(a))
            moved = math.sqrt(math.pow(x - x_old, 2) + math.pow(y - y_old, 2))
        if r <= r_lim:
            break
        phi = math.pi - math.acos( ( math.pow(r,2) + math.pow(moved,2) - math.pow(r_old,2) )/(2*r*moved) )
        mt_turn_power = turn_power if a > a_old else -turn_power
        print '    * moved, phi:  %.2f, %.2f' % (moved, mt_turn_power/100*phi)
        if math.fabs(phi) > phi_lim:
            mt_turn.turn(mt_turn_power, 168*tf)
            time.sleep(0.7)
        
    hd_ofs = get_heading_ave(12)
    tt_ofs = (tt_ini + math.radians(hd_ofs - hd_ini)) % (2*math.pi)


詳細を説明します。このプログラムはRough PhaseとDetail Phaseという二つのフェーズがありまして、前者でタイヤの向きをおおまかに揃え、後者で走行しているうちに歪んだ分を修正しながら走行します。

・11行目: これは別のプログラムで出力した、星型の頂点のデスクトップ上の座標です。
・30-35行目: コンパスセンサは1回の出力は±3ぐらいはズレますので、平均して値を安定させています。
・51行目: プログラムを実行してから左クリック長押しして、マウスをマシンにセットするための時間稼ぎです。
・64-79行目: Rough Phaseです。
・67-72行目: 目標地点までの角度は(-PI, PI]の範囲ですが、タイヤの向き(角度)を[0, PI)にしてタイヤの回転方向が前進か後退かで(-PI,PI]の範囲に対応しています。
・73行目: 動く前のコンパスセンサの値をオフセット(offset; ofs)として、ゴールとなるコンパスセンサの値を決めています。
・77-79行目: ゴールとなるコンパスセンサの値になるまでタイヤを方向転換させています。
・81-103行目: Detail Phaseです。
・88行目: ある程度目標地点に近づくと、動く距離が短くなります。
・90行目: マウスが動いているのにもかかわらず、カーソルが動かない時がごくたまに発生していたので、それを回避する目的です。基本的には発生しません。
・92行目: 本当はsleepは必要ないのですが、入れたほうが安定したので入れてあります。その代わり描いた絵はちょっと醜くなります。
・98行目: 前の位置と、ちょっと動いた後の位置と、目標地点からなる細長い三角形から、余弦定理を使って修正すべき角度を算出しています。
・101行目: 修正すべき角度がある程度小さいときは何もしないで進みます。
・102行目: ギアの遊びがあるので、修正すべき角度の分だけモーターを回しても追いつきません。ここではかなり多めに回しています。

次は動画編です。

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

お絵かきロボ2 - 組み立て編

こんばんは、matkです。

完成図を先に紹介します。
順に、上から、前から、後ろから(以上、上の段)、左から、右から、裏から(以上、下の段)です。

SANY0016.jpg SANY0018.jpg SANY0017.jpg

SANY0019.jpg SANY0020.jpg SANY0021.jpg

ここからは製作記(苦労話)です。

まずは、マウスのカーソルを動かさないで方向転換する仕組みに苦労しました。単純なキャタピラのような車輪ですと、ぐるっと時計回りに方向転換するときに、持ってるマウスのカーソルもえらい勢いで右のほうに行ってしまいます。

そこでターンテーブルを使って、タイヤをその場で回転させるようにしました。ただしバランスのため最低二つはタイヤがないといけません。そのタイヤの向きをなるべく揃えながら回転させるために、その二つのタイヤの動かす仕組み(ギア構成)を対称形で作りました。タイヤを回す(マシンが移動する)ギア構成も対称です。これでギアの「遊び」込みでほとんど向きと動きが揃うはずです(6枚目の「裏から」の写真参照)。

次に苦戦したところは、少しでも早く動くとやっぱり持ってるマウスが動くところでした。これはワームギアを使って減速することで解決しました(第1回レゴ会にて修正)。

その次の問題は、はじめはマウスを三本目の足として使っていたのですが、マウスにかかる圧力が強すぎて、床にひっかかったりしてうまく走行できません。しぶしぶ二つのタイヤの向きと揃うような、ただのキャスターのタイヤをつけました(二枚目の写真の青色やら赤色が写っているターンテーブル)。こちらはギア構成は微妙に異なります。


最後に苦労したのは、コンパスセンサの位置です。コンパスセンサはタイヤの向きをおおまかに合わせる時に使います。コンパスセンサのおかげでプログラムがだいぶシンプルになりました。その目的のため、タイヤの向きと連動していなくてはいけません。はじめはキャスターにつけていたのですが、振動によるブレが大きくてやめました。

それ以前に、モーターの近くだとコンパスセンサはうまく測れません。HiTechnic社のホームページには「NXT本体、NXTのモーターから10~15cmは離してね」と書いてありましたが、7~8cmぐらいで大丈夫に思います。また、モーターのケーブルの口に丸っこい部分に近いとダメであって、先端のオレンジ色の部分からはある程度近くても大丈夫なことが分かりました。結局、なるべくモーターから離れたところに、四つ目のターンテーブルをつけて回転が揃うようにしました。

また、コンパスセンサは白い部分が上だと北を向くと0、東を向くと90、南を向くと180、西を向くと270というように時計回りに回ると値が増えていきます。これは今まで習ってきた数学では反時計回りに角度が増えていくのと逆方向で、プログラムがちょっとややこしくなります。そこで、白い部分が下でとりつけたところ、反時計回りで値が増えていくようになり、プログラムがシンプルになりました。


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

お絵かきロボ2 - 設計編

こんばんは、matkです。

かなり長い間お絵かきロボと格闘していました。2ヶ月弱ぐらい?かな。今回は設計・組み立て・プログラミングのどれも時間がかかって、途中からお仕事のような気分でプログラミングしてました。

まずは設計編です。前回は、精度をあまり気にせずに組んだらグダグダになりました。原因は、ギアとギアの間には「遊び」と呼ばれるわずかな隙間がありまして、この影響で、ギアを右に3回転してから、すぐに左に3回転しても元の位置には戻らないためです。これが積もり積もると、プログラムの中ではちゃんと計算して回転しても、線の終点の位置は大きくずれてしまい、結果として現実のペンの位置はグダグダになってしまいます。また、普通にこの精度の問題に真っ向対決を挑もうとすると、大きな絵は描けずに小さい絵になってしまいます。

そこで考えました。大きな絵を描けて、なおかつペンの終点の位置はずれないようにする方法を。線の途中は少しぐらいズレても絵としてはOKなはず…。

これを実現するには、線を引っ張っている最中にもペンの行き先を補正する・ナビゲートするような仕組みがあればOKです。今回はパソコンの「マウス」を持たせて、パソコン上のカーソルの座標を見ながらペンの行き先を補正するようにしました。

あれほど「レゴで精度と戦うな」と肝に銘じていたにもかかわらず、ちょっとした思いつきでどうしても戦いたくなったのです。つくづくウルトラバカです(^-^;)

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

お絵かきロボ - 動画(失敗)

今までの投稿ですでに悲劇的なエンディング(?)を匂わせていましたが、現在のところの、残念すぎるモナリザは以下になります。


こんな不細工なモナリザは見たことないですね…。妻に爆笑されましたが投稿に至りました。

この失敗の主な原因は分かりました。それはターンテーブルです。ウォームギアだけがあやしいかと思っていましたが、ターンテーブル自体がカチャカチャ動くため、全く細かい制御に向きません。特に、お絵かきロボのようにギアが順回転と逆回転を繰り返すような場合はものすごくブレます。どれくらいブレるかというと、ハンド部分 とまんなか部分の間のターンテーブルがカチャカチャすると、ペン先が1mmぐらいブレます。まんなか部分と付け根部分の間のターンテーブルがカチャカチャすると、ペン先が5mm近くブレます。これでは、モナリザの細かい直線部分が全く描けず、また、一周すべきところが結局1cmぐらいずれて戻ってきてもおかしくありません。逆にターンテーブルを使いたいならば、回転する方向はなるべく片方に統一すること。そして、ターンテーブルからペン先の距離をなるべく短くすること。この2つを満たせば使えるとは思います。

僕があきらめが悪いので、ターンテーブルを40歯のギアに変えるなりして、もう少し粘ってみようと思います。

●2011.2.2 追記
ギア変えたり、いっそ車型を検討してみたり、色々粘りましたが、どうしても解像度を高めたまま精度よく書くというのは実現できませんでした。期待していた方ごめんなさい。頑張れば頑張るほどにyoutubeの方の設計のよさを思い知らされます。腕にモーターを含まないようにすることで腕を軽くして、解像度をやや低めにすることで誤差がかなり小さくなっているようです。

お絵かきロボ完成にはまだレゴ力が足りないと思われるので、またいつかリベンジしようと思います。今は誤差があまり問題にならないレゴミシンに着手中です。まとまった休日がないから時間がかかりそうです…。

にほんブログ村 その他趣味ブログ 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ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。