[[2017b/Member]]

&size(24){目次};
#contents
*初めに [#l6e5e22a]
課題3のフィールドと大まかな説明を載せておく。
#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]]を見ていただきたい。

また、班のメンバーは私のほかに[[riko:http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2Friko%2FMission3]]氏と[[tono:http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2Ftono%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]
***アーム全体 [#b36deda8]
#ref(2017b/Member/arso/Mission3/DSCN1191.jpg,70%,アーム全体)
　　　　　　　　　　　　写真4：アーム全体

アーム全体は写真4のようになっている。いくつかの機能を持っているので、以下にそれを示す。
***センサー部分 [#aef23ab7]
#ref(2017b/Member/arso/Mission3/DSCN1193.jpg,70%,超音波センサー)
　　　　　　　　　写真5：超音波センサー

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

　　　　　　写真6：スタンド　　　　　　　　　　　　　写真7：スタート後のスタンド

これは、スタート前のルールのためだけのパーツである。スタート前は写真6のように立っており、センサーなどが接地しないように働き、始まると写真7のように倒れるようになっている。また、この部品が動きの邪魔になることはなかった。
***コップをつかむ機構 [#fee8441c]

#ref(2017b/Member/arso/Mission3/DSCN1192.jpg,70%,アーム上)
　　　　　　　　　　　写真8：上から見たアーム

アームを上から見ると写真8のようになっている。使用した部品は必要最低限にしたはずだが、重さと大きさが結構あったため、自重で腕が下がるのを防いだり、障害物にぶつからないようにする必要が出て来た。

&ref(2017b/Member/arso/Mission3/DSCN1205.jpg,70%,アーム下);
&ref(2017b/Member/arso/Mission3/コップ固定.jpg,70%,コップ固定);

　　　　　　　写真9：下から見たアーム　　　　　　　　　　　　図2：コップを固定（上から見た図）

写真9の赤線で囲まれた部分が今回の工夫である。これがあることによって、図2のようにコップを3点で支えることができ、コップが安定するようになった。パーツを腕のにはつけられなかったが、大きな問題にはならなかった。また、腕の先にあるタイヤは回転するようになっており、コップが少し離れていても内側に引き込んでつかむことができる。

#ref(2017b/Member/arso/Mission3/コップつかむ2.jpg,70%,コップを浮かせる)
　　　　　　　　　　　　　図3：割りばしの枠越え

本課題では、割り箸で作った枠の中にピンポン玉を入れるにはコップを持ち上げすぎてはいけない。このロボットではピンポン玉が転がった後に回収できないからだ。そのため、ピンポン玉を枠に入れるには、紙コップをつかんだ後少しだけ浮かせる必要があるが、アームの自重のために「少し持ち上げる」ということができない。それの解決策が、このパーツである。この支え棒を腕より少し下に取り付けることで、図3のように、コップをつかんだときに腕で支えた点より下から押されて前方が浮き上がるのである。

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

写真9のように、&color(blue){青矢印};の向きにモーターを動かすと写真右側の黒い歯車も当然その向きに回る。その回転を受けて&color(purple){紫矢印};の向きに真ん中の薄い灰色の歯車が回転する。すると、写真左側の黒色のウォームギアも同じ方向に回転する。それによって、腕の根元にある2つの歯車が&color(red){赤矢印};の方向に回転し腕が閉じるのである。逆にモーターを回せば当然腕は開く。

&ref(2017b/Member/arso/Mission3/DSCN1198.jpg,70%,コップをつかむ);
&ref(2017b/Member/arso/Mission3/コップ.jpg,70%,コップを持ち上げる);

　　　　　写真10：コップをつかんだ状態  　　　　     　　　写真11：コップを持ち上げた状態

写真10は腕を閉じてコップをつかんだ状態である。この状態でさらにモーターを腕を閉じる方向に回すと、どこかで腕の歯車が回らなくなる。すると、写真9の中心の歯車が回らなくなり、写真11のようにアームごと持ちあがる。

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

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

通信をして子機がメッセージ1を受け取ると、図5のように子機は前進してコップを軽くつかみ固定する。
#ref(2017b/Member/arso/Mission3/積み重ね4.jpg,70%,積み重ね4)
　　　　　　　　　　図6：コップの重ね方3

その後、図6のように親機も前進する。このときの移動距離の制御が難しかったので、コップにぶつかるぐらい前進させて、その後後退して位置を調整するようにした。位置を決めた後、腕を少し下げて紙コップをかぶせるようにし、子機と2度目の通信を行う。
#ref(2017b/Member/arso/Mission3/積み重ね5.jpg,70%,積み重ね5)
　　　　　　　　　　図7：コップの重ね方4

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

親機も腕を下ろしていくと図8のようにコップを重ねられる。3つ目を重ねるときも同じ方法である。


*プログラムについて [#gac8807e]
プログラムは、ライントレースなどの移動のプログラムと紙コップ関連のプログラムとで別々に作った。私は紙コップ関連のほうを担当したのでこちらを主に解説する。移動のプログラムについてもコメント文は入れておくが、詳しい内容はriko氏の[[課題2のプログラム:http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2Friko%2FMission2#t3e7a27b]]及び[[課題3のプログラム:http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2Friko%2FMission3#j25e3fbb]]を参照していただきたい。ちなみに、音のプログラムもriko氏のものである。

ここで私たちの班が立てた計画を大まかに示しておく。大まかというのは細かい移動を省いているからだ。子機の移動の順番を&color(blue){青丸};、親機の移動の順番を&color(red){赤丸};で示している。
#ref(2017b/Member/arso/Mission3/第3回コース1.jpg,70%,順路1)
　　　　　　　　　　　　　　　　　　　図9：順路1

子機はA地点からスタートし、ある程度直進してから超音波センサーでピンポン玉の入った紙コップを探して取得する（&color(blue){青の経路��};）。その後ピンポン玉を枠内に入れ（&color(blue){青の経路��};）、&color(green){緑色の丸};の地点で待機する（&color(blue){青の経路��};）。

親機はD地点を後ろ向きでスタートし、ある程度進む（&color(red){赤の経路��};）と黒線を見つけるまで回転する（&color(red){赤の経路��};）。黒線を見つけるとライントレースを行う（&color(red){赤の経路��};）。交差点に着くと、超音波センサーを使ってピンポン玉の入った紙コップを探し取得する（&color(red){赤の経路��};）。その後&color(red){赤の経路��};でピンポン玉を枠内に入れる。子機と親機で通信を行い、&color(purple){紫色の丸};の地点に紙コップを重ねる（&color(blue){青の経路��};及び&color(red){赤の経路��};）。
#ref(2017b/Member/arso/Mission3/第3回コース2.jpg,65%,順路2)
　　　　　　　　　　　　　　　　　　　図10：順路2

順路1の後、子機は何もせずに&color(green){緑色の丸};の地点で待機する。

親機はある程度後退した後（&color(red){赤の経路��};）、ライントレースに復帰する（&color(red){赤の経路��};）。ライントレースをした後（&color(red){赤の経路��};）、交差点判断で超音波センサーを使い、ピンポン玉の入った紙コップを探し取得する（&color(red){赤の経路��};）。その後ピンポン玉を枠内に入れる（&color(red){赤の経路��};）。位置を調整した後（&color(red){赤の経路��};）、子機と親機で通信を行い、&color(purple){紫色の丸};の地点に紙コップを重ねる（&color(blue){青の経路��};及び&color(red){赤の経路��};）。親機が重ねたコップをY地点に置いて終了する（&color(red){赤の経路��};）。

**モーターの対応 [#ced6b9ef]
今回のプログラムでは、アームのある方をロボットの前方として、モーターA,B,Cは以下のように対応している。子機親機とも同じ対応である。

 OUT_A　　//右側のタイヤ
 OUT_B　　//左側のタイヤ
 OUT_C　　//アーム

**親機 [#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  //おそらく高いソのシャープの音
 
 sub pose_SE()
 {
     PlayTone(MI_H,90);Wait(100);  //ミを0.09秒鳴らし0.1秒間待つ
     PlayTone(DO_H,90);Wait(100);  //高いドを0.09秒鳴らし0.1秒間待つ
     PlayTone(MI_H,90);Wait(100);  //高いミを0.09秒鳴らし0.1秒間待つ
     PlayTone(DO_H,90);Wait(100);  //高いドを0.09秒鳴らし0.1秒間待つ
 }
 
 sub one_UP_SE() 
 {
     PlayTone(MI,110);Wait(120);     //ミを0.11秒鳴らし0.12秒間待つ
     PlayTone(SO_H,110);Wait(120);   //高いソを0.11秒鳴らし0.12秒間待つ
     PlayTone(MI_H,110);Wait(120);   //高いミを0.11秒鳴らし0.12秒間待つ
     PlayTone(DO_H,110);Wait(120);   //高いドを0.11秒鳴らし0.12秒間待つ
     PlayTone(RE,110);Wait(120);     //レを0.11秒鳴らし0.12秒間待つ
     PlayTone(SO_SH,110);Wait(120);  //高いソのシャープを0.11秒鳴らし0.12秒間待つ
 }
 
 sub coin_SE() 
 {
     PlayTone(SI,90);Wait(100);       //シを0.09秒鳴らし0.1秒間待つ
     PlayTone(MI_H,140);Wait(150);    //高いミを0.14秒鳴らし0.15秒間待つ
 }
 
 sub Start_SE() 
 {
     PlayTone(MI,100);Wait(110);   //ミを0.1秒鳴らし0.11秒間待つ
     PlayTone(MI,100);Wait(250);   //ミを0.1秒鳴らし0.25秒間待つ
     PlayTone(MI,100);Wait(160);   //ミを0.1秒鳴らし0.16秒間待つ
     PlayTone(DO,90);Wait(100);    //ドを0.09秒鳴らし0.1秒間待つ
     PlayTone(MI,90);Wait(230);    //ミを0.11秒鳴らし0.12秒間待つ
     PlayTone(SO_H,90);Wait(420);  //高いソを0.09秒鳴らし0.42秒間待つ
     PlayTone(SO,170);Wait(180);   //ソを0.17秒鳴らし0.18秒間待つ
 }

***移動関連 [#td2ff648]
 #define average 45  //光センサーの値の基準
 
 #define move1 OnFwd(OUT_A,40);OnFwd(OUT_B,-35);  //右のタイヤを前に40、左のタイヤを後ろに35の強さで動かす
 #define move2 OnFwd(OUT_A,30);OnFwd(OUT_B,-20);  //右のタイヤを前に30、左のタイヤを後ろに20の強さで動かす
 #define move3 OnFwd(OUT_A,54);OnFwd(OUT_B,54);   //右のタイヤを前に54、左のタイヤを前に54の強さで動かす
 #define move4 OnFwd(OUT_B,30);OnFwd(OUT_A,-20);  //左のタイヤを前に30、右のタイヤを後ろに20の強さで動かす
 #define move5 OnFwd(OUT_B,40);OnFwd(OUT_A,-35);  //左のタイヤを前に40、右のタイヤを後ろに35の強さで動かす
 
 #define reverse_move1 OnFwd(OUT_A,-10);OnFwd(OUT_B,-30);   //右のタイヤを後ろに10、左のタイヤを後ろに30の強さで動かす
 #define reverse_move2 OnFwd(OUT_A,-54);OnFwd(OUT_B,-50);   //右のタイヤを後ろに54、左のタイヤを後ろに50の強さで動かす
 #define reverse_move3 OnFwd(OUT_B,-10);OnFwd(OUT_A,-30);   //左のタイヤを後ろに10、右のタイヤを後ろに30の強さで動かす
 
 sub Stop_Motion() 
 {
     Wait(300);    //0.3秒待つ
     pose_SE();    //サブ関数pose_SE()を呼び出す
     Wait(1000);   //1秒待つ
 }
 
 sub L_Assist_line(int Assist_time)
 {
     long ta=CurrentTick();  //時間を測定
     while(CurrentTick()-ta<=Assist_time){   //測定時間がAssist_time以下の間繰り返す
                if(SENSOR_2<average-11){     //光センサーが得た値が(基準値-11)未満のとき
                  move1;                       //move1を呼び出す
          }else if(SENSOR_2<average-7){      //光センサーが得た値が(基準値-7)未満のとき
                  move2;                       //move2を呼び出す
          }else if(SENSOR_2<average+7){      //光センサーが得た値が(基準値+7)未満のとき
                  move3;                       //move3を呼び出す
          }else if(SENSOR_2<average+11){     //光センサーが得た値が(基準値+11)未満のとき
                  move4;                       //move4を呼び出す
          }else {                            //光センサーが得た値が(基準値+11)以上のとき
                  move5;                       //move5を呼び出す
          }
      }
     coin_SE();      //サブ関数coin_SE()を呼び出す
 }
 
 sub R_Assist_line(int Assist_time) 
 {
     long tb=CurrentTick();     //時間を測定
     while(CurrentTick()-tb<=Assist_time){   //測定時間がAssist_time以下の間繰り返す
                if(SENSOR_2<average-11){     //光センサーが得た値が(基準値-11)未満のとき
                  move5;                       //move5を呼び出す
          }else if(SENSOR_2<average-7){      //光センサーが得た値が(基準値-7)未満のとき
                  move4;                       //move4を呼び出す
          }else if(SENSOR_2<average+7){      //光センサーが得た値が(基準値+7)未満のとき
                  move3;                       //move3を呼び出す
          }else if(SENSOR_2<average+11){     //光センサーが得た値が(基準値+11)未満のとき
                  move2;                       //move2を呼び出す
          }else{                             //光センサーが得た値が(基準値+11)以上のとき
             move1;                            //move1を呼び出す
          }
     }
     coin_SE();      //サブ関数coin_SE()を呼び出す
 }
 
 sub L_line(int Assist_time,int Stop_time) 
 {
     L_Assist_line(Assist_time);     //サブ関数L_Assist_line(int Assist_time)を呼び出す
     long t1=CurrentTick();          //時間を測定
     while(CurrentTick()-t1<=Stop_time){   //測定時間がStop_time以下の間繰り返す
                if(SENSOR_2<average-11){   //光センサーが得た値が(基準値-11)未満のとき
                  move1;                     //move1を呼び出す
          }else if(SENSOR_2<average-7){    //光センサーが得た値が(基準値-7)未満のとき
                  move2;                     //move2を呼び出す
                  t1=CurrentTick();          //時間を初期化
          }else if(SENSOR_2<average+7){    //光センサーが得た値が(基準値+7)未満のとき
                  move3;                     //move3を呼び出す
                  t1=CurrentTick();          //時間を初期化
          }else if(SENSOR_2<average+11){   //光センサーが得た値が(基準値+11)未満のとき
                  move4;                     //move4を呼び出す
                  t1=CurrentTick();          //時間を初期化
          }else {                          //光センサーが得た値が(基準値+11)以上のとき
                  move5;                     //move5を呼び出す
                  t1=CurrentTick();          //時間を初期化
         }
      }
     Off(OUT_AB);      //停止
     coin_SE();        //サブ関数coin_SE()を呼び出す
 }
 
 sub R_line(int Assist_time,int Stop_time)
 {
     R_Assist_line(Assist_time);     //サブ関数R_Assist_line(int Assist_time)を呼び出す
  
     long t2=CurrentTick();                //時間を測定
     while(CurrentTick()-t2<=Stop_time){   //測定時間がStop_time以下の間繰り返す
                if(SENSOR_2<average-11) {   //光センサーが得た値が(基準値-11)未満のとき
                  move5;                      //move5を呼び出す
          }else if(SENSOR_2<average-8){     //光センサーが得た値が(基準値-8)未満のとき
                  move4;                      //move4を呼び出す
                  t2=CurrentTick();           //時間を初期化
          }else if(SENSOR_2<average+6){     //光センサーが得た値が(基準値+6)未満のとき
                  move3;                      //move3を呼び出す
                  t2=CurrentTick();           //時間を初期化
          }else if(SENSOR_2<average+11){    //光センサーが得た値が(基準値+11)未満のとき
                  move2;                      //move2を呼び出す
                  t2=CurrentTick();           //時間を初期化
          }else{                            //光センサーが得た値が(基準値+11)以上のとき
                  move1;                      //move1を呼び出す
                  t2=CurrentTick();           //時間を初期化
          }
     }
     Off(OUT_AB);     //停止
     coin_SE();       サブ関数coin_SE()を呼び出す
 }
 
 sub White_to_Black(int blight,int A_speed,int B_speed)
 {
      while(SENSOR_2>blight)[     //光センサーが得た値がblightより大きい間繰り返す
           OnFwd(OUT_A,A_speed);    //右をA_speedで動かす
           OnFwd(OUT_B,B_speed);    //左をB_speedで動かす
      }
     Off(OUT_AB);    //停止
 }
 
 sub Black_to_White(int blight,int A_speed,int B_speed)
 {
     while(SENSOR_2<blight){     //光センサーが得た値がblight未満の間繰り返す
          OnFwd(OUT_A,A_speed);    //右をA_speedで動かす
          OnFwd(OUT_B,B_speed);    //左をB_speedで動かす
      }
     Off(OUT_AB);    //停止
 }
 
 sub reverse_R_Assist_line(int Assist_time)
 {
     long tc=CurrentTick();   //時間を測定
     while(CurrentTick()-tc<=Assist_time){  //測定時間がAssist_time以下のとき
                if(SENSOR_2<average-7){            //光センサーが得た値が(基準値-7)未満のとき
                  reverse_move3;                  //reverse_move3を呼び出す
          }else if(SENSOR_2<average+7){     //光センサーが得た値が(基準値+7)未満のとき
                  reverse_move2;                  //reverse_move2を呼び出す
          }else{                            //光センサーが得た値が(基準値+7)以上のとき
                  reverse_move1;                  //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){   //測定時間がStop_time以下のとき
               if(SENSOR_2<average-7){     //光センサーが得た値が(基準値-7)未満のとき
                 reverse_move3;              //reverse_move3を呼び出す
         }else if(SENSOR_2<average+7){     //光センサーが得た値が(基準値+7)未満のとき
                 reverse_move2;              //reverse_move2を呼び出す
                 t3=CurrentTick();           //時間を初期化
         }else{                            //光センサーが得た値が(基準値+7)以上のとき
                 reverse_move1;              //reverse_move1を呼び出す
          }
      }
     Off(OUT_AB);     //停止
 }
 
 sub reverse_Start_Set()
 {
     reverse_move2;    //reverse_move2を呼び出す
     Start_SE();       //サブ関数Start_SE()を呼び出す
     Wait(2000);       //2秒間待つ
     White_to_Black(48,20,35);　   //サブ関数White_to_Blackに数値を代入して呼び出す
 }

***コップ関連 [#q1feb784]
 #define FAST 50   //モーターの強さの定義(強)
 #define SLOW 30   //モーターの強さの定義(弱)
  
 #define CONN 1         //通信の接続番号
 #define SIGNALON1 11   //通信用のメッセージ1
 #define SIGNALON2 12   //通信用のメッセージ2
 
 int d;   //dを整数とする
 const float diameter=5.45;   //diameterを定数5.45とする。これはタイヤの直径である
 const float tread=11.3;      //treadを定数11.3とする。これはタイヤのトレッド幅である。
 const float pi=3.1415;       //piを定数3.1415とする。これは円周率である。
ここまで定義である。
 sub keep_cop()   //コップを持ち上げたまま維持する
 {
     ResetTachoCount(OUT_C);        //アームの回転角をリセット
     RotateMotor(OUT_C,-SLOW,50);   //モーターを強さSLOWで50度回転させアームを持ち上げる
     OnFwd(OUT_C,-20);              //アームを強さ20で持ち上げる方向に動かし続ける
 }
コップを持ち上げたまま維持するためのプログラムである。連続で試行したときは何度かアームが壊れた（少し無理に動かし続けるため）。

 sub fwdDist(float d) //距離dcm前進させる
 {
     long angle=d/(diameter*pi)*360.0;   //dcm進ませるのに必要なタイヤの回転角
     RotateMotorEx(OUT_AB,SLOW,angle,0,true,true);  //ロボットをその回転角だけ強さSLOWで動かす
     }
ロボットを任意の距離(cm)だけ進ませるためのプログラムである。dは進ませたい距離を表している。

 sub turnAng(long ang)   //ang度の時計回りの旋回
 {
     long angle=tread/diameter*ang;     //ang度の旋回に必要な回転角度
     RotateMotorEx(OUT_AB,SLOW,angle,100,true,true);  //ロボットをその角度だけ旋回させる
 }
ロボットを任意の角度だけ時計回りに旋回させるプログラムである。angは旋回させたい角度を表している。
     int searchDirection(long ang)    //ロボットがその時向いている方向を中心にang度の範囲でコップを探しそこまでの距離を測定する
 {
     long tacho_min;    //最も近い距離となるタイヤの回転数
     int d_min=300;     //最も近い距離の仮の最小値。十分大きい値にしておく
 
     long angle=(tread/diameter)*ang;   //旋回角度からタイヤの回転を計算する
     turnAng(ang/2);            //指定された角度の半分を旋回
     ResetTachoCount(OUT_AB);   //角度計測を初期化
     OnFwdSync(OUT_AB,SLOW,-100);    //反時計回りに強さSLOWで旋回
     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;
 }
先生が授業中に配ったプリントを参考にして書いた、ロボットを中心に、任意の範囲にあるものを見つけてそこまでの距離を測定するプログラムである。angは物体を探させたい範囲を表している。

 sub tag()     //コップを重ねる
 {
     keep_cop();    //コップを持ち上げて維持する。
     d=searchDirection(15);  //ロボットの向きを中心に角度15度の範囲で子機が置いたコップを探す
     if(d>5.0){     //測定した距離が5cmより遠いとき
       SendRemoteNumber(CONN,MAILBOX1,SIGNALON1);     //子機に通信しメッセージ1を送る（図4）
       Wait(5000);                     //5秒間待つ（図5）
       fwdDist(d-8.0);                 //測定した距離の8�兌蠢阿濃澆泙�
       ResetTachoCount(OUT_ABC);       //全てのモーターの回転角を初期化
       RotateMotorEx(OUT_AB,-SLOW,40,0,true,true);    //タイヤを40度回転させて前進
       RotateMotor(OUT_C,SLOW,50);                    //モータを50度動かしアームを下げる
       SendRemoteNumber(CONN,MAILBOX1,SIGNALON2);     //子機に通信しメッセージ2を送る（図6）
       Wait(5000);        //5秒間待つ（図7）
       RotateMotor(OUT_C,SLOW,100);     //モータを100度動かしアームを下げる
       ResetTachoCount(OUT_ABC);        //全てのモーターの回転角を初期化
       RotateMotor(OUT_C,SLOW,720);     //モータを720度動かしアームを開く（図8）
       RotateMotorEx(OUT_AB,-SLOW,120,0,true,true);    //タイヤを120度回転させて後退
 }
 }
コップを重ねるための親機のプログラムである。通信を行っている以外は特別なものはない。ライントレースを含める前の実験段階では、このプログラムでコップを重ねることに成功した。コメント文中の図の番号は[[コップを重ねる方法:http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2Farso%2FMission3#f9944814]]を参照。

 sub catch_cop(long ang1)      //コップをつかむ
 {   
     ResetTachoCount(OUT_C);     //アームのモーターを初期化
     RotateMotor(OUT_C,-SLOW,ang1);     //強さSLOWで角度ang1だけモーターを回し腕を閉じる
 }
腕を任意の角度だけ閉じるサブ関数である。ang1はアームのモーターの回転角度である。腕の角度ではない。
 sub release_cop(long ang2)    //コップを放す
 {
     ResetTachoCount(OUT_C);     //アームのモーターを初期化
     RotateMotor(OUT_C,SLOW,ang2);     //強さSLOWで角度ang2だけモーターを回し腕を開く
 }
腕を任意の角度だけ開くサブ関数である。ang1はアームのモーターの回転角度である。腕の角度ではない。
腕を任意の角度だけ開くサブ関数である。ang2はアームのモーターの回転角度である。腕の角度ではない。
 sub release_ball()        //ピンポン玉を枠に入れる
 {   
     ResetTachoCount(OUT_ABC);     //全てのモーターの回転角を初期化
     OnFwd(OUT_AB,SLOW);
     Wait(700);      //0.7秒間強さSLOWで前進
     Off(OUT_AB);    //停止
     ResetTachoCount(OUT_C);       //アームのモーターの回転角を初期化（いらない）
     RotateMotor(OUT_C,-SLOW,50);  //モーターを50度回転させアームを持ち上げる
     OnFwd(OUT_C,-20);        //
     Wait(1000);      //1秒間持ち上げたまま維持する
     OnFwd(OUT_AB,-SLOW);
     Wait(700);       //強さSLOWで0.7秒間後退する
     Off(OUT_AB);     //停止
 }
ピンポン玉を枠に入れるためのプログラムである。実験段階では0.7秒間の後退の代わりに回転角で試し、その時はうまくピンポン玉を入れることができていた。本番用のプログラムに組み込んだ時に動作がおかしいと言われたので変えたのだが、結局上手くいかなかった。

***プログラム全体 [#o46e00ab]
使ったものをそのまま載せたので、コップ関連のプログラムとライントレースのプログラムが混ざってしまっている。見づらくなってしまい、プログラムの試行と修正のときにスムーズにいかなかったことも失敗の原因といえるだろう。メイン関数は下の方にある。メイン関数のコメント文中の経路の番号は「[[プログラムについての:http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2Farso%2FMission3#gac8807e]]」[[図9:http://yakushi.shinshu-u.ac.jp/robotics/?plugin=attach&refer=2017b%2FMember%2Farso%2FMission3&openfile=%C2%E83%B2%F3%A5%B3%A1%BC%A5%B91.jpg]]及び[[図10:http://yakushi.shinshu-u.ac.jp/robotics/?plugin=attach&refer=2017b%2FMember%2Farso%2FMission3&openfile=%C2%E83%B2%F3%A5%B3%A1%BC%A5%B92.jpg]]を参照。

 #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);          //交差点で0.5秒間後退
     Off(OUT_AB);        //停止
     White_to_Black(43,20,25);       //位置を調整
     White_to_Black(33,20,25);       //位置を調整
     ResetTachoCount(OUT_A);         //右側のタイヤの回転角を初期化
     RotateMotor(OUT_A,-SLOW,150);   //右側のタイヤのモーターを150度後ろ向きに回転させて方向転換
     d=searchDirection(15);      //ロボットを中心に15度の範囲で紙コップを探し距離を測定する
     if(d>5.0){       //距離が5cmより遠い場合
       catch_cop(600);       //腕を少し閉じておく（腕の左や右の空間にコップが入り込むとつかめなくなるため）
       OnFwd(OUT_AB,SLOW);
       Wait(1000);           //1秒間前進
       Off(OUT_AB);          //停止
       catch_cop(500);       //コップをつかむ
       OnFwd(OUT_AB,-SLOW);
       Wait(400);            //0.4秒間後退
       Off(OUT_AB);          //停止
 }                           //��終了後からここまでが赤の経路��
     ResetTachoCount(OUT_B);         //左側のタイヤの回転角を初期化
     RotateMotor(OUT_B,-SLOW,110);   //左側のタイヤのモーターを110度後ろ向きに回転させて方向転換
     release_ball();      //ピンポン玉を枠に入れる（赤の経路�ァ�
     ResetTachoCount(OUT_A);       //右側のタイヤの回転角を初期化
     RotateMotor(OUT_A,-SLOW,130);//右側のタイヤのモーターを130度後ろ向きに回転させて方向転換
     tag();                  //コップを重ねる（赤の経路�Α�
     OnFwd(OUT_AB,-SLOW);
     Wait(1000);              //1秒間後退
     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);           //ロボットを中心に範囲15度でコップを探し距離を測定する
     if(d>5.0){              //距離が5cmより遠い場合
       catch_cop(600);       //腕を少し閉じておく（腕の左や右の空間にコップが入り込むとつかめなくなるため）
       OnFwd(OUT_AB,SLOW);
       Wait(1000);           //1秒間前進
       Off(OUT_AB);          //停止
       catch_cop(500);       //コップをつかむ
       OnFwd(OUT_AB,-SLOW);
       Wait(400);            //0.4秒間後退
       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);  //右側のタイヤのモーターを130度回し方向転換（赤の経路��）
     tag();              //コップを重ねる（赤の経路��）
     ResetTachoCount(OUT_C);        //アームのモーターを初期化
     RotateMotor(OUT_C,-SLOW,1100); //アームのモーターを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);            //0.3秒間前進
     Off(OUT_AB);          //停止
     ResetTachoCount(OUT_C);        //アームのモーターを初期化
     RotateMotor(OUT_C,SLOW,1200); //アームのモーターを1100度回し腕を開く（赤の経路��）
 }

**子機 [#u3b39ac6]
***音楽 [#q5a855b5]
音楽は親機と一緒である。親機の[[音楽:http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2Farso%2FMission3#y202a7a6]]を参照。
***ライントレース関連 [#o06e647d]
親機と異なるのは定義とサブ関数の一部がないことと、異なるサブ関数が1つあることなので追加されたもののみ載せておく。他は親機の[[ライントレース関連:http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2Farso%2FMission3#td2ff648]]を参照。
 sub Start_Set()
 {
     Start_SE();     //サブ関数Start_SE()を呼び出す
     move3;          //move3を呼び出す
     Wait(600);      //0.6秒間待つ
     White_to_Black(48,30,5);    //黒線を探す
 }

***コップ関連 [#mbb4a56f]
こちらも親機と異なるのは通信のプログラムのみなので、それだけ載せておく。他は親機の[[コップ関連:http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2Farso%2FMission3#q1feb784]]を参照。

 #define SIGNALON 11    //親機ではSIGNALON1になっていたので一応
動作には影響しないが、定数名に「1」を書き忘れていたようであるので載せておく。

 sub tag()     //コップを重ねる
 {
     int msg,counter;       //msg,counterを整数とする
     ResetTachoCount(OUT_ABC);            //全てのモーターの回転角を初期化
     RotateMotor(OUT_C,SLOW,1080);        //アームのモーターを1080度回し腕を開く
     RotateMotorEx(OUT_AB,-SLOW,360,0,true,true);       //後退する
     RotateMotor(OUT_C,-SLOW,1080);       //アームのモーターを1080度回し腕を閉じる
 
     while(counter==0){      //カウンターが0の間繰り返す（図4）
          ReceiveRemoteNumber(MAILBOX1,true,msg);    //MAILBOX1の値をmsgに格納
          if(msg==SIGNALON){          //メッセージSIGNALONを受け取った場合 
            ResetTachoCount(OUT_ABC);      //全てのモーターの回転角を初期化
            RotateMotor(OUT_C,SLOW,1080);      //アームのモーターを1080度回し腕を開く
            RotateMotorEx(OUT_AB,SLOW,120,0,true,true);    //前進する
            RotateMotor(OUT_C,-SLOW,1080);     //アームのモーターを1080度回し腕を閉じる
            counter++;                //counterに1を加える（図5）
       }
 }
     counter=0;        //カウンターを0にする
 
     while(counter==0){      //カウンターが0の間繰り返す（図6）
          ReceiveRemoteNumber(MAILBOX1,true,msg);    //MAILBOX1の値をmsgに格納
          if(msg==SIGNALON2) {          //メッセージSIGNALON2を受け取った場合 
            ResetTachoCount(OUT_ABC);      //全てのモーターの回転角を初期化
            RotateMotor(OUT_C,SLOW,360);    //アームのモーターを360度回し腕を開く
            RotateMotorEx(OUT_AB,SLOW,40,0,true,true);     //前進する（図7）
            counter++;                //counterに1を加える
 }
 }
     counter=0;        //カウンターを0にする
     ResetTachoCount(OUT_ABC);      //全てのモーターの回転角を初期化
     RotateMotor(OUT_C,SLOW,720);    //アームのモーターを720度回し腕を開く（図8）
     RotateMotorEx(OUT_AB,-SLOW,120,0,true,true);    //後退する
 }
msgは受け取ったメッセージを格納する変数である。counterは繰り返しのために数字を数える役割をしている。

今回、メッセージを受け取るプログラムをメッセージが届くまで繰り返させるために、カウンターを用いた。counterが0の間は延々とメッセージを受け取るプログラムを回し、メッセージ1を受け取るとif文の中の動作を行ってからcounterに1を加えることでループを抜け、counterを初期化する。その後、同じようにしてメッセージ2が届くまで再びメッセージを受け取るプログラムを回し…、となっている。実験段階では親機と合わせてコップを重ねることに成功したのでおそらく狙い通りになっている。コメント文中の図の番号は[[コップを重ねる方法:http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2Farso%2FMission3#f9944814]]を参照。


***プログラム全体 [#e743cbf5]
親機同様使ったものをそのまま載せたので、コップ関連のプログラムとライントレースのプログラムが混ざってしまっている。メイン関数は下のほうにある。メイン関数のコメント文中の経路の番号は「[[プログラムについての:http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2Farso%2FMission3#gac8807e]]」[[図9:http://yakushi.shinshu-u.ac.jp/robotics/?plugin=attach&refer=2017b%2FMember%2Farso%2FMission3&openfile=%C2%E83%B2%F3%A5%B3%A1%BC%A5%B91.jpg]]及び[[図10:http://yakushi.shinshu-u.ac.jp/robotics/?plugin=attach&refer=2017b%2FMember%2Farso%2FMission3&openfile=%C2%E83%B2%F3%A5%B3%A1%BC%A5%B92.jpg]]を参照。
 #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);     //ロボットを中心に15度の範囲でコップを探す
     if(d>5.0){          //距離が5cmより遠い場合
       catch_cop(700);   //腕を少し閉じておく（腕の左や右の空間にコップが入り込むとつかめなくなるため）
       OnFwd(OUT_AB,SLOW);
       Wait(1400);        //1.4秒間前進
       Off(OUT_AB);       //停止
       catch_cop(300);    //コップをつかむ
       OnFwd(OUT_AB,-SLOW);
       Wait(100);          //0.1秒間後退
       Off(OUT_AB);        //停止（青の経路�　�
 }
     ResetTachoCount(OUT_A);       //右側のタイヤのモーターの回転角を初期化
     RotateMotor(OUT_A,-SLOW,370);   //右側のタイヤのモーターを370度後ろに回し方向転換
     OnFwd(OUT_AB,SLOW);
     Wait(600);            //0.6秒間前進
     Off(OUT_AB);          //停止
     catch_cop(40);        //アームのモーターを40度回しコップを持ち上げる
     OnFwd(OUT_C,-SLOW); 
     Wait(1000);      //持ち上げたまま1秒間維持する
     Off(OUT_C);      //停止
     OnFwd(OUT_AB,-SLOW);
     Wait(1000);      //1秒間後退
     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);    //8つのサブ関数で緑丸の地点に移動（青の経路��）
     release_cop(50);
     tag();         //コップを重ねる（青の経路�ぁ�
     keep_cop();    //コップを持ち上げ維持する
     tag();         //コップを重ねる（青の経路�ァ�
     OnFwd(OUT_AB,-SLOW);
     Wait(1000);      //1秒間後退
     Off(OUT_AB);     //停止
 }

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

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

*考察 [#i63ecd49]
レポートを作るにあたってロボットの機構やプログラムを見直したが、どちらも作りこみが非常に甘かったように思う。

まずロボットについてだが、1つ目は、超音波センサーは左右どちらか（どっちかは忘れました）から超音波を発し、もう一方で受け取っているらしいが、これを横向きに取り付けたことで、受け取りにずれが生じたり、そもそも検知に失敗してしまう原因となった可能性がある。解決策としては、他の班にあったものだが、アームと超音波センサーを前後に分けるという方法だ。これならばセンサーを縦に取り付けることができ、誤検知を減らせただろう。

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

3つ目は、機体が長くなってしまったことだ。これの影響で、障害物にぶつかるのを防いだり、ピンポン玉を枠内に入れるときにコップを浮かせる前に枠に当たらないように動きを調整しなければならなくなった。これは、アームの取り付け位置を変えるか、あるいはアームを全く別のものにするしかないだろう。

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

また、プログラムにも問題があったと考えられる。ライントレースを含めない試行の段階では、ピンポン玉を枠内に入れる動作はほとんど成功していたし、コップを重ねる動作も成功率は高いとは言えなかったができていた。これを発表用のプログラムにすると謎の行動をするようになった。改めてプログラムを見てみると、移動用のプログラムと組み合わせるときに、本来必要な動作を書き忘れたり、逆に不要な動作を入れてしまったようである。さらに、作ったサブ関数を使えるところで使っていなかったり、（必要なのかもしれないが）自分でもよくわからない動作が混じっているようだ。プログラムを作っているときには気づけなかったことから、そのときはかなり集中力がなく切れていたようである。また、、大きな要因となったかは分からないが、移動が合わさることで直前の動作による慣性やロボットの向きに影響を受けた可能性もある。

*感想 [#ha368ba0]
あらゆる状況や問題を考慮した上で、結果を出せるプログラムやロボットを作らなければならないのは当然のことなのだが、結果は遠く及ばないものだった。少ない時間で良いものを作るという理想を実現できなかったのはやはり残念であった。

*まとめ [#mdab3f6e]
これまでの3つの課題を通して、プログラミングの基礎をしっかり学ぶことができたように思う。また、ハード面の重要性もよく理解できた。どれ程優れたプログラムを作れたとしても、ハードがしっかりしていなければ十分な結果を得ることはできないということを実感することができた。これからもプログラミングをすることはあると思うので、この授業で得られた知識や経験を生かしていきたい。
トップ   編集 差分 履歴 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS