課題3 紙コップ&ピンポン球回収ロボットを作って与えられたミッションをクリアせよ

コースとミッション

フィールドの説明

課題3のコース
・フィールドは課題2で使用した紙を使用する。
・図のマジェンタの円に逆さまにした紙コップを置き、その中にピンポン玉を2つずつ入れてお く。
・図の黄色い円に紙コップを通常の向きに置き、障害物とする。直径6cmの円の外周をペンまたは鉛筆でマークしておく。
・紙コップには色をつけたり文字や記号を書いてもよい。
・RISのチームに限り図の緑ラインを引いてもよい。
・割り箸を使って一辺が14cmの正方形を作り、X地点を中心とした円に接するように置く。

ミッション

詳しくはロボティクス入門ゼミ、課題3のページを参照して欲しい。 http://yakushi.shinshu-u.ac.jp/robotics/?plugin=related&page=2017b%2FMission3

メンバー

今回は2チームに分かれ、協力してミッションの達成に挑んだ。メンバーとそれぞれのページを紹介する。 機体Aのチーム ・Taka http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2FTaka%2FMission3 ・Masa http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2FMasa%2FMission3 機体Bのチーム(私のチーム) ・Yoshi http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2FYoshi%2FMission3 ・takeshi(私)

機体の説明

ピンポン球をどう運ぶか

課題2では、紙コップを取って、所定の場所に置くことがミッションのメインの1つだった。今回の課題3では、ピンポン球をまた別の所定の場所に置くことがそれに加わった。ピンポン球は紙コップの中に入っているため、課題2で作った枠型のアームのように、紙コップをただ引きずるかたちで運ぶと、紙コップとピンポン球を別々に回収することが出来ない。その問題を解消するためには、紙コップを掴んで持ち上げる必要がある。

枠と紙コップとピンポン球

上の画像には、X地点にある14cm四方の割り箸で作った枠、紙コップの中に入ったピンポン球の図を示した。この状態からピンポン球を枠の中に入れるには、まず紙コップをピンポン球が転がって行かない程度の高さに持ち上げる。

ピンポン球を運ぶ手順1

このような状態になったら、高さを維持したまま紙コップを枠のところまで移動させる。

ピンポン球を運ぶ手順2

これで、ピンポン球を枠の中に入れることが出来た。ここから、紙コップだけを移動させるためにさらに高く持ち上げる。

ピンポン球を運ぶ手順3

後は、紙コップを枠の外に出し、Y地点まで運ぶ。

ピンポン球を運ぶ手順4

Y地点では、紙コップを重ねると得点が加算されるので、紙コップを持ち上げる最大限の高さは、紙コップ1つ分より少し高くなる程度にする。

紙コップを掴んで持ち上げる機構

NXTのセットには、モーターが3つしかない。右タイヤ、左タイヤですでにモーターを2つ使ってしまっているので、・掴む・持ち上げる という2つの動作を1つのモーターで行わなければならない。今回のミッションは、その動作を行う機構がキーとなる。

アームの動き

先ほど、ピンポン球を運ぶ過程の説明で、紙コップを上げる動きを図で示したが、実際はこのようなアームを使って、紙コップを囲む、掴む、上げる という一連の動作をご覧の通り1つのモーターで行う。

開いている手

これは開いている状態のアームの手を上から見た図である。アームの手は、可動な棒、非可動な棒の2本の棒からなっており、可動な棒の根本には、回転する軸がある。軸が右回りに回転することで2本の棒の感覚が狭くなっていき、紙コップを掴む。

閉じている手

すると、このように可動な棒がこれ以上動かなくなる。次に、紫色で示した回転軸がモーターからどのように動力の伝達をされているのか説明する。

アームの歯車

アームを側面から詳しく見てみると、モーターが一番右にあって、そこから左に向かって3つ平歯車が並んでいるのがまず確認できる。一番左の歯車は、傘歯車になっていて、もう一つある小さな平歯車(少し確認しにくい)と垂直に噛み合っている。

アームの歯車の図1

小さな平歯車を黄色で示した。この小さな平歯車が先ほど紫色で示した回転軸に連結されている。モーターが回転すると、図の通りその回転軸が回転して、最終的に可動な棒が動く。

アームの歯車の図2

先ほど説明した通り、可動な棒がこれ以上動かなくなるときが来る。すると、モーターの動力の伝達先が、手を閉じることから、アーム自体を持ち上げることに切り替わる。

アームの歯車の図3

