目次 #contents *課題2の説明 [#cfefc71d] 詳しくは #ls2(2014a/Mission2,http://yakushi.shinshu-u.ac.jp/robotics/?2014a%2FMission2) を御覧ください。 *本体 [#ocd3ff90] **メイン機 [#cf89587f] &ref(2014a/Member/an/Mission2/IMG_0044.JPG,15%,本体);&ref(2014a/Member/an/Mission2/IMG_0045.JPG,15%,本体);&ref(2014a/Member/an/Mission2/IMG_0049.JPG,15%,本体); #ref(2014a/Member/an/Mission2/IMG_0046.JPG,15%,本体) アームの巻き戻しが便利になるようにハンドルを使いました。 #ref(2014a/Member/an/Mission2/IMG_0047.JPG,15%,本体) 後ろに偏らないように付けました。 #ref(2014a/Member/an/Mission2/IMG_0048.JPG,15%,本体) 紙コップが機体の正面に来るように棒をつけ、アームには紙コップを取りやすくするためにタイヤを付けました。 #ref(2014a/Member/an/Mission2/IMG_0050.JPG,15%,本体) アームを動かせるためにいろんな部品を使いました。 #ref(2014a/Member/an/Mission2/IMG_0051.JPG,15%,本体) 光センサーはライントレースのために下を向けて2個使い、紙コップを認識するために前向きに一個使いました。 **サブ機 [#o9dbb49d] &ref(2014a/Member/an/Mission2/IMG_0037.JPG,15%,本体);&ref(2014a/Member/an/Mission2/IMG_0038.JPG,15%,本体); &ref(2014a/Member/an/Mission2/IMG_0040.JPG,15%,本体);&ref(2014a/Member/an/Mission2/IMG_0041.JPG,15%,本体); サブ機の役割は紙コップの位置を少し調整するだけなのでコンパクトにアームは固定された形で作りました。 *作戦 [#sc3db92a] メイン機はライントレースしながら紙コップを探して取り運びます。メイン機が一個目の紙コップを取る間にサブ機が2つの紙コップを軸上に運んでメイン機の探し取りを易しくします。 *プログラム [#u0e8a53f] **メイン機のプログラム [#c69062ee] #define set_power_L SetPower(OUT_A,1);SetPower(OUT_C,1); // 速度を遅くする #define THRESHOLD 45 // しきい値 #define go_forward OnFwd(OUT_AC); // 前進する #define turn_right0 OnFwd(OUT_A);OnRev(OUT_C); // 右旋回する #define turn_right1 OnFwd(OUT_A);Off(OUT_C); // 右回転する #define turn_left1 Off(OUT_A);OnFwd(OUT_C); // 左回転する #define turn_left0 OnRev(OUT_A);OnFwd(OUT_C); // 左旋回する #define STEP 1 // 動作を1回行う #define U_turn OnFwd(OUT_A);OnRev(OUT_C);Wait(180);Off(OUT_AC); // Uターンする /*コップを持ち上げる、下ろす動きをマクロ化*/ #define cup_up OnFwd(OUT_B);Wait(130);Off(OUT_B); // コップを持ち上げる #define cup_down OnRev(OUT_B);Wait(130);Off(OUT_B); // コップを下ろす /*グローバル変数。コップの色に応じて1〜3の値をとり、点Aからのパターン分岐を決める。*/ int number=0; /*論理積を使ったライントレースのサブルーチン*/ sub line(){ if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); // 2つのセンサーの値が閾値より大きい場合前進する }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); // センサー1の値が閾値より大きく、センサー3の値が閾値より小さい場合右回転する }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); // センサー1の値が閾値より小さく、センサー3の値が閾値より大きいとき左回転する }else{go_forward; Wait(STEP); // 以外の場合には前進する } } /*紙コップを掴んだ後に「点A」に辿り着いてから行う操作のサブルーチン*/ sub carry_cup(){ if(number==1){ while((SENSOR_2<47)&&(SENSOR_2>35)){ if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); // 2つのセンサーの値が閾値より大きい場合前進する }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); // センサー1の値が閾値より大きく、センサー3の値が閾値より小さい場合右回転する }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); // センサー1の値が閾値より小さく、センサー3の値が閾値より大きいとき左回転する }else{ go_forward; Wait(STEP); // 以外の場合には前進する } } go_forward; Wait(10); // 紙コップに完全に接触するまで前進。そこからの位置調整も要調査 Off(OUT_AC); cup_down; U_turn; while((SENSOR_2<47)&&(SENSOR_2>35)){ // 紙コップBかCを認識するまで前進させる if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); // 2つのセンサーの値が閾値より大きい場合前進する }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); // センサー1の値が閾値より大きく、センサー3の値が閾値より小さい場合右回転する }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); // センサー1の値が閾値より小さく、センサー3の値が閾値より大きいとき左回転する }else{ go_forward; Wait(STEP); // 以外の場合には前進する } } }else if(number==2){ while((SENSOR_1>THRESHOLD)||(SENSOR_3>THRESHOLD)){ // 交差点を認識するまで前進させる if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); // 2つのセンサーの値が閾値より大きい場合前進する }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); // センサー1の値が閾値より大きく、センサー3の値が閾値より小さい場合右回転する }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); // センサー1の値が閾値より小さく、センサー3の値が閾値より大きいとき左回転する }else{ go_forward; Wait(STEP); // 以外の場合には前進する } } turn_right0; Wait(75); // コップ2の逆方向を向かせる go_forward; Wait(10); U_turn; // 交差点でそのまま左折するとコップ2を押してしまう為 while((SENSOR_2<47)&&(SENSOR_2>35)){ // 紙コップ2を認識するまで前進させる if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); // 2つのセンサーの値が閾値より大きい場合前進する }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); // センサー1の値が閾値より大きく、センサー3の値が閾値より小さい場合右回転する }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); // センサー1の値が閾値より小さく、センサー3の値が閾値より大きいとき左回転する }else{ go_forward; Wait(STEP); // 以外の場合には前進する } } go_forward; Wait(20); // 紙コップに完全に接触するまで前進。そこからの位置調整も要調査 Off(OUT_AC); cup_down; OnRev(OUT_AC); Wait(130); while((SENSOR_1>THRESHOLD)||(SENSOR_3>THRESHOLD)){ // 交差点を認識するまで前進させる if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); // 2つのセンサーの値が閾値より大きい場合前進する }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); // センサー1の値が閾値より大きく、センサー3の値が閾値より小さい場合右回転する }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); // センサー1の値が閾値より小さく、センサー3の値が閾値より大きいとき左回転する }else{ go_forward; Wait(STEP); // 以外の場合には前進する } } turn_left0; Wait(75); // 軸にそって紙コップBかCに向かせる while((SENSOR_2>35)&&(SENSOR_2<47)){ // 紙コップを認識するまで if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); // 2つのセンサーの値が閾値より大きい場合前進する }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; // センサー1の値が閾値より大きく、センサー3の値が閾値より小さい場合右回転する Wait(STEP); }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); // センサー1の値が閾値より小さく、センサー3の値が閾値より大きいとき左回転する }else{ go_forward; Wait(STEP); // 以外の場合には前進する } } }else{ while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){ // 交差点を認識するまで前進させる if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); // 2つのセンサーの値が閾値より大きい場合前進する }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; // センサー1の値が閾値より大きく、センサー3の値が閾値より小さい場合右回転する Wait(STEP); }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); // センサー1の値が閾値より小さく、センサー3の値が閾値より大きいとき左回転する }else{ go_forward; Wait(STEP); // 以外の場合には前進する } } go_forward; Wait(10); while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){ // 交差点を認識するまで前進させる go_forward; } turn_right0; Wait(75); while((SENSOR_2>35)&&(SENSOR_2<47)){ // 紙コップを認識するまで go_forward; } go_forward; Wait(25); Off(OUT_AC); cup_down; OnRev(OUT_AC); Wait(110); while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){ // 交差点を認識するまで前進させる OnRev(OUT_AC); } turn_right0; Wait(75); } while((SENSOR_2<47)&&(SENSOR_2>35)){ // 紙コップを認識するまで if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); // 2つのセンサーの値が閾値より大きい場合前進する }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); // センサー1の値が閾値より大きく、センサー3の値が閾値より小さい場合右回転する }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); // センサー1の値が閾値より小さく、センサー3の値が閾値より大きいとき左回転する }else{ go_forward; Wait(STEP); // 以外の場合には前進する } } } /*開始直後の紙コップAを掴み振り向くまでの動きをインライン関数化*/ void start_dash(){ go_forward; Wait(120);//スタート地点からでる為の動き while((SENSOR_1>THRESHOLD)||(SENSOR_3)>THRESHOLD){ // 交差点を認識するまで前進 go_forward; } turn_right0; Wait(75); while((SENSOR_2<47)&&(SENSOR_2>35)){ // 紙コップを認識するまで if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); // 2つのセンサーの値が閾値より大きい場合前進する }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); // センサー1の値が閾値より大きく、センサー3の値が閾値より小さい場合右回転する }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); // センサー1の値が閾値より小さく、センサー3の値が閾値より大きいとき左回転する }else{ go_forward; Wait(STEP); // 以外の場合には前進する } } go_forward; Wait(100); // コップに接触するまで Off(OUT_AC); if(SENSOR_2>47){ // 白 number=1; }else if((SENSOR_2<47)&&(SENSOR_2>37)){ number=2; }else{ number=3; } // センサーの値に応じてnumberに値を入れる cup_up; U_turn; } task main(){ SetSensor(SENSOR_1,SENSOR_LIGHT); SetSensor(SENSOR_2,SENSOR_LIGHT); SetSensor(SENSOR_3,SENSOR_LIGHT); set_power_L; int cross=0; // 交差点通過回数のカウンタ start_dash(); // コップAを掴む carry_cup(); go_forward; Wait(50); // コップBに接触するまで Off(OUT_AC); if(SENSOR_2>47){ number=1; }else if((SENSOR_2>35)&&(SENSOR_2<47)){ number=2; }else{ number=3; }//センサーの値に応じてnumberに値を入れる cup_up; U_turn; ClearTimer(0); // タイマー設置 while(Timer(0)<40){ if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); // 2つのセンサーの値が閾値より大きい場合前進する }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); // センサー1の値が閾値より大きく、センサー3の値が閾値より小さい場合右回転する }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); // センサー1の値が閾値より小さく、センサー3の値が閾値より大きいとき左回転する }else{ go_forward; Wait(STEP); // 以外の場合には前進する } } carry_cup(); go_forward; Wait(50); // コップCに接触するまで Off(OUT_AC); if(SENSOR_2>47){ number=1; }else if((SENSOR_2>35)&&(SENSOR_2<47)){ number=2; }else{ number=3; } // センサーの値に応じてnumberに値を入れる cup_up; U_turn; ClearTimer(0); while(Timer(0)<20){ if((SENSOR_1>THRESHOLD)&&(SENSOR_3>THRESHOLD)){ go_forward; Wait(STEP); // 2つのセンサーの値が閾値より大きい場合前進する }else if((SENSOR_1>THRESHOLD)&&(SENSOR_3<THRESHOLD)){ turn_right1; Wait(STEP); // センサー1の値が閾値より大きく、センサー3の値が閾値より小さい場合右回転する }else if((SENSOR_1<THRESHOLD)&&(SENSOR_3>THRESHOLD)){ turn_left1; Wait(STEP); // センサー1の値が閾値より小さく、センサー3の値が閾値より大きいとき左回転する }else{ go_forward; Wait(STEP); // 以外の場合には前進する } } carry_cup(); Off(OUT_AC); } **サブ機のプログラム [#n5d79e32] サブ機の役割は紙コップを軸上に運び、メイン機を手伝うことです。 task main() { OnFwd(OUT_AC);Wait(540); OnFwd(OUT_C);OnRev(OUT_A);Wait(150); OnFwd(OUT_AC);Wait(150); // 紙コップを軸の上に置く OnRev(OUT_AC);Wait(100); OnFwd(OUT_C);OnRev(OUT_A);Wait(160); OnFwd(OUT_AC);Wait(155); OnFwd(OUT_A);OnRev(OUT_C);Wait(150); OnFwd(OUT_AC);Wait(350); OnFwd(OUT_A);OnRev(OUT_C);Wait(270); OnFwd(OUT_AC);Wait(90); // 紙コップを軸の上に置く OnRev(OUT_AC);Wait(100); Off(OUT_AC); } *自分の役割 [#kd883d83] 主にサブ機のアーム作り、プログラミングの補正を担当しました。 *感想、反省 [#j19adb07] 面白かったですが、結果は少し残念です。 伊藤くんのメイン機の方を手伝って上げたければもう少しはいい結果になったのではないかと思います。 伊藤くんのメイン機の方を手伝ってあげてたらもう少しはいい結果になったのではないかと思います。