2019a/Member

目次

課題について

課題内容

ピンポン玉または青と赤のボールを運搬して、所定の容器の中に入れる。
詳しいルール等は 2019a/Mission3 を参照。
また課題となるコースは以下。
箱の中の2つ、地面にある2つの黄色いボールが運搬対象のボールである。

課題コース

戦略

今回の課題において、我々はロボットを2台作ることにした。
一つは地面にある2つの球を担当し、もう一つは箱にある2つの球を担当する。
ロボットの構造、またどの様に動くかは後述する。

ロボットについて

私たちの班は2種類のロボットを作った。
ひとつめは、アームが一本角のように伸びているもの。
ふたつめは、アームが刺又型になっていているもの。
その構造的特徴から我々はそれぞれカブトムシ型ロボット、クワガタ型ロボット、
略してカブトロボ、クワガタロボと名付けた。

カブトロボ

全体像

カブト全体

カブトロボの全体像である。
右に付いているのが球を取るアーム、車体の端には光センサーがついている。
また背中には持ち上げた球が転がる道が備え付けられている。

アームと背中の構造

カブトアーム

アームの先にはゴムが計3本が張られている。
この輪ゴムの柔軟性を利用して、球を取る仕組みである。
また、背中には左右に壁となるブロックをつけ、アームが上がり球が転がり落ちるとき、
ゴールである箱に落ちるよう、そのルートを強制している。

球をキャッチする方法

球のキャッチする仕組みをもう少し詳しく見てみよう。
以下の写真は実際の球のキャッチのコマ送りだ。

カブトキャッチ1
カブトキャッチ2
カブトキャッチ3

.棔璽襪凌寝爾僕茲浸、上に固定されていたアームが勢いよく下がる。
⇔悒乾爐僚斉霎により、球をすり抜けアームが最下点まで下ろされる。
5紊鬚垢衄瓦韻晋紂▲乾爐聾気侶曽に戻るため球は下に落ちなくなる。
キャッチ成功!

球をシュートする方法

キャッチした後、球を箱に入れる仕組みは以下の通りだ。
(*キャッチからの通し番号になっている)

カブトシュート1
カブトシュート2
カブトシュート3

ぅ◆璽爐球を乗せたまま上がる。
ゥ◆璽爐上がりきると、球が重力に従って転がり落ちる。
Ε蹈椒奪箸稜愧罎鬚泙辰垢暗召り落ち、先にある箱に入る。
シュート成功!

クワガタロボ

全体像

クワガタ全体

クワガタロボの全体像である。
左に付いているのが箱を挟むアーム、背中にはカブトロボと同じ球が通る道がついている。
このロボはライントレースは使わないため、光センサーは付いていない。

アームと背中の構造

クワガタアーム

アーム(グレーの棒)は二又に分かれている。
目標となる箱は上部が出っ張ている形をしている。
そのため、箱にこの刺又を差し込めば上に持ち上げても
箱の上部が引っ掛かり持ち上げられるのだ。
持ち上げると、白の二又の棒がストッパーとなり中の球だけが転がり落ちる。
あとはカブトロボと同じように背中を伝って箱にゴールする。

箱を挟み取る方法

それでは詳しく挟み取りを見てみよう。
以下の写真は挟み取る動作のコマ送りだ。

クワガタ挟み1
クワガタ挟み2
クワガタ挟み3

“△忙彬瑤鮑垢傾む。
奥まで刺しこむとロボットの動作に従い箱は引っ張られる。
*ただし後ろに動くと箱はその場に置き去りになるので、
 アームを少し上げ箱と地面の接触をなくす必要がある。
差し込み成功!

箱を持ち上げシュートする方法

箱を持ち上げ球をシュートする方法は以下の通りだ。

クワガタシュート1
クワガタシュート2
クワガタシュート3

.◆璽爐鮠紊欧襪犯△里びれに引っ掛かり箱は持ち上がる。
白い棒の刺又がストッパーとなって箱は落ちず中の球だけ転がり落ちる。
 いわゆるちゃぶ台返しのような形。
E召り出た球は背中の通路を伝って箱に落ちる。
シュート成功!

プログラミングについて

定義の設定

