[[2017b/Member]]

&size(24){目次};
#contents
*初めに [#l6e5e22a]
#ref(2017b/Member/arso/Mission3/2017b-mission3.png,50%,フィールド)
図1:フィールド

フィールドは図1のように課題2と同じものを使う。図の&color(#e4007f){マジェンタの円};に、ピンポン玉を2つ入れたコップを逆さまにして置く。また、図の&color(yellow,black){黄色い円};に障害物の紙コップを通常の向きに置く。Xの茶色い枠にピンポン玉を入れ、Yの地点に紙コップを置くことで得点を得られる。紙コップは重ねておくと得点に加算される。障害物は動かしすぎると減点される。

詳しいルールは[[2017年度後期の課題3:http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMission3]]を見ていただきたい。

*ロボットについて [#k7c9df8b]
**ロボット全体 [#q4e2f724]
#ref(2017b/Member/arso/Mission3/DSCN1188.jpg,70%,ロボットの全体)
写真1:ロボットの全体

今回の課題に対しては同じロボットを2つ作った。これは、コップを重ねることがこのロボットのようなアームでは1台では上手くいかなかったこと(後述する)と、ピンポン玉の回収を効率よくできるのではないかと考えたからである。また、1台が失敗したときにある程度補完できると踏んだのも理由の1つである。しかし、機体が長くなりすぎたことが後々響いた
**本体(車体) [#eac27e9d]
#ref(2017b/Member/arso/Mission3/DSCN1190.jpg,70%,光センサー)
写真2:光センサー

課題2のときの同様、タイヤ間の中心に取り付けることで移動などを楽にした。
#ref(2017b/Member/arso/Mission3/DSCN1206.jpg,70%,支え)
写真3:支え

この部分は、アームが重くなったことでアームを持ち上げているときと下ろしているときで重心がずれ、機体が揺れるのを防ぐためのものである。
**アーム [#t4c50f54]
#ref(2017b/Member/arso/Mission3/DSCN1191.jpg,70%,アーム全体)
写真4:アーム全体

アーム全体はこのようになっており、いくつかの機能を持っている。以下にそれを示す。
***センサー部分 [#aef23ab7]
#ref(2017b/Member/arso/Mission3/DSCN1193.jpg,70%,超音波センサー)
超音波センサーは、アームの後方の動かない部分から伸ばして取り付けた(画像では少し見にくいと思う)。また、センサーの下に見える円形の物体は、これがセンサーにぶつかることでアームが垂れ下がらないようにするための部品である。これは、ルールにある、スタート前の接地面はその領域(A地点及びD地点)の枠から出ないようにすることへの対策と、コップをつかむときにその中心付近をつかめるようにするためのものである。しかし、センサーの誤検知につながった可能性もある(未検証)。
***スタンド [#s702b520]
&ref(2017b/Member/arso/Mission3/DSCN1201.jpg,70%,スタンド1);
&ref(2017b/Member/arso/Mission3/DSCN1200.jpg,78%,スタンド2);

#ref(2017b/Member/arso/Mission3/DSCN1201.jpg,70%,スタンド1)
      写真5:スタンド          写真6:スタート後のスタンド

#ref(2017b/Member/arso/Mission3/DSCN1200.jpg,70%,スタンド2)
これは、スタート前のルールのためだけのパーツである。スタート前は写真5のように立っており、センサーなどが設置しないように働き、始まると写真6のように倒れるようになっている。また、この部品が動きの邪魔になることはなかった。
***コップをつかむ機構 [#fee8441c]

#ref(2017b/Member/arso/Mission3/DSCN.jpg,70%,ロボットの全体)
#ref(2017b/Member/arso/Mission3/DSCN1192.jpg,70%,アーム上)
写真7:上から見たアーム

#ref(2017b/Member/arso/Mission3/DSCN.jpg,70%,ロボットの全体)
#ref(2017b/Member/arso/Mission3/DSCN1205.jpg,70%,アーム下)
写真8:下から見たアーム

#ref(2017b/Member/arso/Mission3/ギア.jpg,70%,回転の仕組み)
写真9:アームの開閉

#ref(2017b/Member/arso/Mission3/DSCN1198.jpg,70%,コップをつかむ)
写真10:コップをつかんだ状態

#ref(2017b/Member/arso/Mission3/コップ.jpg,70%,コップを持ち上げる)
写真11:コップを持ち上げた状態

**コップを重ねる方法 [#f9944814]
本番ではここまでたどり着けなかったが、重ねるプログラムだけの場合はできていたのでその方法をここで説明しておく。まず、子機はピンポン玉を置いてから所定の位置で待機する。このとき、子機は一旦コップを放して後退し、腕を閉じておく。これを怠ると親機の超音波センサーが子機の腕を誤検知してしまう。その間に親機もピンポン玉を置き、子機のほうに向かう。
#ref(2017b/Member/arso/Mission3/積み重ね1.jpg,70%,積み重ね1)


次に、図のように親機はコップを持ち上げたままコップの近くに来る。その後、超音波センサーでコップを認識したら子機と通信し、親機は一時停止する。コップは持ち上げたままである。
#ref(2017b/Member/arso/Mission3/積み重ね2.jpg,70%,積み重ね2)


通信をすると、図のように子機は前進してコップを軽くつかみ固定する。
#ref(2017b/Member/arso/Mission3/積み重ね4.jpg,70%,積み重ね4)


その後、親機も前進する。この時の距離の制御が難しかったので、コップにぶつかった後も少しだけ前進させたままにし、それが終わった後、後退して位置を調整するようにした。腕を少し下げる。
#ref(2017b/Member/arso/Mission3/積み重ね5.jpg,70%,積み重ね5)


図ではわかりにくいが、ここで子機は腕を少し開いて親機が重ねたコップが引っ掛からないようにし、同時に少しだけ前進し、コップを押す(図で子機の腕が短くなっているのは腕を開いているような感じを出そうと試みたためである)。
#ref(2017b/Member/arso/Mission3/積み重ね6.jpg,70%,積み重ね6)


親機も腕を下ろしていくとコップが重なる。3つ目を重ねるときも同じ方法である。


*プログラムについて [#gac8807e]
**右側機体 [#a54feeb4]
***定義 [#y202a7a6]
 #define SO 392 
 #define DO 523 
 #define MI 659 
 #define SO_H 784 
 #define SI 988 
 #define DO_H 1047 
 #define MI_H 1318 
 #define RE 1175 
 #define SO_SH 1568 

 #define FAST 50
 #define SLOW 30
 #define CONN 1
 #define SIGNALON1 11
 #define SIGNALON2 12

 int d;
 const float diameter=5.45;
 const float tread=11.3;
 const float pi=3.1415;

 #define move1 OnFwd(OUT_A,40);OnFwd(OUT_B,-35); 
 #define move2 OnFwd(OUT_A,30);OnFwd(OUT_B,-20); 
 #define move3 OnFwd(OUT_A,54);OnFwd(OUT_B,54); 
 #define move4 OnFwd(OUT_B,30);OnFwd(OUT_A,-20); 
 #define move5 OnFwd(OUT_B,40);OnFwd(OUT_A,-35); 

 #define average 45 

 #define reverse_move1 OnFwd(OUT_A,-10);OnFwd(OUT_B,-30);
 #define reverse_move2 OnFwd(OUT_A,-54);OnFwd(OUT_B,-50);
 #define reverse_move3 OnFwd(OUT_B,-10);OnFwd(OUT_A,-30);


***サブ関数 [#jfca507a]

 sub pose_SE()
 {
     PlayTone(MI_H,90);Wait(100);
     PlayTone(DO_H,90);Wait(100);
     PlayTone(MI_H,90);Wait(100);
     PlayTone(DO_H,90);Wait(100);
 }

 sub one_UP_SE() 
 {
     PlayTone(MI,110);Wait(120);
     PlayTone(SO_H,110);Wait(120);
     PlayTone(MI_H,110);Wait(120);
     PlayTone(DO_H,110);Wait(120);
     PlayTone(RE,110);Wait(120);
     PlayTone(SO_SH,110);Wait(120);
 }

 sub coin_SE() 
 {
     PlayTone(SI,90);Wait(100);
     PlayTone(MI_H,140);Wait(150);
 }

 sub Start_SE() 
 {
     PlayTone(MI,100);Wait(110);
     PlayTone(MI,100);Wait(250);
     PlayTone(MI,100);Wait(160);
     PlayTone(DO,90);Wait(100);
     PlayTone(MI,90);Wait(230);
     PlayTone(SO_H,90);Wait(420);
     PlayTone(SO,170);Wait(180);
 }

 sub keep_cop()
 {
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,-SLOW,50);
     OnFwd(OUT_C,-20);
 }

 sub fwdDist(float d)
 {
     long angle=d/(diameter*pi)*360.0;
     RotateMotorEx(OUT_AB,SLOW,angle,0,true,true);
     }

 sub turnAng(long ang)
 {
     long angle=tread/diameter*ang;
     RotateMotorEx(OUT_AB,SLOW,angle,100,true,true);
 }

     int searchDirection(long ang)
 {
     long tacho_min;
     int d_min=300;
 
     long angle=(tread/diameter)*ang;
     turnAng(ang/2);
     ResetTachoCount(OUT_AB);
     OnFwdSync(OUT_AB,SLOW,-100);
     while(MotorTachoCount(OUT_A)<=angle){
          if(SensorUS(S3)<d_min){
            d_min=SensorUS(S3);
            tacho_min=MotorTachoCount(OUT_A);
 }
 }
     OnFwdSyncEx(OUT_AB,SLOW,100,RESET_NONE);
 
     until(MotorTachoCount(OUT_A)<=tacho_min||SensorUS(S3)<=d_min);
     Wait(14);
     Off(OUT_AB);
     Wait(500);
     return d_min;
 }

 sub tag()
 {
     keep_cop();
     d=searchDirection(15);
 
     if(d>5.0){
     SendRemoteNumber(CONN,MAILBOX1,SIGNALON1);
     Wait(5000);
     fwdDist(d-8.0);
     ResetTachoCount(OUT_ABC);
     RotateMotorEx(OUT_AB,-SLOW,40,0,true,true);
     RotateMotor(OUT_C,SLOW,50);
 
     SendRemoteNumber(CONN,MAILBOX1,SIGNALON2);
     Wait(5000);
     RotateMotor(OUT_C,SLOW,100);
     ResetTachoCount(OUT_ABC);
     RotateMotor(OUT_C,SLOW,720);
     RotateMotorEx(OUT_AB,-SLOW,120,0,true,true);
 }
 }

 sub catch_cop(long ang1)
 {   
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,-SLOW,ang1);
 }

 sub release_cop(long ang2)
 {
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,SLOW,ang2);
 }

 sub release_ball()
 {   
     ResetTachoCount(OUT_ABC);
     OnFwd(OUT_AB,SLOW);
     Wait(700);
     Off(OUT_AB);
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,-SLOW,50);
     OnFwd(OUT_C,-20);
     Wait(1000);
     OnFwd(OUT_AB,-SLOW);
     Wait(700);
     Off(OUT_AB);
 }

 sub Stop_Motion() 
 {
     Wait(300); 
     pose_SE(); 
     Wait(1000); 
 }

 sub L_Assist_line(int Assist_time)
 {
     long ta=CurrentTick();
     while (CurrentTick()-ta<=Assist_time){
         if (SENSOR_2<average-11) {
             move1
         }else if (SENSOR_2<average-7){
             move2;
         } else if (SENSOR_2<average+7){  
             move3;
         } else if (SENSOR_2<average+11){
             move4;
         } else {
             move5;
          }
      }
     coin_SE();
 }

 sub R_Assist_line(int Assist_time) 
 {
     long tb=CurrentTick();
     while (CurrentTick()-tb<=Assist_time){
         if (SENSOR_2<average-11) {
             move5;
         } else if (SENSOR_2<average-7){
             move4;
         } else if (SENSOR_2<average+7){  
             move3;
         } else if (SENSOR_2<average+11){
             move2;
         } else {
             move1;
          }
     }
     coin_SE(); 
 }

 sub L_line(int Assist_time,int Stop_time) 
 {
     L_Assist_line(Assist_time);
     long t1=CurrentTick();
     while (CurrentTick()-t1<=Stop_time){
         if (SENSOR_2<average-11) {
             move1;
         } else if (SENSOR_2<average-7){
             move2;
             t1=CurrentTick();
         } else if (SENSOR_2<average+7){  
             move3;
             t1=CurrentTick();
         } else if (SENSOR_2<average+11){
             move4;
             t1=CurrentTick();
         } else {
             move5;
             t1=CurrentTick();
         }
      }
     Off(OUT_AB);
     coin_SE(); 
 }

 sub R_line(int Assist_time,int Stop_time)
 {
     R_Assist_line(Assist_time); 
  
     long t2=CurrentTick();
     while(CurrentTick()-t2<=Stop_time){
         if(SENSOR_2<average-11) {
             move5;
         }else if (SENSOR_2<average-8){
             move4;
             t2=CurrentTick();
         }else if (SENSOR_2<average+6){  
             move3;
             t2=CurrentTick();
        }else if (SENSOR_2<average+11){
             move2;
             t2=CurrentTick();
         } else {
             move1;
             t2=CurrentTick();
          }
     }
     Off(OUT_AB);
     coin_SE();
 }

 sub White_to_Black(int blight,int A_speed,int B_speed)
 {
      while (SENSOR_2>blight){
          OnFwd(OUT_A,A_speed);
          OnFwd(OUT_B,B_speed);
      }
     Off(OUT_AB);
 }

 sub Black_to_White(int blight,int A_speed,int B_speed)
 {
     while (SENSOR_2<blight){
         OnFwd(OUT_A,A_speed);
         OnFwd(OUT_B,B_speed);
      }
     Off(OUT_AB);
 }

 sub reverse_R_Assist_line(int Assist_time)
 {
     long tc=CurrentTick();
     while (CurrentTick()-tc<=Assist_time){
         if (SENSOR_2<average-7){
             reverse_move3;
         } else if (SENSOR_2<average+7){  
             reverse_move2;
         } else {
             reverse_move1;
          }
      }
 }

 sub reverse_R_line(int Assist_time,int Stop_time)
 {
     reverse_R_Assist_line(Assist_time);
     long t3=CurrentTick();
     while (CurrentTick()-t3<=Stop_time){
         if (SENSOR_2<average-7){
             reverse_move3;
         } else if (SENSOR_2<average+7){  
             reverse_move2;
             t3=CurrentTick();
         } else {
             reverse_move1;
          }
      }
     Off(OUT_AB);
 }

 sub reverse_Start_Set()
 {
     reverse_move2;
     Start_SE();
     Wait(2000);
     White_to_Black(48,20,35);
 }

***プログラム全体 [#o46e00ab]
 #define SO 392 
 #define DO 523 
 #define MI 659 
 #define SO_H 784 
 #define SI 988 
 #define DO_H 1047 
 #define MI_H 1318 
 #define RE 1175 
 #define SO_SH 1568 
 
 sub pose_SE()
 {
     PlayTone(MI_H,90);Wait(100);
     PlayTone(DO_H,90);Wait(100);
     PlayTone(MI_H,90);Wait(100);
     PlayTone(DO_H,90);Wait(100);
 }
 sub one_UP_SE() 
 {
     PlayTone(MI,110);Wait(120);
     PlayTone(SO_H,110);Wait(120);
     PlayTone(MI_H,110);Wait(120);
     PlayTone(DO_H,110);Wait(120);
     PlayTone(RE,110);Wait(120);
     PlayTone(SO_SH,110);Wait(120);
 }
 sub coin_SE() 
 {
     PlayTone(SI,90);Wait(100);
     PlayTone(MI_H,140);Wait(150);
 }
 
 sub Start_SE() 
 {
     PlayTone(MI,100);Wait(110);
     PlayTone(MI,100);Wait(250);
     PlayTone(MI,100);Wait(160);
     PlayTone(DO,90);Wait(100);
     PlayTone(MI,90);Wait(230);
     PlayTone(SO_H,90);Wait(420);
     PlayTone(SO,170);Wait(180);
 }
 
 #define FAST 50
 #define SLOW 30
 #define CONN 1
 #define SIGNALON1 11
 #define SIGNALON2 12
 
 int d;
 const float diameter=5.45;
 const float tread=11.3;
 const float pi=3.1415;
 
 sub keep_cop()
 {
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,-SLOW,50);
     OnFwd(OUT_C,-20);
 }
 
 sub fwdDist(float d)
 {
     long angle=d/(diameter*pi)*360.0;
     RotateMotorEx(OUT_AB,SLOW,angle,0,true,true);
 }
 
 sub turnAng(long ang)
 {
     long angle=tread/diameter*ang;
     RotateMotorEx(OUT_AB,SLOW,angle,100,true,true);
 }
 
 int searchDirection(long ang)
 
 {
     long tacho_min;
     int d_min=300;
 
     long angle=(tread/diameter)*ang;
     turnAng(ang/2);
     ResetTachoCount(OUT_AB);
     OnFwdSync(OUT_AB,SLOW,-100);
     while(MotorTachoCount(OUT_A)<=angle){
          if(SensorUS(S3)<d_min){
            d_min=SensorUS(S3);
            tacho_min=MotorTachoCount(OUT_A);
 }
 }
     OnFwdSyncEx(OUT_AB,SLOW,100,RESET_NONE);
 
     until(MotorTachoCount(OUT_A)<=tacho_min||SensorUS(S3)<=d_min);
     Wait(14);
     Off(OUT_AB);
     Wait(500);
     return d_min;
 }
 
 sub tag()
 {
     keep_cop();
     d=searchDirection(15);
 
     if(d>5.0){
       SendRemoteNumber(CONN,MAILBOX1,SIGNALON1);
       Wait(5000);
       fwdDist(d-8.0);
       ResetTachoCount(OUT_ABC);
       RotateMotorEx(OUT_AB,-SLOW,40,0,true,true);
       RotateMotor(OUT_C,SLOW,50);
 
       SendRemoteNumber(CONN,MAILBOX1,SIGNALON2);
       Wait(5000);
       RotateMotor(OUT_C,SLOW,100);
       ResetTachoCount(OUT_ABC);
       RotateMotor(OUT_C,SLOW,720);
       RotateMotorEx(OUT_AB,-SLOW,120,0,true,true);
 }
 }
 
 sub catch_cop(long ang1)
 {   
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,-SLOW,ang1);
 }
 
 sub release_cop(long ang2)
 {
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,SLOW,ang2);
 }
 
 sub release_ball()
 {   
     ResetTachoCount(OUT_ABC);
     OnFwd(OUT_AB,SLOW);
     Wait(700);
     Off(OUT_AB);
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,-SLOW,50);
     OnFwd(OUT_C,-20);
     Wait(1000);
     OnFwd(OUT_AB,-SLOW);
     Wait(700);
     Off(OUT_AB);
 }
 
 #define move1 OnFwd(OUT_A,40);OnFwd(OUT_B,-35); 
 #define move2 OnFwd(OUT_A,30);OnFwd(OUT_B,-20); 
 #define move3 OnFwd(OUT_A,54);OnFwd(OUT_B,54); 
 #define move4 OnFwd(OUT_B,30);OnFwd(OUT_A,-20); 
 #define move5 OnFwd(OUT_B,40);OnFwd(OUT_A,-35); 
 
 #define average 45 
 
 sub Stop_Motion() 
 {
     Wait(300); 
     pose_SE(); 
     Wait(1000); 
 }
 
 sub L_Assist_line(int Assist_time)
 {
     long ta=CurrentTick();
     while (CurrentTick()-ta<=Assist_time){
         if (SENSOR_2<average-11) {
             move1
         }else if (SENSOR_2<average-7){
             move2;
         }else if (SENSOR_2<average+7){  
             move3;
         }else if (SENSOR_2<average+11){
             move4;
         }else {
             move5;
         }
     }
     coin_SE();
 }
 
 sub R_Assist_line(int Assist_time) 
 {
     long tb=CurrentTick();
     while (CurrentTick()-tb<=Assist_time){
         if (SENSOR_2<average-11) {
             move5;
         } else if (SENSOR_2<average-7){
             move4;
         } else if (SENSOR_2<average+7){  
             move3;
         } else if (SENSOR_2<average+11){
             move2;
         } else {
             move1;
          }
      }
     coin_SE(); 
 }
 
 sub L_line(int Assist_time,int Stop_time) 
 {
     L_Assist_line(Assist_time);
     long t1=CurrentTick();
     while (CurrentTick()-t1<=Stop_time){
         if (SENSOR_2<average-11) {
             move1;
         } else if (SENSOR_2<average-7){
             move2;
             t1=CurrentTick();
         } else if (SENSOR_2<average+7){  
             move3;
             t1=CurrentTick();
         } else if (SENSOR_2<average+11){
             move4;
             t1=CurrentTick();
         } else {
             move5;
             t1=CurrentTick();
          }
      }
     Off(OUT_AB);
     coin_SE(); 
 }
 
 sub R_line(int Assist_time,int Stop_time)
 {
     R_Assist_line(Assist_time); 
 
     long t2=CurrentTick();
     while (CurrentTick()-t2<=Stop_time){
         if (SENSOR_2<average-11) {
             move5;
         } else if (SENSOR_2<average-8){
             move4;
             t2=CurrentTick();
         } else if (SENSOR_2<average+6){  
             move3;
             t2=CurrentTick();
         } else if (SENSOR_2<average+11){
             move2;
             t2=CurrentTick();
         } else {
             move1;
             t2=CurrentTick();
          }
      }
     Off(OUT_AB);
     coin_SE();
 }
 
 sub White_to_Black(int blight,int A_speed,int B_speed)
 {
     while (SENSOR_2>blight){
          OnFwd(OUT_A,A_speed);
          OnFwd(OUT_B,B_speed);
      }
     Off(OUT_AB);
 }
 
 sub Black_to_White(int blight,int A_speed,int B_speed)
 {
     while (SENSOR_2<blight){
          OnFwd(OUT_A,A_speed);
          OnFwd(OUT_B,B_speed);
      }
     Off(OUT_AB);
 }
 
 #define reverse_move1 OnFwd(OUT_A,-10);OnFwd(OUT_B,-30);
 #define reverse_move2 OnFwd(OUT_A,-54);OnFwd(OUT_B,-50);
 #define reverse_move3 OnFwd(OUT_B,-10);OnFwd(OUT_A,-30);
 
 sub reverse_R_Assist_line(int Assist_time)
 {
     long tc=CurrentTick();
     while (CurrentTick()-tc<=Assist_time){
          if (SENSOR_2<average-7){
            reverse_move3;
         } else if (SENSOR_2<average+7){  
             reverse_move2;
         } else {
             reverse_move1;
          }
      }
 }
 
 sub reverse_R_line(int Assist_time,int Stop_time)
 {
     reverse_R_Assist_line(Assist_time);
     long t3=CurrentTick();
     while (CurrentTick()-t3<=Stop_time){
          if (SENSOR_2<average-7){
            reverse_move3;
         } else if (SENSOR_2<average+7){  
            reverse_move2;
            t3=CurrentTick();
         } else {
            reverse_move1;
         }
      }
     Off(OUT_AB);
 }
 
 sub reverse_Start_Set()
 {
     reverse_move2;
     Start_SE();
     Wait(2000);
     White_to_Black(48,20,35);
 }
 
 task main()
 {
     SetSensorLight(S2);
     SetSensorLowspeed(S3);
 
     reverse_Start_Set(); 
     L_line(1300,110);
 
     OnFwd(OUT_AB,-SLOW);
     Wait(500);
     Off(OUT_AB);
     White_to_Black(43,20,25);
     White_to_Black(33,20,25);
 
     ResetTachoCount(OUT_A);
     RotateMotor(OUT_A,-SLOW,150);
 
     d=searchDirection(15);
     if(d>5.0){
       catch_cop(600);
       OnFwd(OUT_AB,SLOW);
       Wait(1000);
       Off(OUT_AB);
       catch_cop(500);
       OnFwd(OUT_AB,-SLOW);
       Wait(400);
       Off(OUT_AB);
 }
 
     ResetTachoCount(OUT_B);
     RotateMotor(OUT_B,-SLOW,110);
     release_ball();
 
     ResetTachoCount(OUT_A);
     RotateMotor(OUT_A,-SLOW,130);
 
     tag();
 
     OnFwd(OUT_AB,-SLOW);
     Wait(1000);
     Off(OUT_AB);
 
     White_to_Black(48,35,25);
     L_line(300,130);
     Black_to_White(48,0,-30);
     White_to_Black(33,0,-30);
 
     d=searchDirection(15);
     if(d>5.0){
       catch_cop(600);
       OnFwd(OUT_AB,SLOW);
       Wait(1000);
       Off(OUT_AB);
       catch_cop(500);
       OnFwd(OUT_AB,-SLOW);
       Wait(400);
       Off(OUT_AB);
 
 }
 
     White_to_Black(33,-30,-30);
     L_line(150,130);
     Black_to_White(48,30,30);
     release_ball();
 
     ResetTachoCount(OUT_A);
     RotateMotor(OUT_A,-SLOW,130);
 
     tag();
 
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,-SLOW,1100);
 
     White_to_Black(33,-30,-30);
     Black_to_White(48,-30,-30);
     White_to_Black(33,-30,-30);
 
     OnFwd(OUT_AB,SLOW);
     Wait(300);
     Off(OUT_AB);
 
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,SLOW,1200);
 }
**左側機体 [#u3b39ac6]
***定義 [#uf63f794]
 #define SO 392
 #define DO 523
 #define MI 659
 #define SO_H 784
 #define SI 988
 #define DO_H 1047
 #define MI_H 1318
 #define RE 1175
 #define SO_SH 1568

 #define SIGNALON 11
 #define SIGNALON2 12
 #define SLOW 30

 const float diameter=5.45;
 const float tread=11.3;
 const float pi=3.1415;

 int d;

 #define move1 OnFwd(OUT_A,40);OnFwd(OUT_B,-35);
 #define move2 OnFwd(OUT_A,30);OnFwd(OUT_B,-20);
 #define move3 OnFwd(OUT_A,54);OnFwd(OUT_B,54);
 #define move4 OnFwd(OUT_B,30);OnFwd(OUT_A,-20);
 #define move5 OnFwd(OUT_B,40);OnFwd(OUT_A,-35);
 
 #define average 45
 
***サブ関数 [#f93baec9]
 
 sub fwdDist(float d)
 {
     long angle=d/(diameter*pi)*360.0;
     RotateMotorEx(OUT_AB,SLOW,angle,0,true,true);
 }

 sub turnAng(long ang)
 {
     long angle=tread/diameter*ang;
     RotateMotorEx(OUT_AB,SLOW,angle,100,true,true);
 }

 int searchDirection(long ang)
 {
     long tacho_min;
     int d_min=300;
 
     long angle=(tread/diameter)*ang;
     turnAng(ang/2);
     ResetTachoCount(OUT_AB);
     OnFwdSync(OUT_AB,SLOW,-100);
     while(MotorTachoCount(OUT_A)<=angle){
          if(SensorUS(S3)<d_min){
            d_min=SensorUS(S3);
            tacho_min=MotorTachoCount(OUT_A);
 }
 }
     OnFwdSyncEx(OUT_AB,SLOW,100,RESET_NONE);

     until(MotorTachoCount(OUT_A)<=tacho_min||SensorUS(S3)<=d_min);
          Wait(14);
          Off(OUT_AB);
          Wait(500);
          return d_min;
 }

 sub tag()
 {
     int msg,counter;
     ResetTachoCount(OUT_ABC);
     RotateMotor(OUT_C,SLOW,1080);
     RotateMotorEx(OUT_AB,-SLOW,360,0,true,true);
     RotateMotor(OUT_C,-SLOW,1080);
 
     while(counter==0){
          ReceiveRemoteNumber(MAILBOX1,true,msg);
          if(msg==SIGNALON) {
            ResetTachoCount(OUT_ABC);
            RotateMotor(OUT_C,SLOW,1080);
            RotateMotorEx(OUT_AB,SLOW,120,0,true,true);
            RotateMotor(OUT_C,-SLOW,1080);
            counter++;
       }
 }
     counter=0;
 
     while(counter==0){
          ReceiveRemoteNumber(MAILBOX1,true,msg);
          if(msg==SIGNALON2) {
            ResetTachoCount(OUT_ABC);
            RotateMotor(OUT_C,SLOW,360);
            RotateMotorEx(OUT_AB,SLOW,40,0,true,true);
            counter++;
 }
 }
     counter=0;
     ResetTachoCount(OUT_ABC);
     RotateMotor(OUT_C,SLOW,720);
     RotateMotorEx(OUT_AB,-SLOW,120,0,true,true);
 }

 sub pose_SE()
 {
     PlayTone(MI_H,90);Wait(100);
     PlayTone(DO_H,90);Wait(100);
     PlayTone(MI_H,90);Wait(100);
     PlayTone(DO_H,90);Wait(100);
 }

 sub one_UP_SE()
 {
     PlayTone(MI,110);Wait(120);
     PlayTone(SO_H,110);Wait(120);
     PlayTone(MI_H,110);Wait(120);
     PlayTone(DO_H,110);Wait(120);
     PlayTone(RE,110);Wait(120);
     PlayTone(SO_SH,110);Wait(120);

 }

 sub coin_SE()
 {
     PlayTone(SI,90);Wait(100);
     PlayTone(MI_H,140);Wait(150);
 }

 sub Start_SE()
 {
     PlayTone(MI,100);Wait(110);
     PlayTone(MI,100);Wait(250);
     PlayTone(MI,100);Wait(160);
     PlayTone(DO,90);Wait(100);
     PlayTone(MI,90);Wait(230);
     PlayTone(SO_H,90);Wait(420);
     PlayTone(SO,170);Wait(180);
 }

 sub Stop_Motion()
 {
     Wait(300);
     pose_SE();
     Wait(1000);
 }

 sub L_Assist_line(int Assist_time)
 {
     long ta=CurrentTick();
     while (CurrentTick()-ta<=Assist_time){
         if (SENSOR_2<average-11) {
           move1
        }else if (SENSOR_2<average-7){
           move2;
        } else if (SENSOR_2<average+7){  
            move3;
        } else if (SENSOR_2<average+11){
             move4;
         } else {
             move5;
          }
      }
     coin_SE();
 }

 sub R_Assist_line(int Assist_time)
 {
     long tb=CurrentTick();
     while (CurrentTick()-tb<=Assist_time){
          if (SENSOR_2<average-11) {
             move5;
         } else if (SENSOR_2<average-7){
             move4;
         } else if (SENSOR_2<average+7){  
             move3;
         } else if (SENSOR_2<average+11){
             move2;
         } else {
             move1;
          }
      }
     coin_SE();
 }

 sub L_line(int Assist_time,int Stop_time)
 {
     L_Assist_line(Assist_time);
     long t1=CurrentTick();
     while (CurrentTick()-t1<=Stop_time){
         if (SENSOR_2<average-11) {
             move1;
         } else if (SENSOR_2<average-7){
             move2;
             t1=CurrentTick();
         } else if (SENSOR_2<average+7){  
             move3;
             t1=CurrentTick();
         } else if (SENSOR_2<average+11){
             move4;
             t1=CurrentTick();
         } else {
             move5;
             t1=CurrentTick();
          }
      }
     Off(OUT_AB);
     coin_SE();
 }

 sub R_line(int Assist_time,int Stop_time)
 {
     R_Assist_line(Assist_time);
  
     long t2=CurrentTick();
     while (CurrentTick()-t2<=Stop_time){
         if (SENSOR_2<average-11) {
             move5;
         } else if (SENSOR_2<average-8){
             move4;
             t2=CurrentTick();
         } else if (SENSOR_2<average+6){  
             move3;
             t2=CurrentTick();
         } else if (SENSOR_2<average+11){
             move2;
             t2=CurrentTick();
         } else {
             move1;
             t2=CurrentTick();
          }
      }
     Off(OUT_AB);
     coin_SE();
 }

 sub White_to_Black(int blight,int A_speed,int B_speed)
 {
     while (SENSOR_2>blight){
          OnFwd(OUT_A,A_speed);
          OnFwd(OUT_B,B_speed);
      }
     Off(OUT_AB);
 }

 sub Black_to_White(int blight,int A_speed,int B_speed)
 {
     while (SENSOR_2<blight){
          OnFwd(OUT_A,A_speed);
          OnFwd(OUT_B,B_speed);
      }
     Off(OUT_AB);
 }

 sub Start_Set()
 {
     Start_SE();
     move3;
     Wait(600);
     White_to_Black(48,30,5);
 }

 sub catch_cop(long ang1)
 {   
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,-SLOW,ang1);
 }

 sub release_cop(long ang2)
 {
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,SLOW,ang2);
 }

 sub release_ball()
 {   
     ResetTachoCount(OUT_ABC);
     OnFwd(OUT_AB,SLOW);
     Wait(700);
     Off(OUT_AB);
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,-SLOW,50);
     OnFwd(OUT_C,-20);
     Wait(1000);
     OnFwd(OUT_AB,-SLOW);
     Wait(700);
     Off(OUT_AB);
 }

 sub keep_cop()
 {
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,-SLOW,50);
     OnFwd(OUT_C,-20);
 }

***プログラム全体 [#e743cbf5]
 #define SO 392
 #define DO 523
 #define MI 659
 #define SO_H 784
 #define SI 988
 #define DO_H 1047
 #define MI_H 1318
 #define RE 1175
 #define SO_SH 1568

 #define SIGNALON 11
 #define SIGNALON2 12
 #define SLOW 30

 const float diameter=5.45;
 const float tread=11.3;
 const float pi=3.1415;

 int d;

 sub fwdDist(float d)
 {
     long angle=d/(diameter*pi)*360.0;
     RotateMotorEx(OUT_AB,SLOW,angle,0,true,true);
 }

 sub turnAng(long ang)
 {
     long angle=tread/diameter*ang;
     RotateMotorEx(OUT_AB,SLOW,angle,100,true,true);
 }

 int searchDirection(long ang)
 {
     long tacho_min;
     int d_min=300;

     long angle=(tread/diameter)*ang;
     turnAng(ang/2);
     ResetTachoCount(OUT_AB);
     OnFwdSync(OUT_AB,SLOW,-100);
     while(MotorTachoCount(OUT_A)<=angle){
          if(SensorUS(S3)<d_min){
            d_min=SensorUS(S3);
            tacho_min=MotorTachoCount(OUT_A);
 }
 }
     OnFwdSyncEx(OUT_AB,SLOW,100,RESET_NONE);

     until(MotorTachoCount(OUT_A)<=tacho_min||SensorUS(S3)<=d_min);
          Wait(14);
          Off(OUT_AB);
          Wait(500);
          return d_min;
 }

 sub tag()
 {
     int msg,counter;
     ResetTachoCount(OUT_ABC);
     RotateMotor(OUT_C,SLOW,1080);
     RotateMotorEx(OUT_AB,-SLOW,360,0,true,true);
     RotateMotor(OUT_C,-SLOW,1080);
 
     while(counter==0){
          ReceiveRemoteNumber(MAILBOX1,true,msg);
          if(msg==SIGNALON) {
            ResetTachoCount(OUT_ABC);
            RotateMotor(OUT_C,SLOW,1080);
            RotateMotorEx(OUT_AB,SLOW,120,0,true,true);
            RotateMotor(OUT_C,-SLOW,1080);
            counter++;
       }
 }
     counter=0;
     while(counter==0){
          ReceiveRemoteNumber(MAILBOX1,true,msg);
          if(msg==SIGNALON2) {
            ResetTachoCount(OUT_ABC);
            RotateMotor(OUT_C,SLOW,360);
            RotateMotorEx(OUT_AB,SLOW,40,0,true,true);
            counter++;
 }
 }
     counter=0;
     ResetTachoCount(OUT_ABC);
     RotateMotor(OUT_C,SLOW,720);
     RotateMotorEx(OUT_AB,-SLOW,120,0,true,true);
 }

 sub pose_SE()
 {
     PlayTone(MI_H,90);Wait(100);
     PlayTone(DO_H,90);Wait(100);
     PlayTone(MI_H,90);Wait(100);
     PlayTone(DO_H,90);Wait(100);
 }

 sub one_UP_SE()
 {
     PlayTone(MI,110);Wait(120);
     PlayTone(SO_H,110);Wait(120);
     PlayTone(MI_H,110);Wait(120);
     PlayTone(DO_H,110);Wait(120);
     PlayTone(RE,110);Wait(120);
     PlayTone(SO_SH,110);Wait(120);

 }

 sub coin_SE()
 {
     PlayTone(SI,90);Wait(100);
     PlayTone(MI_H,140);Wait(150);
 }

 sub Start_SE()
 {
     PlayTone(MI,100);Wait(110);
     PlayTone(MI,100);Wait(250);
     PlayTone(MI,100);Wait(160);
     PlayTone(DO,90);Wait(100);
     PlayTone(MI,90);Wait(230);
     PlayTone(SO_H,90);Wait(420);
     PlayTone(SO,170);Wait(180);
 }

 #define move1 OnFwd(OUT_A,40);OnFwd(OUT_B,-35);
 #define move2 OnFwd(OUT_A,30);OnFwd(OUT_B,-20);
 #define move3 OnFwd(OUT_A,54);OnFwd(OUT_B,54);
 #define move4 OnFwd(OUT_B,30);OnFwd(OUT_A,-20);
 #define move5 OnFwd(OUT_B,40);OnFwd(OUT_A,-35);

 #define average 45

 sub Stop_Motion()
 {
     Wait(300);
     pose_SE();
     Wait(1000);
 }

 sub L_Assist_line(int Assist_time)
 {
     long ta=CurrentTick();
     while (CurrentTick()-ta<=Assist_time){
         if(SENSOR_2<average-11) {
           move1
        }else if (SENSOR_2<average-7){
           move2;
        }else if (SENSOR_2<average+7){  
            move3;
        }else if (SENSOR_2<average+11){
             move4;
        }else {
             move5;
          }
      }
     coin_SE();
 }

 sub R_Assist_line(int Assist_time)
 {
     long tb=CurrentTick();
     while (CurrentTick()-tb<=Assist_time){
          if(SENSOR_2<average-11) {
             move5;
         }else if(SENSOR_2<average-7){
             move4;
         }else if(SENSOR_2<average+7){  
             move3;
         }else if(SENSOR_2<average+11){
             move2;
         }else {
             move1;
         }
      }
     coin_SE();
 }

 sub L_line(int Assist_time,int Stop_time)
 {
     L_Assist_line(Assist_time);
     long t1=CurrentTick();
     while (CurrentTick()-t1<=Stop_time){
         if (SENSOR_2<average-11) {
             move1;
         } else if (SENSOR_2<average-7){
             move2;
             t1=CurrentTick();
         } else if (SENSOR_2<average+7){  
             move3;
             t1=CurrentTick();
         } else if (SENSOR_2<average+11){
             move4;
             t1=CurrentTick();
         } else {
             move5;
             t1=CurrentTick();
          }
      }
     Off(OUT_AB);
     coin_SE();
 }

 sub R_line(int Assist_time,int Stop_time)
 {
     R_Assist_line(Assist_time);
  
     long t2=CurrentTick();
     while (CurrentTick()-t2<=Stop_time){
         if (SENSOR_2<average-11) {
             move5;
         } else if (SENSOR_2<average-8){
             move4;
             t2=CurrentTick();
         } else if (SENSOR_2<average+6){  
             move3;
             t2=CurrentTick();
         } else if (SENSOR_2<average+11){
             move2;
             t2=CurrentTick();
         } else {
             move1;
             t2=CurrentTick();
          }
      }
     Off(OUT_AB);
     coin_SE();
 }

 sub White_to_Black(int blight,int A_speed,int B_speed)
 {
     while (SENSOR_2>blight){
          OnFwd(OUT_A,A_speed);
          OnFwd(OUT_B,B_speed);
      }
     Off(OUT_AB);
 }

 sub Black_to_White(int blight,int A_speed,int B_speed)
 {
     while (SENSOR_2<blight){
          OnFwd(OUT_A,A_speed);
          OnFwd(OUT_B,B_speed);
      }
     Off(OUT_AB);
 }

 sub Start_Set()
 {
     Start_SE();
     move3;
     Wait(600);
     White_to_Black(48,30,5);
 }

 sub catch_cop(long ang1)
 {   
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,-SLOW,ang1);
 }

 sub release_cop(long ang2)
 {
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,SLOW,ang2);
 }

 sub release_ball()
 {   
     ResetTachoCount(OUT_ABC);
     OnFwd(OUT_AB,SLOW);
     Wait(700);
     Off(OUT_AB);
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,-SLOW,50);
     OnFwd(OUT_C,-20);
     Wait(1000);
     OnFwd(OUT_AB,-SLOW);
     Wait(700);
     Off(OUT_AB);
 }

 sub keep_cop()
 {
     ResetTachoCount(OUT_C);
     RotateMotor(OUT_C,-SLOW,50);
     OnFwd(OUT_C,-20);
 }

 task main()
 {
     SetSensorLight(S2);
     SetSensorLowspeed(S3);
     Start_Set();
     R_line(300,150);

     d=searchDirection(15);
     if(d>5.0){
       catch_cop(700);
       OnFwd(OUT_AB,SLOW);
       Wait(1400);
       Off(OUT_AB);
       catch_cop(300);
       OnFwd(OUT_AB,-SLOW);
       Wait(100);
       Off(OUT_AB);
 }
     ResetTachoCount(OUT_A);
     RotateMotor(OUT_A,-SLOW,370);
     OnFwd(OUT_AB,SLOW);
     Wait(600);
     Off(OUT_AB);

     catch_cop(40);
     OnFwd(OUT_C,-SLOW);
     Wait(1000);
     Off(OUT_C);

     OnFwd(OUT_AB,-SLOW);
     Wait(1000);
     Off(OUT_AB);
     White_to_Black(33,35,15);
     Black_to_White(48,35,15);
     White_to_Black(33,40,25);
     Black_to_White(48,-30,0);
     White_to_Black(33,-30,0);
     Black_to_White(50,0,-30);
     White_to_Black(33,35,30);
     Black_to_White(48,35,30);

     release_cop(50);
     tag();
     keep_cop();
     tag();
     OnFwd(OUT_AB,-SLOW);
     Wait(1000);
     Off(OUT_AB);
 }

*結果 [#j0192476]
発表1回目は、子機が1個ピンポン玉を枠内に入れたが、その後動作がおかしくなり、また親機もおかしい動作をして失敗し2点だった。2回目は子機が2個ピンポン玉を枠内に入れたが、その後左側スタート地点(A地点)の障害物のコップを弾き飛ばし4点となった。2回とも失敗である。

技術点は8.8点で、合計12.8点となり、9チーム中6位の結果となった。

*感想と反省 [#i63ecd49]
今回は失敗ばかりである。

まずロボットについてだが、1つ目は、超音波センサーは左右どちらか(どっちか忘れました)から超音波を発し、反対側で受け取っているらしいが、これを横向きに取り付けたことで、受け取りにずれが生じたり、全く関係のないものを検知したりする原因となったようである。解決策としては、他の班にあったものだが、アームと超音波センサーを前後に分けるという方法だ。これならばセンサーを縦に取り付けることができ、誤検知を減らせただろう。

2つ目は、アームの自重が大きくなり、コップを持ち上げた状態を維持するのが難しくなったことだ。上手く引っかかった場合は止まることもあるが、基本的に持ち上げただけではアームは落ちてしまうので、維持するためには弱い力を加え続ける必要があった。これだけなら簡単そうだが、加える時間や強さを誤るとアームが破損してしまうため、調整が大変だった。

3つ目は、機体が長くなってしまったことだ。これの影響で、障害物にぶつかるのを防いだり、ピンポン玉を枠内に入れるときにコップを浮かせる前に枠に当たらないように動きを調整しなければならなくなった。

要するに、3つの理由で機体の動きが制限されてしまったということだ。当然、ロボットに合わせてプログラムを作るので、上手くいくように努力したが、結果はひどいものだった。
卓越したプログラム作成能力があれば異なる結果を得られたかもしれないが、ないものを言っても仕方ない。

また、プログラムにも問題はあったと考えられる。ライントレースを含めない試行の段階では、ピンポン玉を枠内に入れる動作やコップを重ねる通信は、コップを重ねられるかはいまいち不安定だったがどちらも成功していた。これを発表用のプログラムにするとできなかったのだ。ライントレースと組み合わせるときに、本来必要な動作を書き忘れたりしたのかもしれない。

*まとめ [#mdab3f6e]


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