ここまでが、アームを掴んで持ち上げる動作の説明である。逆に、アームを下ろして紙コップを置くには、モーターを逆回転させれば良い。このモーターを回転させるためのプログラムには、モーターのパワーと回転時間の情報だけではなく、モーターの回転角度の情報を入れなければならない。前者だけだと電池の消耗により、モーターの回転する角度が大きく変わってしまうからである。アームが動けなくなった後もモーターを回転させてしまうため、歯車に大きな負担がかかってしまったり、モーターの回転が足りなくて、アームの動きが足りなくなってしまったりする。このプログラムについては後で書く。

超音波センサー

これが超音波センサーである。音が出されてから反射して返ってくるまでの時間で距離を測る。

機体の全体図

機体を上から見た図

機体の全体を上から見る。スタンダードな車のロボットに、先ほど説明したアームと超音波センサーが装着されている。今回、2チームに分かれたが、同じ機体を2台作った。唯一違うところは、私のチームの機体に、相方の趣向でタッチセンサーを使ったボタンがついていることである。

ミッションクリアの為の作戦

初期位置

2台の機体にそれぞれ機体A、機体Bと名付け、取る紙コップを分担した。数字は、紙コップを割り箸の枠まで運んでいく順番、数字の色は、赤が機体Aの担当する紙コップ、青が機体Bの担当する紙コップであることを示している。ここから、ミッション遂行のための手順を説明する。

手順1

まず、機体Aが、先ほど説明した超音波センサーによる位置の判別方法で、紙コップ1の位置を判別する。

手順2

次に、機体Aは前進し、アームの手の中に紙コップ1を入れる。機体Bは位置を調整するため、少し前進する。

手順3

先ほどの説明の通り、機体Aは紙コップ1を少し持ち上げて割り箸の枠のところまで移動させ、ピンポン球を枠の中に入れる。機体Bは、機体Aからメッセージが送られてくるまで待機。

手順4

機体Aは紙コップ1を持ちながら、フィールドの左上まで移動して止まり、メッセージを機体Bに送る。すると機体Bは紙コップ2のところまで移動する。このように、機体同士が邪魔し合わないように、メッセージ機能を使っている。現在枠の中のピンポン球は2個

手順5

機体Bは、紙コップ2を枠まで移動させ、ピンポン球を枠に入れる。

手順6

機体BはY地点まで紙コップを移動させて置く。現在枠の中のピンポン球は4個

手順7

機体Bはフィールドの右上まで移動して止まり、機体Aにメッセージを送る。すると機体AはY地点まで紙コップ1を運び、紙コップ2の上に重ねる。

手順8

機体Aは紙コップ3を取りに行く。

手順9

機体Bは紙コップ3を枠まで運び、ピンポン球を枠に入れる。

手順10

機体Bは、紙コップ3をY地点まで運び、紙コップ1に重ねる。ピンポン球は枠の中にすべて入り、枠の中のピンポン球は6個となった。これで、ミッションクリアとなり、基本点満点を得る。しかし、今回の課題では、調整の時間が前回よりかなり少なくなってしまったため、実際理想通り、完璧な動きをさせるのは非常に難しかった。

プログラム

作戦を実行するためのプログラムである。

機体Aのプログラム

#define SPEED_SLOW 25
const float diameter = 5.45;  //タイヤの直径(cm)
const float track = 10.91;   //タイヤのトレッド幅(cm)
const float pi = 3.1415;    //円周率
void fwdDist(float d) //距離 d cm 前進
{
   long angle = d/(diameter*pi)*360.0;   //必要なタイヤの回転角度
   RotateMotorEx(OUT_AB,SPEED_SLOW, angle,0,true,true);
}
void baDist(float d) //距離 d cm 後進
{
    long angle = d/(diameter*pi)*360.0;   //必要なタイヤの回転角度
    RotateMotorEx(OUT_AB,SPEED_SLOW, -angle,0,true,true);
}