以下、defineを使用し定義したプログラミングである。

  • ライントレース継続の判断時間
    #define MOVE_TIME 250
  • ライントレース時に判断基準となる明度
    詳細は後述
    #define B1 43
    #define G 53
    #define W1 59
    #define W2 61
  • モーターの速さ
    モーターの速さをあらかじめ統一しておくことで作業の効率化が狙える。
    #define SPEED_H 35
    #define SPEED_L 20

動作

  • OnRL この定義はOnRL(x,y)とした時、x=(モーターBの速さ)y=(モーターCの速さ)を表す。
    モーターBが右の車輪、モーターCが左の車輪に連携していて、任意の値をx,yに入れることで
    車体が前後運動、方向転換を行うことができる。
    この定義により後述する定義等の簡略化に貢献している。
    #define OnRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL);
  • 前後運動
    #define go_straight OnRL(SPEED_H,SPEED_H);   //前進
    #define BACK OnRL(-SPEED_H,-SPEED_H);        //後進
  • 旋回
    片方の車輪を前転、もう片方が後転することで、急な方向転換が可能となる。
    回転軸は車輪軸の中央。
    #define rot_r OnRL(-SPEED_L,SPEED_L);   //右旋回
    #define rot_l OnRL(SPEED_L,-SPEED_L);   //左旋回
  • 回転
    片方の車輪を前転、もう片方は停止させることで、緩やかな方向転換が可能となる。
    回転軸は停止車輪の中央。
    #define turn_r OnRL(0,SPEED_L);   //右回転
    #define turn_l OnRL(SPEED_L,0);   //左回転

サブルーチン関数の設定

以下、今回使用したsubルーチン関数である。

ライントレース [follow_line]

この関数は光センサーでコースの線の明度を計測し、
条件に従って指定された動作をして線上を動いていくものだ。
以下がその関数である。

  • 左側の直線トレース
    sub follow_line_l()
    {
        SetSensorLight(S1);
        long t0=CurrentTick();
        while(CurrentTick()-t0<MOVE_TIME){
            if(SENSOR_1>W1){
                rot_r;t0=CurrentTick();
            }
            else if(SENSOR_1>W2){
                turn_r;t0=CurrentTick();
            }
            else if(SENSOR_1>G){
                go_straight;t0=CurrentTick();
            }
            else if(SENSOR_1>B1){
                turn_l;t0=CurrentTick();
            }
            else{
                rot_l;
            }
        }
    } 
  • 右側の直線トレース
    sub follow_line_r()
    {
        SetSensorLight(S1);
        long t0=CurrentTick();
        while(CurrentTick()-t0<MOVE_TIME){
            if(SENSOR_1>W1){
                rot_l;t0=CurrentTick();
            }
            else if(SENSOR_1>W2){
                turn_l;t0=CurrentTick();
            }
            else if(SENSOR_1>G){
                go_straight;t0=CurrentTick();
            }
            else if(SENSOR_1>B1){
                turn_r;t0=CurrentTick();
            }
            else{
                rot_r;
            }
        } 
    }

詳細について述べていく。今回は左側直線トレースを取り上げる。
光センサーがコースの線路の明度を計測。明度ごとに設定された命令をこなす。
明度と命令の設定は以下。

ライン

この表を図化したものが以下である。
緑→が前進、オレンジ→が回転、青→が旋回を示している。

ライン

「follow_line_l」をフローチャート化すると次のようになる。

チャート

  始めにCurrentTickをt0に(リセット)する。
  次に「CurrentTick()-t0<MOVE_TIME」によってCurrentTickの計測時間が
  MOVE_TIME(250)を超えるまでwhileの中をループし続ける。
  while文の中にはif文、else文で構成されていて、光センサーの値によって処理が
  異なるようになっている。光センサーの値が43より大きい値の時(黒以外の時)
  CurrentTickの値は命令遂行後、常にリセットされ、ループは継続される。
  しかし光センサーの値が45以下の時(黒の時)、CurrentTickはリセットされない。
  この状態が一定の時間(MOVE_TIME)続くことで、交差点であると判断し、
  このループから抜け、止まる構造となっている。

一時停止 [short_brake]

