目次

課題について

詳細はこちら2019a/Mission3

ロボットについて

玉を運ぶ

アームはL(L')の玉を直接つかむものとGHIJの玉をプラ容器ごとつかむものの2つとした.

IMG_4076.jpeg
IMG_4072.jpg

プラ容器内の玉はロボット上部の斜面を伝い反対側に落ちるようにした.

IMG_4084.jpeg
IMG_4082.jpg

線に沿っての移動

センサを2つ前部に設置し,その2つのセンサ同士の距離もできるだけ狭めることで交差点での停止動作がより正確にできるようにした.また線の左右どちらを辿ろうともセンサを2つ利用することにでロボットがプログラムにかかわらず線の中央に近いところを走ることができる.

IMG_4075.jpg

プログラムについて

基本動作の定義

まず数値・ロボットが移動するための動きを以下のように定義する

   #define step1 38
   #define step2 45
   #define step3 53
   #define step4 60
   #define step5 68
   #define left0 OnFwd(OUT_A,60);OnRev(OUT_B,60);
   #define left1 OnFwd(OUT_A,60);Off(OUT_B);
   #define right0 OnFwd(OUT_B,60);OnRev(OUT_A,90);
   #define right1 OnFwd(OUT_B,60);Off(OUT_A);
   #define forward OnFwd(OUT_AB,60);
   #define back OnRev(OUT_AB,60);

#defineにより定義されたものは以下の通りである.

プログラム名内容
step1~5光センサの値域を5つに分けたもの
left0左タイヤを動かし左折する
left1両タイヤを動かし左折する
right0右タイヤを固定し右折する
right1両タイヤを動かし右折する
forward前進する
back後進する
   sub follow_line_left()
   {
      SetSensorLight(S1);
      SetSensorLight(S2);
 
      long t0=0,t1=0,st=1;
      while(t1-t0<st){
         if((SENSOR_1<step1)&&(SENSOR_2<step1)){    //2つのセンサが黒を感知するとき
              t0=CurrentTick();
              forward;                 //直進する
              until((SENSOR_1<step3)&&(SENSOR_2<step3));//2つのセンサが灰色を感知するまで
              t1=CurrentTick();
          }else if(SENSOR_1<step1){
              left0;
          }else if(SENSOR_1<step2){
              left1;
          }else if(SENSOR_1<step3){
              forward;     
          }else if(SENSOR_1<step4){
              right1;
          }else if(SENSOR_1<step5){
              right0;
          }
       }
       Off(OUT_AB);
       Wait(1000);
   }
   sub follow_line_right()
    {
      SetSensorLight(S1);
      SetSensorLight(S2);
 
      long t0=0,t1=0,st=1;
      while(t1-t0<st){
          if((SENSOR_1<step1)&&(SENSOR_2<step1)){
              t0=CurrentTick();
              forward;
              until((SENSOR_1<step3)&&(SENSOR_2<step3));
              t1=CurrentTick();
          }else if(SENSOR_2<step1){
              right0;
          }else if(SENSOR_2<step2){
              right1;
          }else if(SENSOR_2<step3){
              forward;     
          }else if(SENSOR_2<step4){
              left1;
          }else if(SENSOR_2<step5){
              left0;
           }
       }
       Off(OUT_AB);
       Wait(1000);
   }
   sub follow_line_back()
   {
      SetSensorLight(S1);
      SetSensorLight(S2);
 
      long t0=0,t1=0,st=1;
      while(t1-t0<st){
          if((SENSOR_1<step1)&&(SENSOR_2<step1)){
              t0=CurrentTick();
              back;
              until((SENSOR_1<step3)&&(SENSOR_2<step3));    
              t1=CurrentTick();
          }else if(SENSOR_2<step1){
              right0;
          }else if(SENSOR_2<step2){
              right1;
          }else if(SENSOR_2<step3){
              back;     
          }else if(SENSOR_2<step4){
              left1;
          }else if(SENSOR_2<step5){
              left0;
          }
       }
       Off(OUT_AB);
       Wait(1000);
   }
   sub start_right()
   {
       right1;
       Wait(100);
   }
   sub start_left()
   {
       left1;
       Wait(100);
   }
   sub across()
   {   
      SetSensorLight(S1);
      SetSensorLight(S2);
 
      long t0=0,t1=0,st=1;
      while(t1-t0<st){
          if((SENSOR_1<step1)&&(SENSOR_2<step1)){
              t0=CurrentTick();
              forward;
              until((SENSOR_1<step3)&&(SENSOR_2<step3));
              t1=CurrentTick();
          }else if(SENSOR_1<step5){
              forward;
           }
       }
       Off(OUT_AB);    
       Wait(1000);
   }

subによって定義されたプログラムは以下の通り.

プログラム名内容
follow_line_left()線の左側を線に沿って前進し交差点で停止する
follow_line_right()線の右側を線に沿って前進し交差点で停止する
sub follow_line_back()線の右側を線に沿って後進し交差点で停止する
start_right()右回転する
start_left()左回転する
across()線上で直進し線から外れると停止する

またロボットがボールを掴み保持したまま移動するためのアーム動作と必要な数値を以下のように定義する.

   sub ball_catch1()
   {
       OnFwd(OUT_A,40);
       Wait(200);
       OnRev(OUT_B,50);
       Wait(500);
       OnRev(OUT_A,60);
       Wait(700);
       Off(OUT_AB);
   }
   sub ball_throw1()
   {
       OnFwd(OUT_A,40);
       Wait(100);
       OnFwd(OUT_B,50);
       Wait(500);
       OnRev(OUT_A,60);
       Wait(700);
       Off(OUT_AB);
   }
   sub ball_catch2()
   {  
       OnRev(OUT_C,50);
       Wait(150);
       Off(OUT_C);
   }

subで定義したプログラムは以下の通り.

ball_catch1()玉を掴み持ち上げる
ball_throw1()玉を下げ置く
ball_catch2()プラ容器を掴む

ロボットがボールを掴み移動するにはロボットが停止したタイミングでアームを動かさなければならない. そのためタイヤを動かすNXTをマスター,アームを動かすNXTをスレーブとして互いにプログラムを終了したことを通信でやり取りできるようにした.

   sub send_msg1()    //メッセージの送受信(親機)
   {
       int msg;
       while(msg!=SIGNALOFF1){             //SIGNALOFFと受け取るまで
           SendRemoteNumber(CONN,MAILBOX1,SIGNALON1);  //CONNにSIGNALON1を送る
           ReceiveRemoteNumber(MAILBOX1,true,msg);   //メッセージを受け取る
            }
   } 
  sub receive_msg1()  //メッセージの送受信と玉を掴むプログラム(子機)
   {
       int msg;
        while(msg!=SIGNALON1){                          //SIGNALONと受け取るまで
           ReceiveRemoteNumber(MAILBOX1,true,msg);      //メッセージを受け取る
            } 
       ball_catch1();                 
       PlaySound(SOUND_UP);
       SendResponseNumber(MAILBOX1,SIGNALOFF1);//親機にSIGNALOFF1を送る
   }

receive_msg1()を実行させたのち,親機でfollow_lineの次にsend_msg1()を実行すると親機は線を辿り終えたところで子機にSIGNALON1を送信し,メッセージを受けた子機がball_catch1()を実行し玉を掴む.また掴み終えたところで子機から親機にSIGNALOFF1を送りそのメッセージを受けた親機はsend_msg1()を終了し次のプログラムを実行するようになる.

またPlaySound(SOUND_UP)は通信が正常にできたときアームを動かし終えた合図としてプログラムの確認用に入れてある.

コースを走るプログラム

実際に発表実行できたプログラムは以下の通り.

マスター側

   #define step1 38
   #define step2 45
   #define step3 53
   #define step4 60
   #define step5 68
   #define left0 OnFwd(OUT_A,60);OnRev(OUT_B,60);
   #define left1 OnFwd(OUT_A,60);Off(OUT_B);
   #define right0 OnFwd(OUT_B,60);OnRev(OUT_A,90);
   #define right1 OnFwd(OUT_B,60);Off(OUT_A);
   #define forward OnFwd(OUT_AB,60);
   #define back OnRev(OUT_AB,60);
   #define CONN 1
   #define SIGNALON1 10 
   #define SIGNALOFF1 11
   #define SIGNALON2 20 
   #define SIGNALOFF2 21
   #define HALF 1900
   sub follow_line_left()
   {
      SetSensorLight(S1);
      SetSensorLight(S2);
 
      long t0=0,t1=0,st=1;
      while(t1-t0<st){
         if((SENSOR_1<step1)&&(SENSOR_2<step1)){    
             t0=CurrentTick();
              forward;                 
              until((SENSOR_1<step3)&&(SENSOR_2<step3));
              t1=CurrentTick();
          }else if(SENSOR_1<step1){
              left0;
          }else if(SENSOR_1<step2){
              left1;
          }else if(SENSOR_1<step3){
              forward;     
          }else if(SENSOR_1<step4){
              right1;
          }else if(SENSOR_1<step5){
              right0;
          }
       }
       Off(OUT_AB);
       Wait(1000);
   }
   sub follow_line_right()
    {
      SetSensorLight(S1);
      SetSensorLight(S2);
 
      long t0=0,t1=0,st=1;
      while(t1-t0<st){
          if((SENSOR_1<step1)&&(SENSOR_2<step1)){
              t0=CurrentTick();
              forward;
              until((SENSOR_1<step3)&&(SENSOR_2<step3));
              t1=CurrentTick();
          }else if(SENSOR_2<step1){
              right0;
          }else if(SENSOR_2<step2){
              right1;
          }else if(SENSOR_2<step3){
              forward;     
          }else if(SENSOR_2<step4){
              left1;
          }else if(SENSOR_2<step5){
              left0;
           }
       }
       Off(OUT_AB);
       Wait(1000);
   }
   sub follow_line_back()
   {
      SetSensorLight(S1);
      SetSensorLight(S2);
 
      long t0=0,t1=0,st=1;
      while(t1-t0<st){
          if((SENSOR_1<step1)&&(SENSOR_2<step1)){
              t0=CurrentTick();
              back;
              until((SENSOR_1<step3)&&(SENSOR_2<step3));    
              t1=CurrentTick();
          }else if(SENSOR_2<step1){
              right0;
          }else if(SENSOR_2<step2){
              right1;
          }else if(SENSOR_2<step3){
              back;     
          }else if(SENSOR_2<step4){
              left1;
          }else if(SENSOR_2<step5){
              left0;
          }
       }
       Off(OUT_AB);
       Wait(1000);
   }
   sub start_right()
   {
       right1;
       Wait(100);
   }
   sub start_left()
   {
       left1;
       Wait(100);
   }
   sub across()
   {   
      SetSensorLight(S1);
      SetSensorLight(S2);
 
      long t0=0,t1=0,st=1;
      while(t1-t0<st){
          if((SENSOR_1<step1)&&(SENSOR_2<step1)){
              t0=CurrentTick();
              forward;
              until((SENSOR_1<step3)&&(SENSOR_2<step3));
              t1=CurrentTick();
          }else if(SENSOR_1<step5){
              forward;
           }
       }
       Off(OUT_AB);    
       Wait(1000);
   }
   sub send_msg1()
   {
       int msg;
       while(msg!=SIGNALOFF1){
           SendRemoteNumber(CONN,MAILBOX1,SIGNALON1);
           ReceiveRemoteNumber(MAILBOX1,true,msg);
          }    
   } 
   sub send_msg2()
   int msg;
   while(msg!=SIGNALOFF2){
       SendRemoteNumber(CONN,MAILBOX2,SIGNALON2);
       ReceiveRemoteNumber(MAILBOX2,true,msg);
  } 
  }
   task main()
   {
       follow_line_left();
       right_s;
       Wait(200);
       forward;
       Wait(900);
       Off(OUT_AB);
       send_msg1();
       right_s;
       Wait(HALF);
       follow_line_right();
       send_msg2()
   }

