#contents
ホーム[[2017b/Member]]
*1課題 [#hfe78483]
*1課題 [#k51f0fe3]

以下コース[[#i74e9ac3]]、ミッション[[#gfc134c7]]、第一コースの内容[[#q780070d]]は[[2017b/Mission2]]のからの引用です。

下の図のようなコースを各チームで作成し、「ミッション」を遂行するためのロボットを作成せよ。

** コース [#o4d47308]
** コース [#i74e9ac3]
黒線の幅は20mmでなるべく均等な濃さにすること。なお図の中の寸法の単位はcmで、黒線についてはその真ん中からの距離である。


#ref(2017b/Mission2/2017b-mission2.png,80%,課題2のコース)

** ミッション [#g924da6d]
** ミッション [#gfc134c7]

次のいずれかのコースで黒い線に沿って動き、紙コップを移動させるロボットを製作せよ。
(相棒と違うコースを選ぶこと。ただし第3コースは3名チーム用とする。)

**選択したコース [#g232e8c2]
**選択したコース [#v1a3a148]

第1コースを選びました。

*** 第1コースの内容 [#x04ca08e]
*** 第1コースの内容 [#q780070d]
+ Aをスタート
+ Bを直進
+ Cを右折
+ Fを直進
+ Rを左折(一時停止)
+ Pを直進
+ X地点の紙コップを取得してコースに戻る
+ Qを左折
+ Sを直進(一時停止)
+ Y地点に紙コップを置いてコースに戻る
+ Sを直進(一時停止)
+ Fを左折(一時停止)
+ Cを右折(一時停止)
+ D地点へ(ゴール)


*2ロボットの紹介 [#hfe78483]

基本のロボットに光センサーとアーム、起動スイッチを付けた構造になっています。

**ロボットの形 [#n402669d]

&ref(2017b/Member/Yoshi/Mission2/rosoku.JPG,60%,ロボの全体像1);
&ref(2017b/Member/Yoshi/Mission2/rozennpou.JPG,60%,ロボ全体像2);
&ref(2017b/Member/Yoshi/Mission2/rosoku.JPG,60%,ロボの全体像(前));
&ref(2017b/Member/Yoshi/Mission2/rozennpou.JPG,60%,ロボ全体像(横));
&ref(2017b/Member/Yoshi/Mission2/robokouhou.jpg,45%,ロボ全体像(後));

左の写真がロボットの全体を横から見た図、真ん中の写真が前方から見た図、右の写真が後方から見た図です。

NXに付属している冊子のロボットを基にして光センサーと接触センサーを取り付けました。

パーツの中でも最も重量のある電池ボックス(兼制御部)の位置を少しだけでも下げるために元は斜めに設置して合ったものを、倒してなるべく水平に設置するよう改良しました。これにより重心が下がりロボットの走行時、旋回時の安定性が向上します。配線の都合上、元の形とは上下を入れ替えています。

*3ロボット作成で工夫した点 [#r3f47f32]

&ref(2017b/Member/Yoshi/Mission2/rozenn.JPG,60%,ロボの全体像(前方から)3);
&ref(2017b/Member/Yoshi/Mission2/rozenn.JPG,60%,ロボの全体像(前2));
&ref(2017b/Member/Yoshi/Mission2/lightsenn.jpg,80%,ロボの光センサー);

光センサーと紙との距離を適切に設定することで取得する数値の安定化を図りました。
光センサーは最初に試作した時から安定して明るさが読み取れたのでそのまま変更せずに使用しました。念のため取り付ける位置を上下にずらして見ましたが、下にずらすと紙面に密着して光が十分に反射せず、上にずらすと紙面からの光が広がってしまいどちらの場合も望んでいた光の変化が得られなかったのでこの位置が最適だと判断しました。

またロボットの左側に接触センサーを取り付け、起動ボタンとして活用しました。
またロボットの左側に接触センサーを取り付け、起動ボタンとして活用しました。場所は特に制約もないので押しやすいところに設置しました。

アームは軽量化の為にシンプルな構造にしました。紙コップより若干大きい形とにして余裕を持たせています。

*4プログラム [#t1f283c1]
**定義した定数 [#o1329b1a]

プログラムを簡略化し、開発しやすくするためにいくつか定数を定義しました。
&br;

以下はその抜粋です

 #define Apower 37
 #define Bpower 34  //直進するのに必要な出力
 #define ttime 950  //90度曲がるのに必要な時間
 #define kanndo 30 //感度、ライントレースの反応の度合い
解説が必要なのは少ないでしょうが、まず上二つが移動用モーターの出力です。数値を左右で変えてまっすぐ先進するようにしました。
まず上二つが移動用モーターの出力です。数値を左右で変えてまっすぐ先進するようにしました。

3つめは旋回時に90°回転するための待ち時間です。

最後の『kanndo』はライントレースの感度です。

**定義したサブ関数 [#m9587eea]

定数と同様にいくつかサブ関数を定義しました。これにより可読性を高め、他人が見てもわかりやすくなります。

サブ関数の中には中にサブ関数を使用しているところがあるので分けて説明します。

***サブ関数その1 [#n49e72fe]
***サブ関数その1 (基本動作)[#n49e72fe]

 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)
 {
     OnFwd(OUT_A,Apower);OnFwd(OUT_B,-Bpower);
     Wait(c*ttime);
     Float(OUT_AB);  //左に曲がる(c×90)度
 }
 sub turnR(float c)
 {
     OnFwd(OUT_A,-Apower);OnFwd(OUT_B,Bpower);
     Wait(c*ttime);
     Float(OUT_AB);  //右に曲がる(c×90)度 
 }
 

ここで上げたサブ関数はロボットを動かす基本動作です。先進、後進、右旋回、左旋回のためのサブ関数です。前進、後進は開始のみ、旋回は回転する角度を指定して旋回します。
使用した変数
-c,float,旋回角度を指定するために使います。1で901度回転するようになっています

***サブ関数その2 [#zb1341d5]
ここで上げたサブ関数はロボットを動かす基本動作です。先進、後進、左旋回、右旋回のためのサブ関数です。前進、後進は開始のみ、旋回は回転する角度を指定して旋回します。

***サブ関数その2(ライントレース用) [#zb1341d5]

 sub turnR2(float d)
 {
     OnFwd(OUT_A,Apower-d*kanndo);OnFwd(OUT_B,Bpower+d*kanndo);
 }
 sub turnL2(float d)
 {
     OnFwd(OUT_A,Apower+d*kanndo);OnFwd(OUT_B,Bpower-d*kanndo);
 }
 sub turnR1(float d)
 {
     OnFwd(OUT_A,-d*kanndo);OnFwd(OUT_B,Bpower+d*kanndo);
 }
 sub turnL1(float d)
 {
     OnFwd(OUT_A,Apower+d*kanndo);OnFwd(OUT_B,-d*kanndo);
 }  

ここで上げたサブ関数はライントレースをする際に使います。黒線と白線の境界からのずれを『d』としてずれに応じて
使用した変数
-d,float,定数のkanndoと合わせて使うことで明るさの目標値とのずれに応じてモータの出力の変化量を決める。0〜1の範囲で変化する。

ここで上げたサブ関数はライントレースをする際に使います。『d』に応じて左右のタイヤの出力を調節して境界の方向に向かいます。

関数が4つあるのは、黒線の右側を走るときと左側を走る為に必要だからです。

***サブ関数その3(右折、左折用関数) [#n4b63b7c]

 sub sennkaiL(int t)
 {
     Off(OUT_AB);
     go_for();
     Wait(t);
     turnL(1);
 }
 sub sennkaiR(int t)
 {
     Off(OUT_AB);
     go_for();
     Wait(t);
     turnR(1);
 }

使用した変数
-t,int,サブ関数その3[[#n4b63b7c]]で使用。ロボットの向きを変えた後に前進する時間(ms)を入力する。

ロボットが右折、左折をするときには黒線の太さがあるためにただ旋回をするだけでなく、太さ分を移動する必要があります。その動きをサブ関数にしました。前進する時間を変更できるようにしたのは曲線を走行した後はロボットの向きが直線と比べて向きが違うのでそのずれを修正するためにしました。

***サブ関数その4(黒線の端から反対側への移動) [#c60ca96a]

 sub hennkouRL()
 {
     turnL(1);
     go_for();
     Wait(300);
     turnR(0.9);
 }
 sub hennkouLR()
 {
     turnR(0.9);
     go_for();
     Wait(300);
     turnL(1);
 }

この二つの関数は、ロボットが走る場所を移す関数です。黒線の右側から左に移ったり、左から右に移ったりします。

交差点の存在を検知するために、ロボットを黒線の右側と左側の変更しながら走る必要があったのでこのプログラムを作りました。

***サブ関数その5(アームの制御) [#u2c92213]

 sub armdown()
 {
     RotateMotor(OUT_C, 20,120);
     Wait(1000);
     Off(OUT_C);
 }
 sub armup()
 {
     RotateMotor(OUT_C, -20,120);
     Wait(1000);
     Off(OUT_C);
 }

このサブ関数は紙コップを捕まえて離すためのプログラムです。RetateMotorを使うことで角度を指定して動かしてずれが起きないようにしました。

***サブ関数その6(黒線の明るさの自動取得) [#d46bbd32]

 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;
 }

使用した変数
-lsw,int,自動取得した紙面上(白色)での明るさ。生データなのでおおよそ600台を示す。lightsensorwhiteの略。
-lsb,int,サブ関数のcyariにて最終的に黒線上での明るさになる。生データなのでおおよそ300台を示す。lightsensorblackの略。
-lsb2,int,サブ関数のcyariにてロボットが移動しているときの最新の明るさセンサーの数値を示す。生データなのでおおよそ300台を示す。lightsensorblackの略。
-i,int,黒線の明るさの計測が終わったか表す数値。1なら計測中、それ以外なら終了を表す。

黒線の明るさの最低値を自動取得するプログラムです。進みながら明るさを取得し比較し記録します。下図のように紙面の白色部分から動き始めて黒線を通過するように通過します。

&ref(2017b/Member/Yoshi/Mission2/light2.gif,80%,光センサーの動き);

実際のプログラムは黒線の明るさがわかった後は必要ないので、最低値から10以上大きい数値を検知したら終了するようになっています。最終的に紙面(白色)と黒線の明るさを画面に表示して、lsbの数値を返して終了です。

***サブ関数その7(ライントレースのプログラム。) [#mb31891c]

 sub linecomonn(int nL,int nR,int lsw,int lsb,int wall2,int point,int ftime,int waitt,int kankaku)
 {
    int deruta=(lsw-lsb)/2; //102
    float wall1=lsb+deruta;  //348+102
    int n=0;
    int ls=0;
    int kauntL=0;
    int kauntR=0;
    long told=0;
    long t0=CurrentTick();
    long tsub=0;
    while (n==0){
        ls=SENSOR_1;
        NumOut(80,LCD_LINE4,ls);
        if (ls>wall1) {
            if (lsw-ls<wall2) {
                turnL1((ls-wall1)/deruta);
                //PlaySound(SOUND_CLICK);
                if ((CurrentTick()-told>=kankaku)&&(CurrentTick()-t0>waitt)) {
                    kauntL=kauntL+1;
                    told=CurrentTick();
                    }
                turnL1((ls-wall1)/deruta);
                //PlaySound(SOUND_CLICK);
                }
            else{
                turnL2((ls-wall1)/deruta);
                }
            ls=SENSOR_1;
            }
        if (ls<=wall1) {
            if (ls-lsb<wall2) {
                if ((CurrentTick()-told>=kankaku)&&(CurrentTick()-t0>waitt)) {
                    kauntR=kauntR+1;
                    told=CurrentTick();
                    }
                turnR1((wall1-ls)/deruta);
                //PlaySound(SOUND_CLICK);
                }
            else{
                turnR2((wall1-ls)/deruta);
                }
            ls=SENSOR_1;
            }
        if(kauntL>=nL){
            n=1;
            }
        if(kauntR>=nR){
            n=1;
            }
        if ((n==1)&&(point==1)) {
            tsub=CurrentTick();
            point=2;
            n=0;
            }
        if ((n==1)&&(point==2)) {
            n=0;
            }
        if((point==2)&&(CurrentTick()-tsub>ftime)) {
            n=1;
            }
        NumOut(0,LCD_LINE1,kauntL);
        NumOut(0,LCD_LINE2,kauntR);
        }
 }

 sub linecomonn2(int nL,int nR,int lsw,int lsb,int wall2,int point,int ftime,int waitt,int kankaku)
 {
    int deruta=(lsw-lsb)/2; //102
    float wall1=lsb+deruta;  //348+102
    int n=0;
    int ls=0;
    int kauntL=0;
    int kauntR=0;
    long told=0;
    long t0=CurrentTick();
    long tsub=0;
    while (n==0){
        ls=SENSOR_1;
        NumOut(80,LCD_LINE4,ls);
        if (ls>wall1) {
            if (lsw-ls<wall2) {
                if ((CurrentTick()-told>=kankaku)&&(CurrentTick()-t0>waitt)) {
                    kauntR=kauntR+1;
                    told=CurrentTick();
                    //PlaySound(SOUND_CLICK);
                    }
                turnR1((ls-wall1)/deruta);
                //PlaySound(SOUND_CLICK);
                }
            else{
                turnR2((ls-wall1)/deruta);
                }
            ls=SENSOR_1;
            }
        if (ls<=wall1) {
            if (ls-lsb<wall2) {
                if ((CurrentTick()-told>=kankaku)&&(CurrentTick()-t0>waitt)) {
                    kauntL=kauntL+1;
                    told=CurrentTick();
                    }
                turnL1((wall1-ls)/deruta);
                //PlaySound(SOUND_CLICK);
                }
            else{
                turnL2((wall1-ls)/deruta);
                }
            ls=SENSOR_1;
            }
        if(kauntL>=nL){
            n=1;
            }
        if(kauntR>=nR){
            n=1;
            }
        if ((n==1)&&(point==1)) {
            tsub=CurrentTick();
            point=2;
            n=0;
            }
        if ((n==1)&&(point==2)) {
            n=0;
            }
        if((point==2)&&(CurrentTick()-tsub>ftime)) {
            n=1;
            }
        NumOut(0,LCD_LINE1,kauntL);
        NumOut(0,LCD_LINE2,kauntR);
        }
 }

ライントレースをするためのプログラムです。黒線の右側を走る用のものと、左側を走る用のものと二つ作りました。構造は後述します。

**実行するプログラム [#t144e1c4]

ここまでで定義したものを使って「埼玉」と書くために以下のようにtask mainを作りました
ここまでで定義したものを使って指定のルート通るために以下のようにtask mainを作りました

 task main ()
 {
    ResetTachoCount(OUT_C);
    penup();
    go_backY(0.9);  //調整1
    pendown();
    go_backX(0.5); //よこ(土)
    penup();
    go_forY(0.3);
    pendown();
    go_forX(0.5); //よこ
    penup();
    go_backX(0.25); //よこ
    pendown();
    go_backY(0.5); //縦
    penup();
    go_backX(0.6);//移動to大
    go_backY(0.4);
    pendown();
    go_forY(0.2); //たて()大
    go_XY(0.25); //斜め1
    pendown();
    go_XY2(0.25); //斜め2
    go_forX(0.25);
    pendown();
    go_backX(0.5); //横(大)
    penup();
    go_forY(0.5); //移動to可
    go_forX(0.4);
    pendown();
    go_backX(0.5); //可はじめ
    go_forY(0.5);
    penup();
    go_forX(0.4);
    go_backY(0.1);
    pendown();
    go_backY(0.3); //口
    go_backX(0.3);
    go_forY(0.3);
    go_forX(0.3);
    penup();
    go_backX(0.9); //移動to(玉)
    go_forY(0.5);
    go_backY(0.3);
    pendown();
    go_backY(1.2);//  |
    penup();
    go_forX(0.3);
    pendown();
    go_backX(0.6);//  --
    penup();
    go_forY(0.6);
    pendown();
    go_forX(0.6);//  --
    penup();
    go_forY(1);
    go_backY(0.3);
    pendown();
    go_backX(0.6);//  --
    penup();
    go_forX(0.2);
    go_backY(0.4);
    pendown();
    go_XY2(0.25);//斜め(玉)
    penup();
    SetSensorLight(S1);
    SetSensorMode(S1,SENSOR_MODE_RAW);
    SetSensorTouch(S4);
    armup();  //初期位置を下げた状態で開始している
    int lsw=SENSOR_1;  //紙面(白色)の明るさ測定
    int lsb=cyari(lsw);  //黒線の明るさ測定
    while(SENSOR_4==0) {}  //準備完了、ボタンが押されるまで待機
    linecomonn(50,2,lsw,lsb,30,0,0,1000,2000);  //A→C
    PlaySound(SOUND_UP);
    sennkaiR(200);
    linecomonn(1,50,lsw,lsb,30,1,2000,1500,2000);  //C→急カーブと急カーブの真ん中
    PlaySound(SOUND_UP);
    hennkouRL();
    linecomonn2(50,1,lsw,lsb,30,1,2000,1500,2000);  //急カーブと急カーブの真ん中→Eの手前
    PlaySound(SOUND_UP);
    hennkouLR();
    linecomonn(50,1,lsw,lsb,30,0,0,1000,2000);  //Eの手前→E
    PlaySound(SOUND_UP);
    sennkaiR(0);
    hennkouRL();
    linecomonn2(1,1,lsw,lsb,80,0,0,1000,1000);  //E→R
    PlaySound(SOUND_UP);
    Off(OUT_AB);
    Wait(2000);                //円開始
    sennkaiL(200);
    linecomonn2(1,50,lsw,lsb,20,1,700,1000,1500);  //R→P
    Off(OUT_AB);
    turnR(1);  //紙コップをとる
    armdown();
    turnL(0.9);
    linecomonn2(1,50,lsw,lsb,20,0,0,1000,1500);  //P→Q
    PlaySound(SOUND_UP);
    sennkaiL(500);
    linecomonn(50,1,lsw,lsb,30,0,0,1000,1000);  //Q→S
    Off(OUT_AB);
    PlaySound(SOUND_UP);
    Wait(2000);
    linecomonn(50,1,lsw,lsb,30,0,0,1000,800);  //S→S(一周)
    turnR(2.7);  //紙コップを離す
    armup();
    turnR(1.7);
    PlaySound(SOUND_UP);
    Wait(2000);
    go_for();
    Wait(200);
    Float(OUT_AB);
    linecomonn(1,1,lsw,lsb,40,0,0,800,800);  //S→F
    Off(OUT_AB);
    PlaySound(SOUND_UP);
    Wait(2000);
    sennkaiL(400);
    linecomonn(50,1,lsw,lsb,30,0,0,1000,2000);  //F→C
    Off(OUT_AB);
    PlaySound(SOUND_UP);
    Wait(2000);
    turnR(1);
    go_for();  //直進してDに
    Wait(2100);
    Off(OUT_AB);
    while(SENSOR_4==0) {  //終了
    PlaySound(SOUND_UP);
    Wait(1500);
 }
 }

特出して書くことはないと思いますが、「埼」から「玉」に移動する際にY軸の位置をレールの端に合わせてずれにくいようにしています
通過するコースごとにそれぞれ合わせて数値を決定しました。

*5実行結果 [#yce51899]

こうして作ったロボットとプログラムを動かしてみた。まず倍率を0.8倍にして動かした。
こうして作ったロボットとプログラムを動かしてみた。
&br;
その結果
その結果は発表会のように2回目で成功した。一回目は誤検知を防止する部分が過剰反応をして交差点認識を破棄してしまい成功しなっかた。

&ref(2017b/Member/Yoshi/Mission1/kekka1.jpg,87%,埼玉1);
&br;
のように最後の一画が少しはみ出た。なので今度は倍率を0.5倍にして動かした。
&br;
その結果
*6ライントレースの方法と交差点認識 [#x945e665]

&ref(2017b/Member/Yoshi/Mission1/kekka2.jpg,87%,埼玉1);
&br;
となりはみ出ずに書くことができた。
ここでは私が行ったライントレースの方法について説明します。
**ライントレースの変遷 [#w8eb3f07]

まずライントレースの方法として比例制御式を採用しました、理由はほかの数値ごとに区切ってモーターの出力を決定するよりも構造的に調整が比例定数一つで済むので簡単だからです。そうして試作したのが下記のプログラムです
    int deruta=(lsw-lsb)/2;
    float wall1=lsb+deruta;
    int wall2=30;
    while (true){
        ls=SENSOR_1;
        NumOut(80,LCD_LINE4,ls);
        while (ls>wall1) {
            if (lsw-ls<wall2) {
                PlaySound(SOUND_CLICK);
                turnL1((ls-wall1)/deruta);
                }
            else{
            turnL2((ls-wall1)/deruta);
                }
            ls=SENSOR_1;
            }
        while (ls<=wall1) {
            if (ls-lsb<wall2) {
                PlaySound(SOUND_CLICK);
                turnR1((wall1-ls)/deruta);
                }
            else{
            turnR2((wall1-ls)/deruta);
                }
            ls=SENSOR_1;
            }
        }
このころはまだ試作段階でtask main に直接書いていました。設定した変数は次の通りです。

-deruta,int,lswとlsbの差の半分、wall1を設定したり、dを求める時に使う
-wall1,int,lswとlsbの平均値で白色と黒色の境界部分の推定値、この数値をlsと比較してロボットがどちら側によっているか推測する。
-ls.,int,現在の明るさセンサーの数値、lightsensorの略
-wall2,int,中心とのずれが非常に大きい時に大きく回るための数値、実質的な交差点認識の基準値

基本的には比例制御をしていますが、ロボットが大きくずれたときに例外としてturn○1を起動して大きく回転するようにしました。大きくずれたという判断の基準としてlsがlswかlsbからwall2以内になっているかを基準としました。wall2の30数値はderutaがおおよそ100程度なので適当に3割としました。本当は2次関数的に制御する方が構造席にシンプルですが、係数の最適化が困難ではないかと考えたのでこうしました。

この状態でライントレースを行った結果このようになりました。

&ref(2017b/Member/Yoshi/Mission2/Video.gif,120%,滑らか);

カーブもスムーズに動きまた交差点は直進しました。またwall2の条件を満たすのが交差点や急カーブなどの曲率が高いところのみということが判明しました。そのためwall2を交差戦認識の基準としました。その結果が下記のプログラムです。
 sub linecomonn(int nL,int nR,int lsw,int lsb,int wall2)
 {
    int deruta=(lsw-lsb)/2;
    float wall1=lsb+deruta;
    int n=0;
    int ls=0;
    int kauntL=0;
    int kauntR=0;
    long told=0;
    while (n==0){
        ls=SENSOR_1;
        NumOut(80,LCD_LINE4,ls);
        while (ls>wall1) {
            if (lsw-ls<wall2) {
                turnL1((ls-wall1)/deruta);
                PlaySound(SOUND_CLICK);
                if (CurrentTick()-told>=2000) {
                    kauntL=kauntL+1;
                    told=CurrentTick();
                    PlaySound(SOUND_CLICK);
                    }
                }
            else{
                turnL2((ls-wall1)/deruta);
                }
            ls=SENSOR_1;
            }
        while (ls<=wall1) {
            if (ls-lsb<wall2) {
                turnR1((wall1-ls)/deruta);
                PlaySound(SOUND_CLICK);
                if (CurrentTick()-told>=2000) {
                    kauntR=kauntR+1;
                    told=CurrentTick();
                    }
                }
            else{
                turnR2((wall1-ls)/deruta);
                }
            ls=SENSOR_1;
            }
        if(kauntL>=nL){
            n=1;
            }
        if(kauntR>=nR){
            n=1;
            }
        NumOut(0,LCD_LINE1,kauntL);
        NumOut(0,LCD_LINE2,kauntR);
        }
 }

ライントレースのサブ関数を作りました、また交差点を検知した(wall2の条件を満たした)時のカウント機能を付けてきて、規定回数以上交差点認識したらライントレースを終了するようにしました。以下が追加した変数です。

-nL,int,左側にはみ出しすぎた結果交差点認識した回数の上限値、この数値を超えて左側で交差点認識をするとライントレースが止まる。
-nR,int,右側にはみ出しすぎた結果交差点認識した回数の上限値、この数値を超えて右側で交差点認識をするとライントレースが止まる。
-n,int,ライントレースを続けるか決定する数値。0なら継続、それ以外なら終了を表す。
-kauntL,int,左側で交差点認識した回数を表す。
-kauntR,int,右側で交差点認識した回数を表す。
-told,long,前回交差点を検知した時の時間を表す。

ライントレースの開始直後は位置が不安定でwaal2の条件を満たして誤って交差点認識をすることがあったので、例外処理としてライントレース開始2秒間は交差点認識のカウントをしないようにしました。また交差点を直進するさいに複数回交差点検地をする可能性があったので、同様に例外処理として検知してから2秒間は交差点認識のカウントをしないようにしました。

次は交差点を認識した後指定した時間を経過してからライントレースを終了するものを考えました。
 sub linecomonn(int nL,int nR,int lsw,int lsb,int wall2,int point)
 {
    int deruta=(lsw-lsb)/2;
    float wall1=lsb+deruta;
    int n=0;
    int ls=0;
    int kauntL=0;
    int kauntR=0;
    long told=0;
    long t0=CurrentTick();
    long tsub=0;
    while (n==0){
        ls=SENSOR_1;
        NumOut(80,LCD_LINE4,ls);
        while (ls>wall1) {
            if (lsw-ls<wall2) {
                turnL1((ls-wall1)/deruta);
                PlaySound(SOUND_CLICK);
                if ((CurrentTick()-told>=2000)&&(CurrentTick()-t0>1000)) {
                    kauntL=kauntL+1;
                    told=CurrentTick();
                    }
                turnL1((ls-wall1)/deruta);
                PlaySound(SOUND_CLICK);
                }
            else{
                turnL2((ls-wall1)/deruta);
                }
            ls=SENSOR_1;
            }
        while (ls<=wall1) {
            if (ls-lsb<wall2) {
                if ((CurrentTick()-told>=2000)&&(CurrentTick()-t0>1000)) {
                    kauntR=kauntR+1;
                    told=CurrentTick();
                    }
                turnR1((wall1-ls)/deruta);
                PlaySound(SOUND_CLICK);
                }
            else{
                turnR2((wall1-ls)/deruta);
                }
            ls=SENSOR_1;
            }
        if(kauntL>=nL){
            n=1;
            }
        if(kauntR>=nR){
            n=1;
            }
        if ((n==1)&&(point==1)) {
            tsub=CurrentTick();
            point=2;
            n=0;
            }
        if ((n==1)&&(point==2)) {
            n=0;
            }
        if((point==2)&&(CurrentTick()-tsub>2500)) {
            n=1;
            }
        NumOut(0,LCD_LINE1,kauntL);
        NumOut(0,LCD_LINE2,kauntR);
        }
 }

結果的に論理に欠陥があるのか考えたようにいかず使わない機能となりました。追加した変数は以下の通りです。

-point,int,今回追加した機能の状態を示す関数、0なら無効、1なら有効かつnL,nRの条件達成待ち、2なら待機時間経過中を意味する。
-t0,long,ライントレース開始時の時間
-tsub,long,nL,nRで指定した条件を満たした時刻

変更した点は、交差点認識の例外をライントレース開始から1秒間、前回検知してから2秒間に変更したことです。

次の改良として交差点認識の例外条件を通過する経路の特性に合わせて変更できるように変数にしました
  sub linecomonn(int nL,int nR,int lsw,int lsb,int wall2,int point,int waitt,int kankaku)
  {
    int deruta=(lsw-lsb)/2;
    float wall1=lsb+deruta;
    int n=0;
    int ls=0;
    int kauntL=0;
    int kauntR=0;
    long told=0;
    long t0=CurrentTick();
    long tsub=0;
    while (n==0){
        ls=SENSOR_1;
        NumOut(80,LCD_LINE4,ls);
        while (ls>wall1) {
            if (lsw-ls<wall2) {
                turnL1((ls-wall1)/deruta);
                //PlaySound(SOUND_CLICK);
                if ((CurrentTick()-told>=kankaku)&&(CurrentTick()-t0>waitt)) {
                    kauntL=kauntL+1;
                    told=CurrentTick();
                    }
                turnL1((ls-wall1)/deruta);
                PlaySound(SOUND_CLICK);
                }
            else{
                turnL2((ls-wall1)/deruta);
                }
            ls=SENSOR_1;
            }
        while (ls<=wall1) {
            if (ls-lsb<wall2) {
                if ((CurrentTick()-told>=kankaku)&&(CurrentTick()-t0>waitt)) {
                    kauntR=kauntR+1;
                    told=CurrentTick();
                    }
                turnR1((wall1-ls)/deruta);
                PlaySound(SOUND_CLICK);
                }
            else{
                turnR2((wall1-ls)/deruta);
                }
            ls=SENSOR_1;
            }
        if(kauntL>=nL){
            n=1;
            }
        if(kauntR>=nR){
            n=1;
            }
        if ((n==1)&&(point==1)) {
            tsub=CurrentTick();
            point=2;
            n=0;
            }
        if ((n==1)&&(point==2)) {
            n=0;
            }
        if((point==2)&&(CurrentTick()-tsub>2500)) {
            n=1;
            }
        NumOut(0,LCD_LINE1,kauntL);
        NumOut(0,LCD_LINE2,kauntR);
        }
  }

サブ関数の引数にwaittとkankakuを追加しました。
-waitt,int,交差点認識の例外がライントレース開始から何秒間か決める引数
-kankaku,int,交差点認識の例外が前回検知してから何秒間かきめる引数

変更点は特にないです。

  sub linecomonn(int nL,int nR,int lsw,int lsb,int wall2,int point,int ftime,int waitt,int kankaku)
  {
    int deruta=(lsw-lsb)/2;
    float wall1=lsb+deruta;
    int n=0;
    int ls=0;
    int kauntL=0;
    int kauntR=0;
    long told=0;
    long t0=CurrentTick();
    long tsub=0;
    while (n==0){
        ls=SENSOR_1;
        NumOut(80,LCD_LINE4,ls);
        if (ls>wall1) {
            if (lsw-ls<wall2) {
                turnL1((ls-wall1)/deruta);
                //PlaySound(SOUND_CLICK);
                if ((CurrentTick()-told>=kankaku)&&(CurrentTick()-t0>waitt)) {
                    kauntL=kauntL+1;
                    told=CurrentTick();
                    }
                turnL1((ls-wall1)/deruta);
                PlaySound(SOUND_CLICK);
                }
            else{
                turnL2((ls-wall1)/deruta);
                }
            ls=SENSOR_1;
            }
        if (ls<=wall1) {
            if (ls-lsb<wall2) {
                if ((CurrentTick()-told>=kankaku)&&(CurrentTick()-t0>waitt)) {
                    kauntR=kauntR+1;
                    told=CurrentTick();
                    }
                turnR1((wall1-ls)/deruta);
                PlaySound(SOUND_CLICK);
                }
            else{
                turnR2((wall1-ls)/deruta);
                }
            ls=SENSOR_1;
            }
        if(kauntL>=nL){
            n=1;
            }
        if(kauntR>=nR){
            n=1;
            }
        if ((n==1)&&(point==1)) {
            tsub=CurrentTick();
            point=2;
            n=0;
            }
        if ((n==1)&&(point==2)) {
            n=0;
            }
        if((point==2)&&(CurrentTick()-tsub>ftime)) {
            n=1;
            }
        NumOut(0,LCD_LINE1,kauntL);
        NumOut(0,LCD_LINE2,kauntR);
        }
  }
新しく追加したことはなく、交差点検知直後にライントレースが終了するようにモーターを回す指令の入ったwhil文をif文に変更しました。


トップ   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS