目次

課題の説明

メンバー

  • bgpat: スタート→ゴール
  • 只の某: ゴール→スタート

コースの作成

course.png

正確なコースを作り、作業時間を短縮するために、 このような画像ファイルを作成しました。

これを何枚かのA4用紙に分けて印刷して台紙に貼り付け、 インクの薄い部分は黒いペンで塗りました。

しかし、貼り付ける作業に時間がかかってしまい、 あまいいい方法とは言えませんでした。

また、セロハンテープを使って貼り付けたため、テープの部分が少しだけ明るくなってしまいました。 しきい値の調整は難しかったですが、トレースにはほとんど問題ありませんでした。

ロボット本体

ドライブベース

説明書の見本をもとに組み立てました。

見本よりもタイヤの幅を狭くしたことで小回りが効き、 光センサーを前輪に近づけることでトレースがしやすくなりました。

パーツがたくさん余ったので補強をしっかりしておきました。

body.JPG

アーム

アームはNXT本体の右、車体の右肩にあたる部分に取り付けました。 モーターが車体からはみ出してしまうため、多めの補強を施しました。

アームを上から下ろすことでピンポン玉を確保し、坂に押し上げます。 このとき、ピンポン玉の位置がずれても確保できるように坂を3箇所作りました。

ピンポン玉を確保したままゴールまで運びます。 シュートするときは、アームを上げるとピンポン玉が坂を転がります。

DSC01846.jpg

プログラム

定数

車体の構成に関する部分です

// 入出力系
#define LEFT_MOTER OUT_C
#define RIGHT_MOTER OUT_A
#define LIGHT_SENSOR S3

// スピード
#define SPEED 40

// 回転センサーの係数
#define MOTER_ROTATE (1 / 14.55)

// 車体の幅
#define BODY_WIDTH 12.2

マクロ

コードを簡略化するためのマクロです

// 回転センサーの値を取得する
#define LeftRotation() MotorRotationCount(OUT_C)
#define RightRotation() MotorRotationCount(OUT_A)

移動用モーター制御用関数

OnFwdとOnRevとOffをまとめた関数です。 スピードによって動作が変わるようになっています。

 /*
  * モーターを回転させる
  * スピードが負のときは逆回転
  * 0のときは停止
 */
void On(int leftSpeed, int rightSpeed){
    if(leftSpeed > 0){
        OnFwd(LEFT_MOTER, leftSpeed);
    }else if(leftSpeed){
        OnRev(LEFT_MOTER, -leftSpeed);
    }else{
        Off(LEFT_MOTER);
    }
    if(rightSpeed > 0){
        OnFwd(RIGHT_MOTER, rightSpeed);
    }else if(rightSpeed){
        OnRev(RIGHT_MOTER, -rightSpeed);
    }else{
        Off(RIGHT_MOTER);
    }
}

明るさ判定関数

ライントレースに使います。

授業では触れていませんが、"switch"という構文を使うことを想定しています。 光センサーのしきい値はここに記入します。

/*
 * 明るさを 0 〜 4 で返す
 * 暗い < 0 1 2 3 4 > 明るい
 */
bool Light(){
    if(SENSOR_3 < 40){
        return 0;
    }else if(SENSOR_3 < 45){
        return 1;
    }else if(SENSOR_3 < 50){
        return 2;
    }else if(SENSOR_3 < 55){
        return 3;
    }else{
        return 4;
    }
}

メイン関数のコード

解説はこちら

コード全体が見たい人以外は飛ばしてください。 解説には部分ごとにコードを載せています。

task main(){
    //通過ポイントを数えるための変数
    int corner = 0;
    
    // 車体の向き
    float rotation = 0;
    
    // 光センサー初期化
    SetSensorLight(LIGHT_SENSOR);
    
    // スタートから出る
    On(SPEED, SPEED);
    until(Light() == 0);
    until(Light() == 4);
    
    // トレース開始
    while(corner < 8){
        Wait(1);
        switch(Light()){
            case 0:  //左回転
                On(-SPEED, SPEED);
                break;
            case 1:  //左折
                On(0, SPEED);
                break;
            case 2:  //直進
                On(SPEED, SPEED);
                break;
            case 3:  //右折
                On(SPEED, 0);
                break;
            case 4:  //右回転
                On(SPEED, -SPEED);
                break;
        }
        
        // 回転センサーの値を取得する
        int _tl = LeftRotation();
        int _tr = RightRotation();
        
        // 差から角度を求める
        rotation += 360 * (_tr - tr - _tl + tl) * MOTER_ROTATE / (2 * BODY_WIDTH * PI);
        tl = _tl;
        tr = _tr;
        
        // 通過ポイント
        switch(corner){
            case 0:  // 1回目のヘアピンカーブ
                if(rotation > 90){
                    PlayTone(TONE_C4, 100);
                    corner++;
                }
                break;
            case 1:  // 1回目の交差点
                if(rotation < 10){
                    PlayTone(TONE_D4, 100);
                    On(SPEED, SPEED);
                    until(Light() == 0);
                    until(Light() >= 2);
                    corner++;
                }
                break;
            case 2:  // ピンポン玉を取る位置
                if(rotation < -40){
                    PlayTone(TONE_E4, 100);
                    Wait(500);
                    On(SPEED, SPEED);
                    RotateMotor(OUT_B, 10, 90);
                    On(-SPEED, -SPEED);
                    Wait(500);
                    On(SPEED, 0);
                    until(Light() < 4);
                    corner++;
                }
                break;
            case 3:  // 大きいカーブ終了
                if(rotation < -180){
                    PlayTone(TONE_F4, 100);
                    corner++;
                }
                break;
            case 4:  // 2回目の交差点
                if(rotation > -180){
                    PlayTone(TONE_G4, 100);
                    On(SPEED, 0);
                    until(Light() == 4);
                    On(SPEED, SPEED);
                    until(Light() == 3);
                    corner++;
                }
                break;
            case 5:  // 3回目の交差点
                if(rotation > 10){
                    PlayTone(TONE_A4, 100);
                    On(SPEED, 0);
                    until(Light() >= 3);
                    corner++;
                }
                break;
            case 6:  // 2回目のヘアピンカーブ
                if(rotation > 90){
                    PlayTone(TONE_B4, 100);
                    corner++;
                }
                break;
            case 7:  // ゴール
                if(rotation < -15){
                    PlayTone(TONE_C5, 100);
                    On(0, 0);
                    RotateMotor(OUT_B, 100, -90);
                    corner++;
                }
                break;
        }
   }
   Wait(2000);
}

メイン関数の解説

変数とセンサーの初期化

corner変数には通過ポイントの番号を、 rotation変数には車体の向きを格納します。

トレースに使用する光センサーを初期化します。

//通過ポイントを数えるための変数
int corner = 0;

// 車体の向き
float rotation = 0;

// 光センサー初期化
SetSensorLight(LIGHT_SENSOR);

スタートから出る

黒い線を跨ぐまで直進してスタートエリアから出ます。

On(SPEED, SPEED);
until(Light() == 0);
until(Light() == 4);

ライントレースのループ処理

スタートエリアから出るとライントレースを開始します。 ゴールに到達してシュートするまで繰り返すループ関数です。

1ミリ秒で1ステップ進みます。

シュートをする前にプログラムが終了しないようにするために ループのあとにWaitを入れました。

while(corner < 8){
    Wait(1);

    /*
     * ライントレース
     */

    /*
     * 車体の向きを求める
     */

    /*
     * 通過ポイントごとの処理(交差点を渡るなど)
     */

}
Wait(2000);

ライントレース

明るさを0から4の5段階に分けて、0と4のときはその場で回転、1と3のときは曲がりながら進む、2のときは直進します。

Light関数についてはこちらをご覧ください。

switch(Light()){
    case 0:  //左回転
        On(-SPEED, SPEED);
        break;
    case 1:  //左折
        On(0, SPEED);
        break;
    case 2:  //直進
        On(SPEED, SPEED);
        break;
    case 3:  //右折
        On(SPEED, 0);
        break;
    case 4:  //右回転
        On(SPEED, -SPEED);
        break;
}

車体の向きを求める

rotation.png

この画像のように車体が進んだとき、 回転した角度θは方程式を解くと

θ = {(外側のタイヤが進んだ距離) - (内側のタイヤが進んだ距離)} / (車体の幅)

となります。 これを足していくと車体が向いている方向を求めることができます。

タイヤが進んだ距離はモーターの回転センサーを使って算出しています。 車体の幅は定規で測りました。 0°はスタートした向きとなります。

// 回転センサーの値を取得する
int _tl = LeftRotation();
int _tr = RightRotation();

// 差から角度を求める
rotation += 360 * (_tr - tr - _tl + tl) * MOTER_ROTATE / (2 * BODY_WIDTH * PI);
tl = _tl;
tr = _tr;

通過ポイントごとの処理

通過ポイントを設定して、車体の向きからコースのどのあたりにいるかを求めています。 デバッグ用に音を鳴らしています。

詳細は後述します。