ライントレースが交差点と判断し止まったかどうか視覚化するために使う一旦停止。

sub short_break()
{
    Off(OUT_BC);
    Wait(1000);
}

後進 [back]

long型を使い、backする時間を指定できるようにした。

sub back(long t)
{
    BACK;
    Wait(t);
    Off(OUT_BC);
}

球のキャッチ [arm_down]

カブトロボが球をキャッチするときに使う関数。
キャッチの詳細は #fe44dce4 を参照。

sub arm_down()
{
    OnFwd(OUT_A,70);   //アームを上げる
    Wait(600);
    Off(OUT_A);
    Wait(500);
    OnRev(OUT_A,50);   //アームを下げる
    Wait(500);
    Off(OUT_A);
}

球のシュート [shoot]

ロボが球をシュートするときに使う関数。
シュートの詳細は #m647fc1b#hc5e16c0 を参照。

sub shoot()
{
    OnRev(OUT_A,60);   //アームを下げる
    Wait(700);
    Off(OUT_A);
    OnFwd(OUT_A,55);   //アームを上げる
    Wait(500);
    Off(OUT_A);
}

メインプログラム

以上の定義や関数を使い、メインとなるプログラミングを行う。
以下はコースをどう動くかを表したマップとそのプログラミングである。
緑→が前進、赤→が後進を示している。

カブトver.

マップを見やすくするための都合上、ゴールの箱に近い球を球A、遠い方を球Bとし、
球Aを取りシュートするまでのコースA、
そこから球Bを取りシュートするまでのコースB、
で分けて紹介する。

  • コースA
    コース1
    A'地点をスタート後、ライントレースでJ'地点の交差点(マップでは[交]と表記)まで行く。
    そこからは、Waitの時間を調節しながら進んでいく。
    詳細をプログラムと同時に説明していこう。
    task main()
    {
        Off(OUT_A);      //車体の動き始めのブレで上げてあるアームが下がらないよう止める
        go_straight;     //スタート地点からトレースするラインまで直進
        Wait(1000);
        Off(OUT_BC);
        follow_line_l(); //ライントレース開始(今回は線の左側をトレース)
        short_break();   //交差点判断後、一旦停止
        rot_r;           //右旋回で車体を球Aの方向へ向ける
        Wait(400);
        Off(OUT_BC);
        go_straight;     //球Aまで前進
        Wait(800);
        Off(OUT_BC);
        arm_down();      //アームを下げ球Aをキャッチ
        turn_r;          //右回転で車体のお尻をゴールである箱に向ける
        Wait(3500);
        Off(OUT_BC);
        shoot();         //シュート
  • コースB
    コース2
    球Aをシュートした後、そのまま球Bを取りに行く。
    同様に詳細をプログラムと同時に説明していこう。
        Wait(1000);        //(球Aシュート後の続きから)一度停止
        rot_r;             //右旋回し車体を球Bの方へ向ける  
        Wait(3800); 
        Off(OUT_BC);
        go_straight;       //球Bまで前進
        Wait(6600);
        Off(OUT_BC);
        arm_down();        //アームを下げ球Bをキャッチ
        back(3000);    //球Bを持ったまま後進
        OnRL(0,-SPEED_L);  //左車輪を後転させ車体のお尻をゴールの箱に向ける
        Wait(300);
        Off(OUT_BC);
        shoot();           //シュート
    }

クワガタver.

クワガタロボは箱にある2つの球を一気に運ぶため一度に紹介する。
またこのロボは光センサーを使ったライントレースはしない。

コース3

カブトロボとは違うA地点をスタート後、球の入った箱を取り、ゴールに向かう。
以下、詳細をプログラムと同時に説明していこう。

