2017b/Member/Alex/Mission3

概要

チームメンバー

チョウ カイドウ(自分)   横川 友也   大谷 風吾   カ デンエン

コース

1.png

フィールドの説明

1.フィールドは課題2で使用した紙を使用する。 
2.図のマジェンタの円に逆さまにした紙コップを置き、その中にピンポン玉を2つずつ入れておく。
3.図の黄色い円に紙コップを通常の向きに置き、障害物とする。直径6cmの円の外周をペンまたは鉛
筆でマークしておく。 
4.紙コップには色をつけたり文字や記号を書いてもよい。 
5.割り箸を使って一辺が14cmの正方形を作り、X地点を中心とした円に接するように置く。

基本ルール

1.競技時間は審判が続行不能と判断するまで、あるいはリタイアするまで。
2.図のA地点または(および)D地点からスタートする。ただし接地している部分はそれぞれの領域
内に収まるものとする(線上はOK)。上空部分は領域からはみ出していてもよい。
3.ピンポン玉をX地点に運び、割り箸で正方形に囲まれた領域に入れる。
4.紙コップをY地点がある黒線に囲まれた領域に運ぶ。
5.開始の合図から5秒以内にスタートボタンを押す作業を完了すること。
6.競技が終了するまで、ロボットに触ったり人間が遠隔で操作してはならない。
7.途中でうまく動かなくなった場合、1回限り再スタートすることができる(再スタートの際に
別プログラムで起動してよい)

ロボット

ロボット構造

1.1.png
1.2.png
1.3.jpg

考えている流れ

まずは超音波センサが降ろしている状態でコップを見つける。その後超音波センサを上げて、ロボットが前に進むと共に、コップをロボットの中押し詰める

1.4.png
1.5.png

そして、コップを掴んでまま、超音波センサを下げる、また新しいコップを見つけて、超音波センサを上げて、コップを掴む手も上げる

1.7.png

そして、そのまま上に置く

1.9.png

こういう流れでやっていこうと思っていた

プログラミング

定義

#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

サブルーチン

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;
}

数値

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

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; //コップ離す
} //終わり

親機と子機

#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

結果

結果は0点だった、コップを仕組みはそんなに精確ではなかった。それと、時間が足りなく、三つ
目のコップを探すプログラミングはできなかった。

反省

もっと早めに完成すべきだった、ロボット本体はもっと頑丈に作るべきだった

添付ファイル: file1.9.png 192件 [詳細] file1.7.png 150件 [詳細] file1.6.png 100件 [詳細] file1.5.png 179件 [詳細] file1.4.png 183件 [詳細] file1.3.jpg 169件 [詳細] file1.2.png 167件 [詳細] file1.1.png 171件 [詳細] file1.png 166件 [詳細]

トップ   編集 凍結 差分 履歴 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2018-02-13 (火) 22:27:58