point.png
switch(corner){
    case 0:
        /*
         * あとで解説
         */
        break;
}

  1. 1回目のヘアピンカーブ

    車体の向きが90°を超えると通過ポイント1を超えたことにします。

    if(rotation > 90){
        PlayTone(TONE_C4, 100);
        corner++;
    }
    break;

  2. 1回目の交差点

    車体の向きが10°未満になると通過ポイント2を超えたことにして

    交差点を超えます。

    スタートと同じように線を超えるまで直進して交差点を超えます。

    if(rotation < 10){
        PlayTone(TONE_D4, 100);
        On(SPEED, SPEED);
        until(Light() == 0);
        until(Light() >= 2);
            corner++;
        }
    }
    break;

  3. ピンポン玉を取る位置

    1回目の交差点を超えて

    車体の向きが-40°(320°)より小さくなるとると

    ピンポン玉を拾います。

    直進しながらアームを下げ、

    ボールを確保すると元の位置までバックします。

    右に進みながらライントレースに復帰します。

    if(rotation < -40){
        PlayTone(TONE_E4, 100);
        Wait(500);
        On(SPEED, SPEED);
        RotateMotor(OUT_B, 10, 90);
        On(-SPEED, -SPEED);
        Wait(500);
        On(SPEED, 0);
        until(Light() < 4);
        corner++;
    }
    break;

  4. 大きいカーブ終了

    車体の向きがスタートしたときと反対になると大きいカーブが終了したとみなします。

    if(rotation < -180){
        PlayTone(TONE_F4, 100);
        corner++;
    }
    break;

  5. 2回目の交差点

    大きいカーブが終了して、車体が左に向くと2回目の交差点を越えます。

    進み方や光センサーの条件を変えていますが、1回目の交差点を超えるときとほとんど同じです。

    if(rotation > -180){
        PlayTone(TONE_G4, 100);
        On(SPEED, 0);
        until(Light() == 4);
        On(SPEED, SPEED);
        until(Light() == 3);
        corner++;
    }
    break;

  6. 3回目の交差点

    車体の向きが10°を超えると3回目の交差点です。

    本来は0°ですが、実際動かすと10°くらいが丁度よくなります。

    この手前に直角カーブがありますが、ライントレースの処理だけで通過することができます。

    方向を修正するように右に進み、交差点を越えていきます。

    線から出るとライントレースに戻ります。

    if(rotation > 10){
        PlayTone(TONE_A4, 100);
        On(SPEED, 0);
        until(Light() >= 3);
        corner++;
    }
    break;

  7. 2回目のヘアピンカーブ

    1回目のヘアピンカーブと同じように車体の向きが90°を超えると7個目の通過ポイントです。

    if(rotation > 90){
        PlayTone(TONE_B4, 100);
        corner++;
    }
    break;

  8. ゴール

    車体の向きが15°より小さくなるとゴールです。

    車体はゴールの方を向いているので、アームを上げてシュートします。

    変数cornerが8になるのでループから抜けて、プログラムが終了します。

    if(rotation < -15){
        PlayTone(TONE_C5, 100);
        On(0, 0);
        RotateMotor(OUT_B, 100, -90);
        corner++;
    }
    break;

工夫した点

交差点の判定

課題のコースは決まっているので、 変曲点や交差点に通過ポイントを設定することで コース上のどのあたりを走行しているかを知ることができます。

例)1回目の交差点を超えるとき
1回目交差点を普通にトレースすると左に曲がるので、0°より大きくなると交差点ということになります。
しかし、0°より大きくなる点は1回目の交差点までに存在しているので「スタートしてから車体の向きが90°になる点を通過すると」という条件を付け加えます。
このように通過ポイントを設定することで交差点を判定することができます。

はじめは、多くの人が使っている、カウンターの値が一定値を超えると直角(交差点と直角カーブ)と判定する方法を使おうと考えましたが、 実際試してみると、直角の前が直進の場合にうまく判定できなかったので 別の方法として向きを使う方法を考えました。

メリットは3つあります。 1つ目は判定の精度です。 向きの算出は、本来の向きと多少のズレがあるものの毎回同じ値になるので 判定に失敗することはほとんどありません。 トレースによる誤差も光センサーの位置やしきい値の調整のおかげで吸収できているようでした。 2つ目は判定までの時間です。 交差点の判定がすぐにできるのでスムーズにトレースすることができます。 3つ目は調整にかかる時間です。 通過ポイントの角度を指定するのですが、 コースを見ながら直感的に指定して、少し調整するだけで済み、 カウンターを使う方法よりも調整の時間を短縮できます。

デメリットは、スタートする向きがずれるとうまく判定できないことです。 スタートラインに坂を合わせることで克服しました。

坂を使ったシュート機構

NXTはモーターを接続する場所が3つしかなく、トレースに2個のモーターを使うのでピンポン玉の確保やシュートは1個のモーターだけで行う必要があります。

しかし、シュート機構が思いつかなかったので、車体の前に坂を付けてアームをあげるとピンポン玉が前に転がっていくようにしました。

ピンポン玉を確保するときはアームをおろしてピンポン玉を坂に押し上げます。坂は画像のように3箇所用意し、トレースが多少ズレてもピンポン玉を確保しシュートできるようにしました。坂を増やすことでシュートできる確率が格段に増えました。

saka.png

感想

ピンポン玉をゴールにシュートするところまでできたのでよかったです。

コメント



添付ファイル: filerotation.png 267件 [詳細] filesaka.png 295件 [詳細] fileDSC01846.jpg 312件 [詳細] filebody.JPG 274件 [詳細] filepoint.png 269件 [詳細] filecourse.png 290件 [詳細]

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 単語検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2014-07-29 (火) 19:52:20 (1853d)