void turnAng(long ang)  //角度ang度の時計回りの旋回
{
    long angle = track/diameter * ang;   //必要なタイヤの回転角度
    RotateMotorEx(OUT_AB,SPEED_SLOW, angle,100,true,true);
}
void nturnAng(long ang)  //角度ang度の時計回りの旋回
{
    long angle = track/diameter * ang;   //必要なタイヤの回転角度
    RotateMotorEx(OUT_AB,SPEED_SLOW, angle,0,true,true);
}
int searchDirection(long ang)  //現在の方向を中心にang度の範囲で探し
                                  //障害物までの距離を返す
{
    long tacho_min ;   //もっとも近い距離を実現するタイヤの回転数
    int d_min = 300 ;  //もっとも近い距離の仮の最小値
    long angle = (track/diameter)*ang;  //旋回角度からタイヤの回転を計算
    turnAng(ang/2);           //指定された角度の半分を旋回
    ResetTachoCount(OUT_AB);  //角度計測をリセット
    OnFwdSync(OUT_AB,SPEED_SLOW,-100);   //反時計回りに旋回
    while(MotorTachoCount(OUT_A)<=angle){
       if (SensorUS(S1)<d_min){         //現在の距離が仮の最小値より小さい場合
            d_min = SensorUS(S1);       //仮の最小値を更新
            tacho_min = MotorTachoCount(OUT_A); //この時のタイヤの回転数を記録
        }
    }
    OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE);
    until(MotorTachoCount(OUT_A)<=tacho_min || SensorUS(S1)<=d_min);

    Wait(14);  //微調整
    Off(OUT_AB);Wait(500);
    return d_min;
} 
sub c_up(float d)   //645で最大
{
    RotateMotor(OUT_C,-30,d);
    Off(OUT_C);
}

sub c_down(float d)
{
    RotateMotor(OUT_C,30,d);
    Off(OUT_C);
}
task main()
{
    SetSensorLowspeed(S1);
    //紙コップの方向を探した後、近づいて25cm手前で停止
    int d = searchDirection(30);
    if (d > 10){
        fwdDist(d-25.0);
    }
    turnAng(200);
    c_down(640);
    OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE);
    Off(OUT_AB);
    baDist(12.0);
    c_up(260);   //紙コップをつかむ
    ResetTachoCount(OUT_AB);  //角度計測をリセット
    turnAng(110);
    OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE);
    Off(OUT_AB);
    baDist(11.0);
    c_up(330);    //ピンポン玉をはなす
    ResetTachoCount(OUT_AB);  //角度計測をリセット
    turnAng(90);
    OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE);
    Off(OUT_AB);
    fwdDist(17.0);
    ResetTachoCount(OUT_AB);  //角度計測をリセット
    turnAng(80);
    ResetTachoCount(OUT_AB);  //角度計測をリセット
    int e = searchDirection(40);
    int msg;  // 受け取った値を格納する変数
    while (true) {
        ReceiveRemoteNumber(MAILBOX1,true,msg);
        if (msg == 1){
            break;
        } else {
        Off(OUT_AB);
        }
        Wait(100);
        }
    Off(OUT_AB);Wait(3000);
    ResetTachoCount(OUT_AB);  //角度計測をリセット
    turnAng(30);
    OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE);
    Off(OUT_AB);
    fwdDist(15.0);
    ResetTachoCount(OUT_AB);  //角度計測をリセット
    int f = searchDirection(30);
    if (f > 10){
        fwdDist(f-25.0);
    }
    ResetTachoCount(OUT_AB);  //角度計測をリセット
    turnAng(-156);
    OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE);
    Off(OUT_AB);
    baDist(8.0);
    c_down(100);
    OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE);
    Off(OUT_AB);
    baDist(2.0);
    OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE);  //微調整
    Off(OUT_AB);
    fwdDist(12.0);

    c_up(100);

    ResetTachoCount(OUT_AB);  //角度計測をリセット
    turnAng(230);

    ResetTachoCount(OUT_AB);  //角度計測をリセット
        int g = searchDirection(20);
        if (g > 10){
            fwdDist(g-25.0);
    } 

    turnAng(190);

    c_down(630);
    OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE);
    Off(OUT_AB);
    baDist(16.0);

    c_up(270);

    ResetTachoCount(OUT_AB);  //角度計測をリセット
    turnAng(140);
    c_up(340);
    ResetTachoCount(OUT_AB);  //角度計測をリセット
    turnAng(290);
    OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE);
    Off(OUT_AB);
    baDist(12.0);
    ResetTachoCount(OUT_AB);  //角度計測をリセット
        int h = searchDirection(20);
        if (h > 10){
           fwdDist(h-25.0);
    }
    ResetTachoCount(OUT_AB);  //角度計測をリセット
    turnAng(156);
    OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE);
    Off(OUT_AB);
    baDist(8.0);
    c_down(100);
    OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE);
    Off(OUT_AB);
    baDist(2.0);
    OnFwdSyncEx(OUT_AB,SPEED_SLOW,100,RESET_NONE);
    Off(OUT_AB);
    fwdDist(12.0);
}

詳しくは機体A担当チームのページを参考にして欲しい。

