2018a/Member

缶を積むロボット2台

課題内容

XYの円にある缶を指定の場所に移動させる

ロボットはAまたはA'からスタートする

2018a-mission3.png

構造

私たちは,二つのロボットで挑んだ.

1台目

動作は二段目の缶を持ち,その後落とす

motiage.gif

センサーを使わずに移動をプログラムした.

タイヤは前輪2輪,後輪1輪である.

アームは水平になるように本体を水平に固定した.

yoko.gif

アームはギアを使い,つかむ際にずれないようにした.

ue.gif

2台目

動作は最初に超音波センサで缶の場所を調べる.

1台目が2段目の缶をつかんでいるので1・3段目の缶をつかみ,引いて移動させる.

yoko2.gif
ue2.gif

アームは二つが連動して動く.

アームを付けるために本体を反対に付け裏のジョイントを使った.

naname.gif

挟むときに幅が少しずれるので冗談のアームにタイヤを付けた.

mae2.gif

プログラム

連動

二つが通信して動く.

例:1台目が二段目の缶をつかみ,2台目にメッセージを送り,2台目が1・3段目の缶を動かす.

SendRemoteNumber(1,MAILBOX1,13);//送信
ReceiveRemoteNumber(MAILBOX1,true,msg);   // MAILBOX1の値を受け取りmsgに格納

前進後退(1台目)

直進動作は距離を変えられるようにサブルーチンで作った.

変数iに進みたい距離(cm)を入れる.

また,缶をつかむ際にぶつかっても倒れないようにゆっくり動くためのサブルーチンも用意した.

sub zen(int i)//前進
{
	int t;
	t=i*50;
	OnFwd(OUT_B,50); OnFwd(OUT_C,49);
	Wait(t);
	Off(OUT_BC);
}
sub zeny(int i)//ゆっくり前進
{
	int t;
	t=i*100;
	OnFwd(OUT_B,25); OnFwd(OUT_C,25);
	Wait(t);
	Off(OUT_BC);
}
sub kou(int i)//後退
{
	int t;
	t=i*50;
	OnRev(OUT_B,50);OnRev(OUT_C,49);
	Wait(t);
	Off(OUT_BC);
}

アームの開閉(1台目)

アームはギアの回転で開閉させる.

角度を決めると強くつかめず,次の動作にも進めないのでただ回転させた

sub armc()
{
	OnFwd(OUT_A,30);
}

sub armo()
{
	OnRev(OUT_A,30);
}

回転 (1台目)

本体の回転は右90度と左90度の二つ用意した.

#define kail OnFwd(OUT_B,50);OnRev(OUT_C,50);Wait(500);Off(OUT_BC);
#define kair OnFwd(OUT_C,50);OnRev(OUT_B,50);Wait(500);Off(OUT_BC);

缶を探す(2台目)

/*空き缶を探して、空き缶の手前まで進むプログラム*/
#define SPEED_SLOW 30
const float diameter = 5.45;  // タイヤの直径(cm)
const float track = 10.35;    // タイヤのトレッド幅(cm)
const float pi = 3.1415;      // 円周率

void fwdDist(float d) // 距離 d cm 前進
{
    long angle;
    angle = d/(diameter*pi)*360.0;      // 角度を計算する
    RotateMotorEx(OUT_BC, SPEED_SLOW, angle, 0, true, true);
}

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

int searchDirection(long ang)  // 現在の方向を中心にang度の範囲で探し
                               // 障害物までの距離を返す
{
    long angle,tacho_min=0, tacho_corr ;
    int d, d_min;

    d_min = 300;           // 仮の最小値

    angle = (track/diameter)*ang;  // 旋回角度からタイヤの回転を計算
    turnAng(ang/2);                // 指定された角度の半分を旋回
    ResetTachoCount(OUT_BC);       // 角度計測をリセット

    OnFwdSync(OUT_BC,SPEED_SLOW,-100);   // 反時計回りに旋回
    while(MotorTachoCount(OUT_B)<=angle){

        d = SensorUS(S1);  // 現在の距離

        if (d < d_min){
            d_min = d;        // 仮の最小値を更新
            tacho_min = MotorTachoCount(OUT_B);  // 仮の最小値を更新したときのモータの回 
転角度
        }
    }
    OnFwdSyncEx(OUT_BC,SPEED_SLOW,100,RESET_NONE);
    until(MotorTachoCount(OUT_B) <= tacho_min || SensorUS(S1) <= d_min);

    Wait(14);  // 微調整
    Off(OUT_BC);Wait(500);
    return d_min;
    OnFwd(OUT_C,30);
    OnRev(OUT_B,30);
    Wait(200);
}

