スポンサーサイト

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


にほんブログ村 その他趣味ブログ 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・ブロックへ

コメントの投稿

非公開コメント

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

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