機体Bのプログラム

私たちのチームが担当した、機体Bの動作を行うためのプログラムである。

#define Apower 37
#define Bpower 34  //直進するのに必要な出力
#define ttime 695  //90度曲がるのに必要な時間
#define gotime 689  //20cm進むのに必要な時間
#define SPEED 50
#define SPEED_SLOW 30
sub go_for()//前進
{
    OnFwd(OUT_A,Apower);
    OnFwd(OUT_B,Bpower);
}
sub go_back()//後退
{
    OnFwd(OUT_A,-Apower);
    OnFwd(OUT_B,-Bpower);
}
sub turnL(float c)//左に曲がる(c×90)度
{
    OnFwd(OUT_A,Apower);OnFwd(OUT_B,-Bpower);
    Wait(c*ttime);
    Float(OUT_AB);  
}
sub turnR(float c) //右に曲がる(c×90)度
{
    OnFwd(OUT_A,-Apower);OnFwd(OUT_B,Bpower);
    Wait(c*ttime);
    Float(OUT_AB); 
}
int cyari(int lsw)//スタート時の位置調整
{
    ResetTachoCount(OUT_C);
    SetSensorLight(S1);
    SetSensorMode(S1,SENSOR_MODE_RAW);
    SetSensorTouch(S4);
    int i=1;
    int lsb=SENSOR_1;
    int lsb2=SENSOR_1-1;
    while (i==1){
        go_for();
        lsb2=SENSOR_1;
        NumOut(80,LCD_LINE4,lsb2);
        if (lsb>lsb2) {
            lsb=lsb2;
            }
        if (lsb+10<lsb2) {
            i=10;
            }
        if (lsb>lsw-150) {
            i=1;
            }    
        }
    Off(OUT_AB);
    NumOut(80,LCD_LINE1,lsw);
    NumOut(80,LCD_LINE2,lsb);
    return lsb;
}

この、スタート時の位置調整を行う関数についての詳しい説明は、 http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2FYoshi%2FMission3#d46bbd32を参照して欲しい。

const float diameter=5.45;//タイヤの直径
const float track=11.35;//タイヤのトレッド幅
const float pi=3.1415;//円周率

ここでは、機体の動きに直接関わる部分の機体のサイズと円周率を定義している。

void fwdDist(float d)//d cm進む
{
    long angle=d/(diameter*pi)*360.0;
    RotateMotorEx(OUT_AB,SPEED_SLOW,angle,0,true,true);
}

孤の長さ=円周率×直径×角度/360度なので、必要なタイヤの回転角度=(タイヤの孤の長さ/円周率×タイヤの直径)×360度となる。タイヤの孤の長さはdと同値。

void turnAng(long ang)// ang 度旋回
{
    long angle=track/diameter*ang;
    RotateMotorEx(OUT_AB,SPEED_SLOW,angle,100,true,true);
}

円周率×タイヤの直径×タイヤの回転角度/360度=円周率×タイヤのトレッド幅×旋回角度/360度であるので、整理すると、タイヤの回転角度=タイヤのトレッド幅×旋回角度/タイヤの直径が成り立つ。