モータの動作(2台目)

二つのモータの制御を一つの関数で行う.

#define SPEED_H 40
#define SPEED_L 35
#define OnRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL);
#define go_forward  OnRL(SPEED_H, SPEED_H);    // 前進
#define turn_right1  OnRL(SPEED_L, -SPEED_L);  // 右旋回
#define turn_right0  OnRL(SPEED_L, 0);         // 右折
#define turn_left0 OnRL(0, SPEED_L);         // 左折
#define turn_left1 OnRL(-SPEED_L, SPEED_L);  // 左旋回

全体

1台目

#define kail OnFwd(OUT_B,50);OnRev(OUT_C,50);Wait(500);Off(OUT_BC);
#define kair OnFwd(OUT_C,50);OnRev(OUT_B,50);Wait(500);Off(OUT_BC);
sub armc()
{
	OnFwd(OUT_A,30);
}

sub armo()
{
	OnRev(OUT_A,30);
}

sub zen(int i)
{
	int t;
	t=i*50;
	OnFwd(OUT_B,50); OnFwd(OUT_C,49);
	Wait(t);
	Off(OUT_BC);
}
sub zeny(int i)
{
	int t;
	t=i*100;
	OnFwd(OUT_B,25); OnFwd(OUT_C,25);
	Wait(t);
	Off(OUT_BC);
}
sub kou(int i)
{
	int t;
	t=i*50;
	OnRev(OUT_B,50);OnRev(OUT_C,49);
	Wait(t);
	Off(OUT_BC);
}
task main()
{ 
	int msg,frg1=1,frg2=1;
	zen(51);
	kair;
	zen(75);
	kair;
	zeny(20);
	armc();//xに移動
	SendRemoteNumber(1,MAILBOX1,11);//送信
	while(frg1){
		ReceiveRemoteNumber(MAILBOX1,true,msg);//受信
		if(msg==12){
			armo();
			kou(20);
			kair;
			zen(44);
			kail;
			zeny(20);
			armc();//yに移動
			frg1=0;
			SendRemoteNumber(1,MAILBOX1,13);//送信
		}
	}
	while(frg2){
		ReceiveRemoteNumber(MAILBOX1,true,msg);//受信
		if(msg==14){
			armo();
			frg2=0;
		}
	}
}

2台目

#define SPEED 50
#define SPEED_SLOW 30
#define SPEED_H 40
#define SPEED_L 35
#define OnRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL);
#define go_forward  OnRL(SPEED_H, SPEED_H);    // 前進
#define turn_right1  OnRL(SPEED_L, -SPEED_L);  // 右旋回
#define turn_right0  OnRL(SPEED_L, 0);         // 右折
#define turn_left0 OnRL(0, SPEED_L);         // 左折
#define turn_left1 OnRL(-SPEED_L, SPEED_L);  // 左旋回

/*空き缶を探して、空き缶の手前まで進むプログラム*/
const float diameter = 5.45;  // タイヤの直径(cm)
const float track = 10.35;    // タイヤのトレッド幅(cm)
const float pi = 3.1415;      // 円周率

void fwdDist(float d) // 距離 d cm 前進
{
    long angle;
    angle = d/(diameter*pi)*360.0;      // 角度を計算する
    RotateMotorEx(OUT_BC, SPEED_SLOW, angle, 0, true, true);
}

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

