#author("2021-09-09T19:28:50+09:00","d7","d7")
目次
#contents
*[[課題の説明>2014a/Mission1]] [#ofbcff9d]
*メンバー [#tbb423e0]
-bgpat: 主にプログラム担当 スタート→ゴール
-只の某: 主にロボット作成担当 ゴール→スタート
-bgpat: スタート→ゴール
-[[只の某>2014a/Member/tadanobo/Mission1]]: ゴール→スタート
*コースの作成 [#b0a98824]
&ref(2014a/Member/bgpat/Mission1/course.png,400x300);
&ref(2014a/Member/bgpat/Mission1/course.png,50%);

このような画像ファイルを作成して何枚かのA4用紙に分けて印刷し、台紙に貼り付けました。
薄い部分は黒いペンで塗りつぶしました。
正確なコースを作り、作業時間を短縮するために、
このような画像ファイルを作成しました。

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

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

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

*ロボット本体 [#i40141cf]
**ドライブベース [#i6612d7b]
説明書の見本をもとに組み立てました。

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

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

&ref(2014a/Member/bgpat/Mission1/body.JPG,100%);

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

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

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

&ref(2014a/Member/bgpat/Mission1/DSC01846.jpg,20%);

*プログラム [#xe5408e4]
**定数の定義 [#rc89c212]
コースやロボットによって値は異なります。
 /* トレースのスピード */
 #define SPEED_HIGH 40
 #define SPEED_MEDIUM 30
 #define SPEED_LOW 25
 #define SPEED_ZERO 0

**定数 [#i0ee2036]
車体の構成に関する部分です

 // 入出力系
 #define LEFT_MOTER OUT_C
 #define RIGHT_MOTER OUT_A
 #define LIGHT_SENSOR S3
 
 /* 直角の制御 */
 #define TURN_COUNT_MAX 300
 #define GO_COUNT_MAX 150
 // スピード
 #define SPEED 40
 
 /* アームの制御 */
 #define BALL_DISTANCE 9
 #define BALLBAR_DOWN 100
 #define BALLBAR_UP 1000
 #define BALLBAR_WAIT 1000
 #define BALLBAR_SPEED 50
**関数の定義 [#fabca395]
コードの簡略化のための関数です。
 /*
  * 左右のモーターを回す
  * スピードが正のときは前、負のときは後に回す
 // 回転センサーの係数
 #define MOTER_ROTATE (1 / 14.55)
 
 // 車体の幅
 #define BODY_WIDTH 12.2

**マクロ [#sf63adb2]
コードを簡略化するためのマクロです

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

**移動用モーター制御用関数 [#t3845988]
OnFwdとOnRevとOffをまとめた関数です。
スピードによって動作が変わるようになっています。

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

 /* 赤外線センサーの値を5段階(0〜4)で返す */
**明るさ判定関数 [#eb86e092]
ライントレースに使います。

授業では触れていませんが、[["switch"という構文>http://bricxcc.sourceforge.net/nbc/nxcdoc/nxcapi/switch.html]]を使うことを想定しています。
光センサーのしきい値はここに記入します。

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

 /* 止まる */
 void Stay(){
     Off(OUT_AC);
 }
 
 /* 前へ進む */
 void GoForward(){
     On(SPEED_HIGH, SPEED_HIGH);
 }
 
 /* 左へ進む */
 void GoLeft(){
     On(SPEED_ZERO, SPEED_MEDIUM);
 }
 
 /* 右へ進む */
 void GoRight(){
     On(SPEED_MEDIUM, SPEED_ZERO);
 }
 
 /* 左回転 */
 void TurnLeft(){
     On(-SPEED_LOW, SPEED_LOW);
 }
 
 /* 右回転 */
 void TurnRight(){
     On(SPEED_LOW, -SPEED_LOW);
 }
 
 /* 後ろへ戻る */
 void Back(){
     On(-SPEED_HIGH, -SPEED_HIGH);
 }
 
 /* 左後へ戻る */
 void BackLeft(){
     On(SPEED_ZERO, -SPEED_MEDIUM);
 }
 
 /* 右後へ戻る */
 void BackRight(){
     On(-SPEED_MEDIUM, SPEED_ZERO);
 }
**メイン関数のコード [#sa2e140e]

 /* ボールを見つけるとtrueを返す */
 bool checkBall(){
     if(SensorUS(S1) <= BALL_DISTANCE){
         return true;
     }
     return false;
 }
 
 /* ボールを拾う */
 void catchBall(){
     OnFwd(OUT_B, BALLBAR_SPEED);
     Wait(BALLBAR_DOWN);
     Float(OUT_B);
     Wait(BALLBAR_WAIT);
 }
 
 /* ボールを離す */
 void releaseBall(){
     OnRev(OUT_B, BALLBAR_SPEED);
     Wait(BALLBAR_UP);
     Float(OUT_B);
     Wait(BALLBAR_WAIT);
 }
**メイン関数 [#j9bbb92a]
[[解説はこちら>#k0f87237]]

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

 task main(){
     /* 回転数を記録するための変数 */
     int turnCount = 0;
 
     /* 曲がりながら進んだ回数を記録するための変数 */
     int goCount = 0;
 
     /* 直角の回数 */
     int nCorner = 0;
 
     /* ボールを認識した回数 */
     int ballCount = 0;
 
     /* センサー類を初期化 */
     SetSensorLowspeed(S1);
     SetSensorLight(S3);
 
     while(nCorner < 5){
     //通過ポイントを数えるための変数
     int corner = 0;
     
     // 車体の向き
     float rotation = 0;
     
     // 光センサー初期化
     SetSensorLight(LIGHT_SENSOR);
     
     // スタートから出る
     On(SPEED, SPEED);
     until(Light() == 0);
     until(Light() == 4);
     
     // トレース開始
     while(corner < 8){
         Wait(1);
 
         /* ボールを拾う処理 */
         /* 直角1回目と2回目の間で、まだボールを拾っていない場合 */
         if(nCorner == 1 && ballCount >= 0){
             /* ボールを認識したら */
             if(checkBall()){
                 PlayTone(880, 100);
                 /* 1秒間ボールを認識すれば */
                 if(ballCount++ > 10){
                     /* ボールを拾う */
                     catchBall();
                     ballCount = -1;
                 }
                 /* ボールを認識している間はトレースを一時停止 */
                 Stay();
                 Wait(99);
                 continue;
             }else{
                 ballCount = 0;
             }
         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;
         }
 
         /* トレース */
         switch(Light()){
             /* 明るさレベル0のとき */
             case 0:
                 TurnLeft();
                 if(turnCount < 0 || goCount < 0){
                     turnCount = 0;
                     goCount = 0;
         
         // 回転センサーの値を取得する
         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++;
                 }
                 turnCount++;
                 break;
             /* 明るさレベル1のとき */
             case 1:
                 GoLeft();
                 if(turnCount < 0 || goCount < 0){
                     turnCount = 0;
                     goCount = 0;
             case 1:  // 1回目の交差点
                 if(rotation < 10){
                     PlayTone(TONE_D4, 100);
                     On(SPEED, SPEED);
                     until(Light() == 0);
                     until(Light() >= 2);
                     corner++;
                 }
                 goCount++;
                 break;
             /* 明るさレベル2のとき */
             case 2:
                 GoForward();
             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;
             /* 明るさレベル3のとき */
             case 3:
                 GoRight();
                 if(turnCount > 0 || goCount > 0){
                     turnCount = 0;
                     goCount = 0;
             case 3:  // 大きいカーブ終了
                 if(rotation < -180){
                     PlayTone(TONE_F4, 100);
                     corner++;
                 }
                 goCount++;
                 break;
             /* 明るさレベル4のとき */
             case 4:
                 TurnRight();
                 if(turnCount > 0 || goCount > 0){
                     turnCount = 0;
                     goCount = 0;
             case 4:  // 2回目の交差点
                 if(rotation > -180){
                     PlayTone(TONE_G4, 100);
                     On(SPEED, 0);
                     until(Light() == 4);
                     On(SPEED, SPEED);
                     until(Light() == 3);
                     corner++;
                 }
                 turnCount++;
                 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);
 }
**メイン関数の解説 [#k0f87237]

***変数とセンサーの初期化 [#k6bd95c1]

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

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

 //通過ポイントを数えるための変数
 int corner = 0;
 
         /* 直角判定 */
         /* もし回転数が多く直進回数が少なければ */
         if(turnCount > TURN_COUNT_MAX && goCount < GO_COUNT_MAX){
             PlayTone(440, 100);
             switch(nCorner++){
                 /* 1,2,4回目の直角のとき */
                 case 0:
                 case 1:
                 case 3:
                     /* 交差点を渡る */
                     TurnRight();
                     Wait(turnCount * 3);
                     Off(OUT_AC);
                     Wait(100);
                     GoForward();
                     Wait(100);
                     break;
                 /* 5回目の直角のとき */
                 case 4:
                     /* 向きを訂正して */
                     /* ボールを離す */
                     break;
             }
             turnCount = 0;
             goCount = 0;
         }
 // 車体の向き
 float rotation = 0;
 
 // 光センサー初期化
 SetSensorLight(LIGHT_SENSOR);

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

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

***ライントレースのループ処理 [#j5421e60]

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

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

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

 while(corner < 8){
     Wait(1);
 
     /*
      * ライントレース
      */
 
     /*
      * 車体の向きを求める
      */
 
     /*
      * 通過ポイントごとの処理(交差点を渡るなど)
      */
 
 }
 Wait(2000);

***ライントレース [#f99fefa4]

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

[[Light関数についてはこちら>#eb86e092]]をご覧ください。

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

***車体の向きを求める [#s4d5e350]

#ref(2014a/Member/bgpat/Mission1/rotation.png,50%);
この画像のように車体が進んだとき、
回転した角度θは方程式を解くと
 θ = {(外側のタイヤが進んだ距離) - (内側のタイヤが進んだ距離)} / (車体の幅)
となります。
これを足していくと車体が向いている方向を求めることができます。

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

 // 回転センサーの値を取得する
 int _tl = LeftRotation();
 int _tr = RightRotation();
 
 // 差から角度を求める
 rotation += 360 * (_tr - tr - _tl + tl) * MOTER_ROTATE / (2 * BODY_WIDTH * PI);
 tl = _tl;
 tr = _tr;

***通過ポイントごとの処理 [#nc87e345]

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

詳細は後述します。

#ref(2014a/Member/bgpat/Mission1/point.png,50%);

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

----

+1回目のヘアピンカーブ
~
~車体の向きが90°を超えると通過ポイント1を超えたことにします。
 if(rotation > 90){
     PlayTone(TONE_C4, 100);
     corner++;
 }
 break;
~
+1回目の交差点
~
~車体の向きが10°未満になると通過ポイント2を超えたことにして
~交差点を超えます。
~
~スタートと同じように線を超えるまで直進して交差点を超えます。
~
 if(rotation < 10){
     PlayTone(TONE_D4, 100);
     On(SPEED, SPEED);
     until(Light() == 0);
     until(Light() >= 2);
         corner++;
     }
 }
**解説 [#k0f87237]
***トレース [#f99fefa4]
明るさを0から4の5段階に分けて、0と4のときはその場で回転、1と3のときは曲がりながら進む、2のときは直進するようします。
 break;
~
+ピンポン玉を取る位置
~
~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;
~
+大きいカーブ終了
~
~車体の向きがスタートしたときと反対になると大きいカーブが終了したとみなします。
~
 if(rotation < -180){
     PlayTone(TONE_F4, 100);
     corner++;
 }
 break;
~
+2回目の交差点
~
~大きいカーブが終了して、車体が左に向くと2回目の交差点を越えます。
~
~進み方や光センサーの条件を変えていますが、1回目の交差点を超えるときとほとんど同じです。
~
 if(rotation > -180){
     PlayTone(TONE_G4, 100);
     On(SPEED, 0);
     until(Light() == 4);
     On(SPEED, SPEED);
     until(Light() == 3);
     corner++;
 }
 break;
~
+3回目の交差点
~
~車体の向きが10°を超えると3回目の交差点です。
~本来は0°ですが、実際動かすと10°くらいが丁度よくなります。
~
~この手前に直角カーブがありますが、ライントレースの処理だけで通過することができます。
~
~方向を修正するように右に進み、交差点を越えていきます。
~線から出るとライントレースに戻ります。
~
 if(rotation > 10){
     PlayTone(TONE_A4, 100);
     On(SPEED, 0);
     until(Light() >= 3);
     corner++;
 }
 break;
~
+2回目のヘアピンカーブ
~
~1回目のヘアピンカーブと同じように車体の向きが90°を超えると7個目の通過ポイントです。
~
 if(rotation > 90){
     PlayTone(TONE_B4, 100);
     corner++;
 }
 break;
~
+ゴール
~
~車体の向きが15°より小さくなるとゴールです。
~
~車体はゴールの方を向いているので、アームを上げてシュートします。
~
~変数cornerが8になるのでループから抜けて、プログラムが終了します。
~
 if(rotation < -15){
     PlayTone(TONE_C5, 100);
     On(0, 0);
     RotateMotor(OUT_B, 100, -90);
     corner++;
 }
 break;
~

モーターの回転センサーを使って車体の向きを計算します。(雑に計算していますが大体あっているようなので修正しませんでした。)
いくつかのポイントを設定して、コースのどのあたりにいるかを求めます。
*工夫した点 [#s41c4e3f]
**交差点の判定 [#ge6dd8b5]
課題のコースは決まっているので、
変曲点や交差点に通過ポイントを設定することで
コース上のどのあたりを走行しているかを知ることができます。

交差点を超えるために直角の回数を数えます。
スタート→ゴールの場合は1,2,4回目の直角は交差点、3回目は直角コーナー、5回目はゴールとなります。
交差点と判断すると向きを戻して直進して交差点を渡ります。
直角コーナーはそのままで曲がってくれるのでプログラムは不要です。
ゴールの場合は向きを戻し、ボールをシュートしてループを抜けます。
***ボール [#ec9fadc9]
超音波センサーがボールを見つけるとアームを下ろします。
ただし、超音波センサーの誤動作も考えられるので、ボールを見つけたときにはトレースを中断して、1秒間連続で認識し続けたらボールがあると認識するようにしました。
*感想 [#n5075af2]
なかなか思っている通りに動かず苦労しました。特に値の調整が大変でした。
 例)1回目の交差点を超えるとき
 1回目交差点を普通にトレースすると左に曲がるので、0°より大きくなると交差点ということになります。
 しかし、0°より大きくなる点は1回目の交差点までに存在しているので「スタートしてから車体の向きが90°になる点を通過すると」という条件を付け加えます。
 このように通過ポイントを設定することで交差点を判定することができます。

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

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

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

**坂を使ったシュート機構 [#u9896f26]
~NXTはモーターを接続する場所が3つしかなく、トレースに2個のモーターを使うのでピンポン玉の確保やシュートは1個のモーターだけで行う必要があります。
~しかし、シュート機構が思いつかなかったので、車体の前に坂を付けてアームをあげるとピンポン玉が前に転がっていくようにしました。
~ピンポン玉を確保するときはアームをおろしてピンポン玉を坂に押し上げます。坂は画像のように3箇所用意し、トレースが多少ズレてもピンポン玉を確保しシュートできるようにしました。坂を増やすことでシュートできる確率が格段に増えました。
~&ref(2014a/Member/bgpat/Mission1/saka.png,50%);

*感想 [#e41a8e39]
ピンポン玉をゴールにシュートするところまでできたのでよかったです。


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