[[2017b/Member/Alex/Mission3]]
#contents

*概要 [#z14b363d]
**チームメンバー [#s1f88f59]
チョウ カイドウ(自分)   横川 友也   大谷 風吾   カ デンエン
**コース [#q445b205]
#ref(2017b/Member/Alex/Mission3/1.png,80%)
**フィールドの説明 [#r747fa6f]
 1.フィールドは課題2で使用した紙を使用する。 
 2.図のマジェンタの円に逆さまにした紙コップを置き、その中にピンポン玉を2つずつ入れておく。
 3.図の黄色い円に紙コップを通常の向きに置き、障害物とする。直径6cmの円の外周をペンまたは鉛
 筆でマークしておく。 
 4.紙コップには色をつけたり文字や記号を書いてもよい。 
 5.割り箸を使って一辺が14cmの正方形を作り、X地点を中心とした円に接するように置く。
**基本ルール [#w55d4908]
 1.競技時間は審判が続行不能と判断するまで、あるいはリタイアするまで。
 2.図のA地点または(および)D地点からスタートする。ただし接地している部分はそれぞれの領域
 内に収まるものとする(線上はOK)。上空部分は領域からはみ出していてもよい。
 3.ピンポン玉をX地点に運び、割り箸で正方形に囲まれた領域に入れる。
 4.紙コップをY地点がある黒線に囲まれた領域に運ぶ。
 5.開始の合図から5秒以内にスタートボタンを押す作業を完了すること。
 6.競技が終了するまで、ロボットに触ったり人間が遠隔で操作してはならない。
 7.途中でうまく動かなくなった場合、1回限り再スタートすることができる(再スタートの際に
 別プログラムで起動してよい)

*ロボット [#i7da0f92]
**ロボット構造 [#i9b49f86]
#ref(2017b/Member/Alex/Mission3/1.1.png,50%)
#ref(2017b/Member/Alex/Mission3/1.2.png,50%)
#ref(2017b/Member/Alex/Mission3/1.3.jpg,50%)

*考えている流れ [#ebf68b0c]
まずは超音波センサが降ろしている状態でコップを見つける。その後超音波センサを上げて、ロボットが前に進むと共に、コップをロボットの中押し詰める
#ref(2017b/Member/Alex/Mission3/1.4.png,30%)
#ref(2017b/Member/Alex/Mission3/1.5.png,30%)
そして、コップを掴んでまま、超音波センサを下げる、また新しいコップを見つけて、超音波センサを上げて、コップを掴む手も上げる
#ref(2017b/Member/Alex/Mission3/1.7.png,30%)
そして、そのまま上に置く
#ref(2017b/Member/Alex/Mission3/1.9.png,30%)
こういう流れでやっていこうと思っていた
*プログラミング [#ocf301d8]
**定義 [#gbd547c5]
#define SPEED 100
#define SPEED_SLOW 60
#define turn_left1 OnFwd(OUT_A,80 * x); OnRev(OUT_C,35 * x);
#define turn_left2 OnFwd(OUT_A,100 * x); OnFwd(OUT_C,40 * x);
#define turn_left3 OnFwd(OUT_A,80 * x); OnRev(OUT_C,90 * x);
#define turn_left5 OnFwd(OUT_A,80 * x); OnRev(OUT_C,1 * x);
#define go_forward OnFwd(OUT_A,80 * x); OnFwd(OUT_C,87 * x);
#define go_back OnRev(OUT_A,100 * x); OnRev(OUT_C,85 * x);
#define turn_right1 OnRev(OUT_A,35 * x); OnFwd(OUT_C,80 * x);
#define turn_right2 OnFwd(OUT_A,30 * x); OnFwd(OUT_C,100 * x);
#define turn_right3 OnRev(OUT_A,80 * x); OnFwd(OUT_C,80 * x);
#define AD SendRemoteNumber(1,MAILBOX1,01);       //アームを閉じて締める
#define AU SendRemoteNumber(1,MAILBOX1,02);       //アームを上げる
#define TD OnRev(OUT_B,45 * x); Wait(1000);
#define TU OnFwd(OUT_B,45 * x); Wait(1000);
#define CupOff SendRemoteNumber(1,MAILBOX1,03);   //コップを離す
#define turn_right4 OnRev(OUT_A,80 * x); OnFwd(OUT_C,60 * x);
#define turn_right5 OnRev(OUT_A,80 * x); OnFwd(OUT_C,1 * x);
#define turn_right6 OnRev(OUT_A,1 * x); OnFwd(OUT_C,80 * x);
#define DO  523
#define x 0.65             //かけ数
#define z 1.0
**サブルーチン [#c8a1dbf6]
 sub IKK()
  {
  int BP = 0;
  while (BP < 125){
  if (SENSOR_2 < 45) {
       turn_right1;
       BP++;
  }else{
     if (SENSOR_2 < 49 ) {
      turn_right2;
    } else if (SENSOR_2 < 56 ) {
      go_forward;
    } else if (SENSOR_2 < 58) {
      turn_left2;
    } else {
      turn_left1;
    }
    BP = 0;
    }
   Wait(1);
   }
   Off(OUT_AC);
   PlayTone(DO,100); Wait(100);
   Wait(10);
   BP = 0;
 }

 sub DKK()
  {
  int BP = 0;
  while (BP < 170){
  if (SENSOR_2 < 45) {
       turn_left1;
       BP++;
       }else{
     if (SENSOR_2 < 49 ) {
      turn_left2;
    } else if (SENSOR_2 < 54 ) {
      go_forward;
    } else if (SENSOR_2 < 55) {
      turn_right3;
    } else {
      turn_right4;
    }
    BP = 0;
    }
   Wait(1);
   }
   Off(OUT_AC);
   PlayTone(DO,100); Wait(100);
   BP = 0;
 }


 sub DKK2()
 {
  int BP = 0;
  while (BP < 170){
  if (SENSOR_3 < 45) {
       turn_left1;
       BP++;
       }else{
     if (SENSOR_3 < 49 ) {
      turn_left2;
    } else if (SENSOR_3 < 54 ) {
      go_forward;
    } else if (SENSOR_3 < 55) {
      turn_right3;
    } else {
      turn_right4;
    }
    BP = 0;
    }
   Wait(1);
   }
   Off(OUT_AC);
   PlayTone(DO,100); Wait(100);
   BP = 0;
 }
**数値 [#w5433374]
 const float diameter = 5.45;   //タイヤ直径
 const float track = 10.35;    //タイヤトレッド幅
 const float pi = 3.1415;     //円周率

 void fwdDist(float d) //距離 dcm前進
 {
  long angle = d/(diameter*pi)*360.0;   //必要なタイヤの回転角度
  RotateMotorEx(OUT_AC, SPEED_SLOW, angle, 0, true ,true);
 }

 void turnAng(long ang) //角度ang度の時計回りの旋回
 {
  long angle = track/diameter * ang;
  RotateMotorEx(OUT_AC, SPEED_SLOW, angle, 100, true, true);
 }

 int searchDirection(long ang) //現在の方向を中心にang度の範囲で探し
                                   //障害物までの距離を返す
 {
  long tacho_min;  //最も近い距離を実現するタイヤの回転数
  int d_min = 300;  //最も近い距離の仮の最小値(十分大きくとっておく)
 
 long angle = (track/diameter)*ang;  //旋回角度からタイヤの回転を計算
 turnAng(ang/2);   //指定された角度の半分を旋回
 ResetTachoCount(OUT_AC);   //角度計測をリセット

 OnFwdSync(OUT_AC,SPEED_SLOW,-100);  //半時計回りに旋回
 while(MotorTachoCount(OUT_A)<=angle) {
  if (SensorUS(S1)<d_min){ //現在の距離が借りの最小値より小さい場合
   d_min = SensorUS(S1); //仮の最小値を更新
   tacho_min = MotorTachoCount(OUT_A); //この時のタイヤの回転数を記録
  }
 }
 OnFwdSyncEx(OUT_AC,SPEED_SLOW,100,RESET_NONE);
 until(MotorTachoCount(OUT_A)<=tacho_min || SensorUS(S1)<=d_min);

 Wait(14); //微調整
 Off(OUT_AC);Wait(500);
 return d_min;
 }
**task main [#u639c891]
 task main()
 {
 SetSensorLowspeed(S1);
 SetSensorLight(S2);
 SetSensorLight(S3);
 go_forward;
 Wait(500);  //少し前進
 turn_left3;
 Wait(200);
 while (SENSOR_2 > 45)
 {
 turn_left3;  
 }

 turn_right1;
 Wait(100);
 IKK(); //Fまでライントレース
 turn_left3; 
 Wait(1000); //左向く

 int d = searchDirection(70);//コップ探す
 if (d > 10){
  fwdDist(d-10.0);//コップに近づく
  }

 TU; //超音波センサーをあげる

 go_forward;
 Wait(2900);
 AD; //コップを取る
 Off(OUT_AC);
 Wait(3000);
 go_back;
 Wait(500);

 turn_left3;
 Wait(700);

 while (SENSOR_2 > 45)
 {
 go_back;  
 }
 go_back;
 Wait(250);
 Off(OUT_AC);
 Wait(200);

 while (SENSOR_2 > 45)
 {
 go_back;  
 }

 go_back;
 Wait(450);

 Off(OUT_AC);
 Wait(450);

 while (SENSOR_2 > 45)
 {
 go_back;  
 }

 go_back;
 Wait(250);

 Off(OUT_AC);
 Wait(200);

 turn_right5; 
 Wait(700);

 while (SENSOR_3 > 45)
 {
 turn_right5;  
 }



 DKK2(); //Sを左折
 //Qに到着
 turn_right6;
 Wait(1100);
 while (SENSOR_2 > 45)
 {
 turn_right6;  
 }

 turn_right6;
 Wait(300);

 Off(OUT_AC);
 Wait(1000);

 go_forward;  //Xに入る
 Wait(2500);
 AU; //Xでコップを上げる
 Off(OUT_AC);
 Wait(3000);

 go_back;
 Wait(1200);
 Off(OUT_AC);
 Wait(2000);
 while (SENSOR_2 > 45)
 {
 go_back;  
 }
 go_back;
 Wait(500);

 while (SENSOR_2 > 45)
 {
 go_back;  
 }

 Off(OUT_AC);
 Wait(1000);
 turn_right5;
 Wait(500);

 while (SENSOR_3 > 45)
 {
   turn_right5;
 }
 turn_right5;
 Wait(600);

 while (SENSOR_3 > 45)
 {
   turn_right5;
 }

 Off(OUT_AC);
 Wait(1000);

 go_forward;
 Wait(1000);

 while (SENSOR_2 > 45)
 {
 go_forward;
 }
 Off(OUT_AC);
 Wait(1000);

 while (SENSOR_3 > 45)
 {
 turn_left5;
 }
 turn_left5;
 Wait(300);

  go_forward;
  Wait(500);

 TD; //超音波下げる 

 Off(OUT_AC);
 Wait(2000);
 d = searchDirection(120);//コップ探す
 if (d > 10){
  fwdDist(d-10.0);//コップに近づく
  }
 TU; //超音波センサーをあげる
 Off(OUT_AC);
 Wait(1000);
 go_forward;
 Wait(1300);

 Off(OUT_AC);
 Wait(1000);

 turn_left3;
 Wait(1400);

 Off(OUT_AC);
 Wait(2000);
 go_forward;
 Wait(2300);
 go_back;
 Wait(100);
 Off(OUT_AC);
 Wait(500);
 AD; //コップを取る(重ねる)
 Off(OUT_AC);
 Wait(3000);

 go_back;     //kaiki
 Wait(1900);
 turn_right3;
 Wait(1400);
 go_back;
 Wait(1300);

 while (SENSOR_2 > 45)
 { 
 go_back;  
 }

 turn_left5;
 Wait(1200);
 while (SENSOR_3 > 45)
 {
 turn_left5;  
 }

 turn_right5;
 Wait(300);
 go_forward;
 Wait(1000);
 while (SENSOR_2 > 45)
 {
 go_forward;  //Xまで直進
 }


 AU; //Xでコップを上げる

 turn_left3;
 Wait(200);
 go_back;
 Wait(300);

 CupOff;
 while (SENSOR_2 > 45)
 {
 go_back;  
 }               //黒い線までバックする

 turn_right3;
 Wait(1000); //右折する

 go_forward;
 Wait(1000); //少し直進する


 d = searchDirection(120); //コップ探す
 if (d > 10){
  fwdDist(d-10.0);//コップに近づく
  }
 TU; //超音波センサーをあげる
 go_forward;
 Wait(1000);
 AD; //コップを取る

 go_back;
 Wait(1000);  //バックしてコースにもどる   
 DKK();  //Bまでライントレースする

 turn_left3; //左折する
 Wait(1000);
 DKK(); //Pに到着

 turn_right3;
 Wait(200);
 go_forward;
 Wait(1000); //Xにはいる

 AU; //コップを上げる

 go_back; 
 Wait(1000); //Xから出る
 turn_left3;   //左を向く
 Wait(1000);

 DKK(); //PからQまで円周上をライントレース
 turn_left3;
 Wait(1000);
 DKK(); //Sに到着 

 turn_right3;
 Wait(200);
 go_forward; //Yに入る
 Wait(1000);
 AD; //コップを置く
 CupOff; //コップ離す
 } //終わり
**親機と子機 [#faafbbc5]


 #define ArmDown OnRev(OUT_A,50); Wait(500);
 #define ArmUp   OnFwd(OUT_A,70); Wait(300);
 #define ArmClose OnRev(OUT_C,70); Wait(1000);
 #define ArmOpen OnFwd(OUT_C,30); Wait(500);
 #define ArmStay OnRev(OUT_AC,1);

 task main ()
 {
  int msg;
 while(true)
 {
  ReceiveRemoteNumber(MAILBOX1,true,msg);
 
   if (msg == 01)
  {
  Off(OUT_AC);
  ArmDown;
  ArmClose;
  msg = 04;
  }   
 
   if (msg == 02)
  {
  ArmStay;
  ArmUp;
  msg = 04;
  }  
 
   if (msg == 03)
  {
  Off(OUT_AC);
  ArmUp;
  ArmOpen;
  }
   if (msg == 04)
  {
   ArmStay;
   Off(OUT_A);
   }
  }
 } 
詳しい部分はプログラミングの作者:大谷 風吾のページを参考ください。
http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2Fwaitu%2FMission3
*結果 [#e6d36b30]
 結果は0点だった、コップを仕組みはそんなに精確ではなかった。それと、時間が足りなく、三つ目のコップを探すプログラミングはできなかった。
 結果は0点だった、コップを仕組みはそんなに精確ではなかった。それと、時間が足りなく、三つ
 目のコップを探すプログラミングはできなかった。
*反省 [#ad3686e4]
 もっと早めに完成すべきだった、ロボット本体はもっと頑丈に作るべきだった

トップ   編集 差分 履歴 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS