課題3〜ロボットコンテスト〜

目次

ルール

ロボコンのページ参照

リンク:2012a/ロボコン

車体

車体はチームの仲間が作成したのでそちらへ リンク:2012a/A3/imoto/M3

プログラム

方針

 スタート位置に近い缶をつかみ、シーケンス動作で缶を置きにいく

 NXTを2台使い,Bluetoothを使いNXT同士の通信をしました。またマスターを移動関係のものに,スレーブを缶つかむ動作と上下する動作専用にしました

ソース

マスター(駆動系)

/*--------------------------------------------
	缶積み上げロボット(マスター)
				 ekaki.nxc
---------------------------------------------*/

/*--------------------------------------------
	       	    定数
---------------------------------------------*/
#define PAI			3			//円周率
#define T_R			55			//タイヤの直径(mm)
#define T_D			131			//車体のトレッド(mm)

#define DUTY		40

#define FF			1			//前進
#define FR			2			//右旋回
#define RF			3			//左旋回
#define RR			4			//後進
#define BB			5			//ブレーキ

#define US_KYORI   10

/*--------------------------------------------
	  関数プロトタイプ宣言
---------------------------------------------*/
void mt_ct(int mt_ct,int ang_d);

/*--------------------------------------------
		グローバル変数
---------------------------------------------*/

/*--------------------------------------------
		メインルーチン
---------------------------------------------*/
task main()
{
	int u_sensor1,a=0;
	SetSensorLowspeed(S3);
	SetSensorLowspeed(S4);
	
	OnFwd(OUT_AC,60);
	do{
               u_sensor1 = SensorUS(S3);
		Wait(100);
	}while(u_sensor1 > US_KYORI);
	
	Off(OUT_AC);
	
	SendRemoteNumber(MAILBOX2,queue,1);

       while(a != 1){
       	ReceiveRemoteNumber(MAILBOX1,true,a);
       }
	
	mt_ct(FF,10);
	
	mt_ct(FR,90);
	
	OnFwd(OUT_AC,60);
	do{
		u_sensor1 = SensorUS(S4);
		Wait(100);
	}while(u_sensor1 > US_KYORI);
	
	Off(OUT_AC);
	
	SendRemoteNumber(MAILBOX2,queue,2);
	
}

/*--------------------------------------------
		サブルーチン
---------------------------------------------*/
/* 進む距離回転する角度計算用関数 */
//第一引数:タイヤの回転方向,第2引数:進む距離 or 回転角
void mt_ct(int mt_ct,int ang_d)
{
	int r,d,g,mt;
	int iNum1=0,iNum2=0;
	
	r = T_R * PAI;	//タイヤの円周
	
	// 進む距離計算 
	if(mt_ct == FR || mt_ct == RF){
		d = T_D * PAI * ang_d / 360;
	}
	else{
		d = ang_d;
	}
	
	mt = 360 * d / r;		//進む距離からパルス数計算
	

	switch(mt_ct){
		case FF:			//前進
			OnRevSyncEx(OUT_AC,DUTY,0,RESET_ALL);
			break;
			
		case FR:			//右旋回
			OnRevSyncEx(OUT_AC,DUTY,100,RESET_ALL);
			break;
		
		case RF:			//左旋回
			OnRevSyncEx(OUT_AC,DUTY,-100,RESET_ALL);
			break;
			
		case RR:			//後進
			OnFwdSyncEx(OUT_AC,DUTY,0,RESET_ALL);
			break;
			
		case BB:			//ブレーキ
			Off(OUT_AC);
			break;
			
		default:
			Off(OUT_AC);
			break;
	}
	
	while(abs(iNum1) < mt || abs(iNum2) < mt){			//左右の パルス数が目標値になるまで待つ
		iNum1 = MotorTachoCount(OUT_A);
 		iNum2 = MotorTachoCount(OUT_C);
     	NumOut(0,LCD_LINE1,iNum1);
    	NumOut(0,LCD_LINE2,iNum2);
    }
    
    Off(OUT_AC);			//両輪停止
    
    Wait(500);				//500ms待つ
    
    ResetScreen();			//LCDクリア
	
	Wait(100);				//100ms待つ
}

スレーブ(缶つみ&持ち上げ)

task main()
{ 
	int a=0,b=0;
	while(a == 0){
		ReceiveRemoteNumber(MAILBOX1,true,a);
	}
	
	OnFwd(OUT_A,100);
	Wait(10000);
	OnFwd(OUT_B,100);
	Wait(2000);
	
	Off(OUT_AC);
 	
 	SendResponseNumber(MAILBOX2,1);
	
	while(b == 0){
		ReceiveRemoteNumber(MAILBOX1,true,b);
	}
	
	OnFwd(OUT_A,30);
	Wait(3000);
	OnFwd(OUT_B,40);
	Wait(4000);
 
}

工夫

マスター

 マスターでは,課題1の似顔絵で使用した,タコメータで車体の旋回角度や,進む距離を計算しほぼ正確に車体が動くようにした。

*void mt_ct(int mt_ct,int ang_d)の説明

この関数は課題1の似顔絵ロボットでも使用しましたが説明していなかったのでここで説明します。

定義

このように,トレッドをD[mm],タイヤの直径をS[mm]とするとタイヤの円周Tは

T= S * π [mm]

となる。また,タイヤを一回転させた時のタコメータの値がだいたい360パルスだった。

定義

このようにθだけ右に旋回させたいとき

定義

このようにタイヤの回転パルスPをだせばいい。そこでその値を出す式は

P = (360 * D * π * (θ / 360)) / T 

と出せる。あとは,回転方向を指定して,タコメータの値と計算した値が一致するまでその回転方向を続けるというのがこの関数である。

スレーブ

 スレーブは時間がなくてタコメータを使用した動作ができなかった。

感想

 今回はあまり時間がなく突貫工事でやったがあまりうまく動かすことができなかった。

 Bluetooth通信に時間をかけてしまい,つかむ動作や車体の動かす経路どうりに動かすことができることができなかった。

 ロボットを動かすのは難しいと思った。


添付ファイル: fileスライド3.JPG 198件 [詳細] fileスライド2.JPG 214件 [詳細] fileスライド1.JPG 229件 [詳細]

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2012-08-15 (水) 13:24:19