スレーブ側

   #define CONN 1
   #define SIGNALON1 10 
   #define SIGNALOFF1 11
   #define SIGNALON2 20 
   #define SIGNALOFF2 21
   sub ball_catch1()
   {
       OnFwd(OUT_A,40);
       Wait(200);
       OnRev(OUT_B,50);
       Wait(500);
       OnRev(OUT_A,60);
       Wait(700);
       Off(OUT_AB);
   }
   sub ball_throw1()
   {
       OnFwd(OUT_A,40);
       Wait(100);
       OnFwd(OUT_B,50);
       Wait(500);
       OnRev(OUT_A,60);
       Wait(700);
       Off(OUT_AB);
   }
   sub ball_catch2()
   {  
       OnRev(OUT_C,50);
       Wait(150);
       Off(OUT_C);
   }
   sub receive_msg1()
   {
       int msg;
       while(msg!=SIGNALON1){
           ReceiveRemoteNumber(MAILBOX1,true,msg);  
            } 
       ball_catch1();
       PlaySound(SOUND_UP);
       SendResponseNumber(MAILBOX1,SIGNALOFF1);
   }
   sub receive_msg2()
   {
       int msg;
       while(msg!=SIGNALON2){
           ReceiveRemoteNumber(MAILBOX2,true,msg);
            } 
       ball_throw1();
       PlaySound(SOUND_UP);
       SendResponseNumber(MAILBOX2,SIGNALOFF2);
    }
   task main()
   {
       receive_msg1();
       receive_msg2();
    }

まとめ

準備・練習の時はうまくいったのに本番うまくいかなかったのは電池の残量やプログラムの確認不足といった準備不足であったと思う.だが線を辿る精度をより高めたり,ロボットが停止してから滑らかに玉を拾う動作に移行できたりと今まで学習した内容を使いこなせていたと思う.


添付ファイル: fileIMG_4084.jpeg 5件 [詳細] fileIMG_4082.jpg 8件 [詳細] fileIMG_4072.jpg 8件 [詳細] fileIMG_4075.jpg 9件 [詳細] fileIMG_4076.jpeg 14件 [詳細]

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