task main()
{
    go_straight;       //スタート後、箱の下まで前進
    Wait(1000);
    Off(OUT_BC);
    rot_l;             //右旋回し車体を目標の箱の方向へ向ける
    Wait(2800);
    Off(OUT_BC);
    go_straight;       //前進し刺又アームで箱を差し込む
    Wait(3000);
    Off(OUT_BC);
    rot_l;             //左旋回し車体をゴールの箱の方向へ向ける
    Wait(4500);
    Off(OUT_BC);
    go_straight;       //ゴールの箱付近まで前進
    Wait(3000);
    turn_l;            //左回転をし車体のお尻をゴールの箱に向ける
    Wait(8700);
    Off(OUT_BC);
    Wait(27000);       //衝突や接触を避けるためカブトロボが2つ目の球Bを入れるまで待機
    OnFwd(OUT_A,50);   //バックで箱を置き去りにしないようアームを少し持ち上げる
    Wait(700);
    OnRev(OUT_BC,60);  //勢いよく箱に向かってバック←箱の中の球が転げ出やすいようにする
    Wait(700);
    Off(OUT_BC);
    shoot();           //バックと同時に箱を持ちあげ球を転げ出してシュート
}

感想

ロボットについて

今回、ロボットの制作にあたって構想に多くの時間を費やした。
ロボットを一台にまとめるか、それとも二台作り分担させるか。
地面の球はどのように取りどの様に箱に入れるか。
同様に箱の中の球はどうやって扱うか。
そもそも何点を目標にするか。
などなど、課題が難しくその分手法も多く考えられるためかなり難航した。
ただ、その十分な構想期間があったからこそ、その後の制作を楽にしたのは明らかだ。
今回の課題では、事前の構想をいかに現実的で詳細にできるかが大事だと分かった。

プログラミングについて

ライントレースなど課題2で使用したプログラムの多くをそのまま流用できたので
作業効率をぐんと上げてくれた。
我々はプログラミング初心者なので自分たちのできる範囲内でやりくりしたが
最善を尽くせたと思う。

ロボコンの結果について

私たちの班は結果3位という好成績を残せた。
最後の最後まで調整をするなど、課外で多くの時間を費やした分、素直にうれしい。
それぞれの得意な部分を生かした分担作業が功を奏したと思う。協力的だった班員には感謝だ。

この講義を通して

これにてロボティクス入門ゼミは終了となる。
私にとって、ロボットもプログラミングも全くの未知のものであったため
どの課題もかなり難しく失敗や消費する時間は他の人より多かっただろう。
しかし、普通に生きていたら恐らく関わることなど一切なかったこの2つのジャンルを
経験できたことは大きなアドバンテージになると思う。
将来的に自分の中の持ち駒を増やすことや視野を広げることにつながるからだ。
もちろん今回学んだのはロボット制作やプログラミングの初歩の初歩だろうが、
関わることのなかった新しいものに興味を持てた点で充実した講義であったと思う。


添付ファイル: fileクワガタアームム.png 9件 [詳細] fileクワガタ全体.png 11件 [詳細] fileクワガタアーム.png 9件 [詳細] fileクワガタロボ全体.png 11件 [詳細] fileコース3.png 8件 [詳細] fileコース2.png 4件 [詳細] fileコース1.png 8件 [詳細] file表2.png 8件 [詳細] file表1.png 7件 [詳細] file表.png 2件 [詳細] fileライン制御.png 2件 [詳細] fileチャート改.png 9件 [詳細] file課題コース.png 2件 [詳細] fileロボ_190810_0002.jpg 3件 [詳細] fileロボ_190810_0011.jpg 4件 [詳細] fileロボ_190810_0012.jpg 7件 [詳細] fileロボ_190810_0013.jpg 5件 [詳細] fileロボ_190810_0008.jpg 6件 [詳細] fileロボ_190810_0009.jpg 9件 [詳細] fileロボ_190810_0010.jpg 3件 [詳細] fileロボ_190810_0006.jpg 2件 [詳細] fileロボ_190810_0005.jpg 1件 [詳細] fileロボ_190810_0014.jpg 7件 [詳細] fileロボ_190810_0015.jpg 6件 [詳細] fileロボ_190810_0016.jpg 4件 [詳細] fileロボ_190810_0017.jpg 4件 [詳細] fileロボ_190810_0018.jpg 3件 [詳細] fileロボ_190810_0019.jpg 6件 [詳細] fileロボ_190810_0004.jpg 1件 [詳細] fileロボ_190810_0003.jpg 3件 [詳細] fileロボ_190810_0001.jpg 2件 [詳細]

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2019-08-14 (水) 02:23:04