sub arm(int c)//アームのモーターを回転させる
{
        RotateMotor(OUT_C, 40,c);
    Off(OUT_C);
}
task main ()
{
    ResetTachoCount(OUT_C);
    SetSensorLight(S1);
    SetSensorMode(S1,SENSOR_MODE_RAW);
    SetSensorTouch(S4);
    int lsw=SENSOR_1;
    int lsb=cyari(lsw);
    while(SENSOR_4==0) {}//ボタンを押すとwhileの条件を抜けてスタート
    fwdDist(15);//15cm前進
    Wait(15000);
    turnL(1.1);//99度回転
    PlaySound(SOUND_UP);//確認用サウンド
    Wait(1000);
    fwdDist(30);//30cm前進
    Wait(1000);
    turnAng(40);//40度回転
    PlaySound(SOUND_UP);//確認用サウンド

ここまでで、機体Bの正面が、紙コップ2の方を向いた。

    turnAng(180);//180度旋回
    Wait(1000);

アームはロボットの真後ろについているので、紙コップを掴む前に180度旋回する必要がある。

    fwdDist(15);//15cm前進
    Wait(1000);

アームが紙コップ2の方を向いたあと、すぐにアームを下げると紙コップ2にぶつかってしまうので、アームを少し遠ざける。

    arm(550);//アームを下げる
    Wait(1000);
    go_back();//後退
    Wait(2000);
    Off(OUT_AB);

これで、紙コップが手の中に収まった。

    arm(-180);//紙コップを掴む
    Wait(1000);

紙コップを掴んだことにより、これで先ほど説明した「可動な棒」がこれ以上動かなくなる。

    arm(-45);//アームを少し持ち上げる。
    Wait(1000);

これは、「ピンポン球をどう運ぶか」の項目で説明した、紙コップを少しだけ上げることでピンポン球を枠に入れるための準備をする動作である。

    turnL(1.05);//94.5度左旋回
    Wait(1000);

紙コップの高さを維持したまま、ピンポン球を枠の中に入れる動作である。

    arm(-200);//アームを上げる

紙コップのみを枠から出すために紙コップを高く上げる動作である。

    fwdDist(35);//35cm前進
    turnL(0.4);//36度左旋回
    Wait(1000);

ここまでで、紙コップ2をY地点まで移動させた。

    arm(100);//アームを下ろす

これで紙コップ2をY地点に置くことができた。

    PlaySound(SOUND_UP);//確認用サウンド
    SendRemoteNumber(1,MAILBOX1,1);//機体Aにメッセージを送る

紙コップ2を置くところまで完了したので、フィールドの左上で待機していた機体Aの動作を再開してもらうためのメッセージを送る。

    fwdDist(20);//20cm前進

機体Aの邪魔にならないようにフィールドの右上に避ける。

    while(SENSOR_4==0) {}//ボタンを押すとwhileの条件を抜けてプログラム全体が終了
}

結局、機体Bでは超音波センサーを使わなかった。ロボコンの前日までは、超音波センサーを使った位置判断のプログラムを使っていたが、原因不明のエラーが起きてしまい、結局、距離や角度を直接代入して制御するプログラムを使って紙コップまで移動させた。最も成功する可能性の高い数値をロボコンの直前まで実験して得、ロボコンに臨んだ。

反省と感想

今回の課題は、難易度が今までと比べて段違いに高かったのにもかかわらず、期末テストもあって、調整する時間が足りなかった。もっと成功する確率が高くなるように誤差を減らしてからロボコンに望みたかった。本番では、紙コップ1に入っていたピンポン球を2つ枠に入れ、障害物に1度当たってしまったため、3×2ー1で、今回私たちが得た基本点は5点にとどまった。 かなり苦労した割にはあまり満足いかない結果に終わってしまった。優勝したチームは、圧倒的な実力を見せた。そのチームは、機体の移動には主にライントレースを使っていた様であったので、私たちもライントレースをもっと使ったプログラムを作れば良かった。後期、ロボティクス入門ゼミを取った全体的な感想は、とにかく大変だったことに尽きる。プログラムのみならず、レポートも作らなければならなかったし、先生の指摘を踏まえて自分の説明の足りないところを考察するのもかなり難しかった。でも、最後に○を貰えたときの達成感はかなり大きかった。 これから小学校でもプログラミングが必修化されるようであり、まず教養としてプログラムの能力が求められる時代になってきた。そんな中で少しでもプログラムを身につけられて良かったと思っている。


添付ファイル: file#10.PNG 267件 [詳細] file#9.PNG 249件 [詳細] file#8.PNG 241件 [詳細] file#7.PNG 283件 [詳細] file#6.PNG 242件 [詳細] file#5.PNG 234件 [詳細] file#4.PNG 209件 [詳細] file#3.PNG 197件 [詳細] file#2.PNG 220件 [詳細] file#1.PNG 290件 [詳細] file初期位置.PNG 204件 [詳細] file機体を上から見た図.JPG 193件 [詳細] file超音波センサー.JPG 266件 [詳細] fileアームの歯車の図3.PNG 235件 [詳細] fileアームの歯車の図2.PNG 235件 [詳細] fileアームの歯車の図1.PNG 210件 [詳細] fileアーム俯瞰図2.PNG 233件 [詳細] fileアーム俯瞰図1.PNG 307件 [詳細] fileアームの歯車.JPG 219件 [詳細] file紙コップを上げる.png 275件 [詳細] fileアームの機構.JPG 66件 [詳細] fileピンポン球5.png 191件 [詳細] fileピンポン球4.png 195件 [詳細] fileピンポン球3.png 192件 [詳細] fileピンポン球2.png 202件 [詳細] fileピンポン球1.png 192件 [詳細] file課題3のコース.png 188件 [詳細]

トップ   編集 凍結 差分 履歴 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2018-02-13 (火) 23:55:35