int searchDirection(long ang)  // 現在の方向を中心にang度の範囲で探し
                               // 障害物までの距離を返す
{
    long angle,tacho_min=0, tacho_corr ;
    int d, d_min;

    d_min = 300;           // 仮の最小値

    angle = (track/diameter)*ang;  // 旋回角度からタイヤの回転を計算
    turnAng(ang/2);                // 指定された角度の半分を旋回
    ResetTachoCount(OUT_BC);       // 角度計測をリセット

    OnFwdSync(OUT_BC,SPEED_SLOW,-100);   // 反時計回りに旋回
    while(MotorTachoCount(OUT_B)<=angle){

        d = SensorUS(S1);  // 現在の距離

        if (d < d_min){
            d_min = d;        // 仮の最小値を更新
            tacho_min = MotorTachoCount(OUT_B);  // 仮の最小値を更新したときのモータの回 
転角度
        }
    }
    OnFwdSyncEx(OUT_BC,SPEED_SLOW,100,RESET_NONE);
    until(MotorTachoCount(OUT_B) <= tacho_min || SensorUS(S1) <= d_min);

    Wait(14);  // 微調整
    Off(OUT_BC);Wait(500);
    return d_min;
    OnFwd(OUT_C,30);
    OnRev(OUT_B,30);
    Wait(200);
}

task main()
{
    int msg,frg1=1,frg2=1;
    while(frg1){
    ReceiveRemoteNumber(MAILBOX1,true,msg);   // MAILBOX1の値を受け取りmsgに格納
    if(msg==11){ 
       SetSensorLowspeed(S1);  // 缶の方向を探した後、近づいて10cm手前で停止

       int d = searchDirection(120);
       if (d > 10) {
                     fwdDist(d-10.0);
       }
       OnFwd(OUT_B,30);
       OnFwd(OUT_C,30);
       Wait(500);
       Off(OUT_BC);
       Off(OUT_A);
       OnFwd(OUT_B,30);
       OnFwd(OUT_C,30);
       Wait(400);
       Off(OUT_BC);
       OnRev(OUT_A,30);
       Wait(1000);
       OnRev(OUT_BC,30);
       Wait(1000);
       Off(OUT_BC);
       SendResponseNumber(MAILBOX1,12);
       frg1=0;
         }
    }
    while(frg2){
     ReceiveRemoteNumber(MAILBOX1,true,msg);   // MAILBOX1の値を受け取りmsgに格納
     if(msg==13){   
          turn_left1;
          Wait(600);
          Off(OUT_BC);
          OnFwd(OUT_B,40);
          OnFwd(OUT_C,40);
          Wait(3500);
          Off(OUT_BC);
          turn_right1;
          Wait(1000);
          Off(OUT_BC);
          OnFwd(OUT_BC,30);
          Wait(1000);
          Off(OUT_BC);
          OnFwd(OUT_A,30);
          Wait(500);
          Off(OUT_A);
          OnRev(OUT_BC,30);
          Wait(1000);
          Off(OUT_BC);
          turn_right1;
          Wait(800);
          Off(OUT_BC);
          OnFwd(OUT_B,30);
          OnFwd(OUT_C,30);
          Wait(500);
          Off(OUT_BC);
          turn_left1;
          Wait(1000);
          Off(OUT_BC);
          OnFwd(OUT_BC,30);
          Wait(2000);
          Off(OUT_BC);
          turn_left0;
          Wait(1000);
          Off(OUT_BC);
          OnRev(OUT_A,30);
          Wait(1000);
          OnRev(OUT_BC,30);
          Wait(1000);
          Off(OUT_BC);
          turn_right1;
          Wait(2000);
          Off(OUT_BC);
          go_forward;
          Wait(3500);
          Off(OUT_BC);
          OnFwd(OUT_A,30);
          Wait(1000);
          Off(OUT_A);
          SendResponseNumber(MAILBOX1,14);
          frg2=0;
     }
   }
}

考察

作成について,内容の共有がうまくできていなかったため考えに間違っていたところがあった.

そのため,直前まで調整に時間をとられてしまった.

また,1台目にセンサを付けなかったため調整の手間が増え,しかも正確な動作もできない.


添付ファイル: file2018a-mission3.png [詳細] fileyoko2.gif [詳細] fileyoko.gif [詳細] fileue2.gif 1件 [詳細] fileue.gif [詳細] filenaname.gif 1件 [詳細] filemae2.gif [詳細] filemotiage.gif [詳細]

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 単語検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2018-08-12 (日) 19:25:01 (125d)