2012a/MemberOnly/進行状況A

ロボティクス入門ゼミ 2012a/A5/Liwon7/M3　

アーム部分†


最初のアームがうまくいかなかったので、シンプルにしてできるだけ軽くした。
　　　　　・旧

CIMG2288.JPG
CIMG2289.JPG
CIMG2294.JPG
CIMG2291.JPG


　　　　　・新

CIMG2298.JPG
CIMG2301.JPG
CIMG2296.JPG
↑

プログラム†

↑

フローチャート†

・缶を探すプログラム
・新

無題1.png

左回転させて、超音波センサの値が、最初に動かした時に取得した値より小さければ、右旋回させて前進させる。
・旧

無題.png

左右に回転させて、一番小さい値のところで前進させる。

↑

プログラム（新）†

/*		2012/7/27  ITY		借用 Haxa		*/
/*		2012a_robo		robokon12		*/

/*-------------------------------------------------------------------
				別名定義
-------------------------------------------------------------------*/
#define PWM 		80				// PWM(力の大きさ)
#define PWM_D 		15				// PWM(力の大きさ)
#define S_BR 		40				// 黒線のしきい値
#define S_WH	 	60				// 白線のしきい値
#define DIF 		6				// 差
#define ON 		1				// オン
#define OFF		0				// オフ
#define EN_D		40				// 円の直径
#define TY_B		150				// タイヤベース
#define TY_D		55				// タイヤの直径
#define PAI		3.14				// 円周率3.14

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

/*-------------------------------------------------------------------
				関数宣言
-------------------------------------------------------------------*/
void LCD_D();						// LCDの表示
void TRACE();						// ライントレース
void START();						// スタート
void mt_ct(int mt_ct,int ang_d);

/*-------------------------------------------------------------------
				グローバル変数
-------------------------------------------------------------------*/
int s_lowse1,s_lowse2,s_light1,s_light2;		// 変数宣言

/*-------------------------------------------------------------------
				メインルーチン
-------------------------------------------------------------------*/
task main()
{
    SetSensorLight(IN_4);				// 光センサの宣言
    SetSensorLight(IN_3);				// 光センサの宣言
    SetSensorTouch(IN_2);				// タッチセンサの宣言
    SetSensorLowspeed(IN_1);				// 超音波センサ
    int speed,diff,nairin,in;				// 変数宣言

	float p;					// フロートで宣言
	int temp;					// 一時退避
	int cnt=0,cnt_t0=0,cnt_t1=0,mode=0;		// 変数の初期化
	int temp0=50,temp1=50,cnt_t=0;			// 変数の初期化
	int time=15,pw=80;				// 時間15ms_PMW80%

	temp = SensorUS(IN_1);				// 初期の値を取得
	while(true){					// 繰り返す
		s_lowse1 = SensorUS(IN_1);		// 変数の代入
		s_lowse2 = Sensor(IN_2);		// 変数の代入
		s_light1 = Sensor(IN_3);		// 変数の代入
		s_light2 = Sensor(IN_4);		// 変数の代入

		SendRemoteNumber(MAILBOX2,true,2);	// 2を送る(掴む動作の開始)
		while(true){
			ReceiveRemoteNumber(MAILBOX3,true,in);//メッセージを受けとる
			if(in==3){			// 3が来るまでまで無限ループ
				break;			// 抜ける
			}
		SendRemoteNumber(MAILBOX2,true,1);	// 1を送る(掴む動作の停止)
  		}
//		START();				// 開始
		mt_ct(FF,100);				// 前進
		mt_ct(FF,200);				// 前進
		mt_ct(RF,93);				// 90度左旋回
		mt_ct(FF,640);				// 前進
		
		p = 1;					// pに1を代入
		while(SensorUS(IN_1) > 8){		// 超音波センサの値が8以上の時
			mt_ct(RF,(10*p+6));		// 左旋回
			if((temp+3) > SensorUS(IN_1)){	// センサが初期の値より小さい時
				p = p * 0.6;		// 回転角を小さくする
				RotateMotor(OUT_AC,pw,60);// 前進
				mt_ct(FR,(15*p));	// 右旋回
 			}
			else if(p < 1){			// pが1より小さい時(反応があった時)
				mt_ct(FR,15);		// 右旋回
				p = 1;			// pを初期化
			}
		}
		OnFwdSync(OUT_AC,80,0);			// 前進する
		while((Sensor(IN_2) == OFF)){};		// タッチセンサに反応があるまで前進
		Off(OUT_AC);				// モータの停止
		SendRemoteNumber(MAILBOX2,true,2);	// 2を送る(缶を降ろす動作)
		Wait(6000);				// 6秒間待つ
		OnRevSync(OUT_AC,100,0);		// 1秒間後退する
		Wait(1000);
		mt_ct(RF,200);				// 90度左旋回
		mt_ct(FF,600);				// 前進
		p = 1;					// pの初期化
		while(SensorUS(IN_1) > 6){		// 超音波センサの値が6以上の時
			mt_ct(RF,10);			// 左旋回
			if((temp+5) > SensorUS(IN_1)){	// センサが初期の値より小さい時
				RotateMotor(OUT_AC,pw,200);// 前進
				mt_ct(FR,(10*p*10)/10);	// 右旋回
				p = p * 0.6;		// 回転角を小さくする
			}
			else 	p = 1;			// pの　初期化
		}
		OnFwdSync(OUT_AC,80,0);			// 前進する
		while((Sensor(IN_2) == OFF)){};		// タッチセンサに反応があるまで前進
		Off(OUT_AC);				// 停止
		while(true){};				// 停止
//		ResetScreen();				// LCDの初期化
//		LCD_D();				// LCDの表示
//		TRACE();				// トレースの開始
 	}
}

/*-------------------------------------------------------------------
				サブルーチン
-------------------------------------------------------------------*/
/* 進む距離回転する角度計算用関数 */
void mt_ct(int mt_ct,int ang_d)
{
	int r,d,g,mt;
	int iNum1=0,iNum2=0;
	r = TY_D * PAI;	//タイヤの円周
	
	// 進む距離計算 
	if(mt_ct == FR || mt_ct == RF){
		d = TY_B * PAI * ang_d / 360;
	}
	else{
		d = ang_d;
	}
	mt = 360 * d / r;		//進む距離からパルス数計算
	
	switch(mt_ct){
 		case FF:			//前進
			OnFwdSyncEx(OUT_AC,PWM,0,RESET_ALL);
			break;
 		case FR:			//右旋回
			OnFwdSyncEx(OUT_AC,PWM,100,RESET_ALL);
			break;
		case RF:			//左旋回
			OnFwdSyncEx(OUT_AC,PWM,-100,RESET_ALL);
			break;
		case RR:			//後進
			OnRevSyncEx(OUT_AC,PWM,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);
       }
   
       Off(OUT_AC);				//両輪停止
       Wait(500);				//500ms待つ
}
/*-------------------------------------------------------------------
	進む距離回転する角度計算用関数　はHexaさんより借用
-------------------------------------------------------------------*/

void LCD_D(){						// LCDの表示
	while(true){
		ResetScreen();
		NumOut(10,48,SensorUS(IN_1),0);		// 超音波センサの値を表示
		NumOut(10,40,SensorUS(IN_2),0);		// 超音波センサの値を表示
		NumOut(10,32,Sensor(IN_3),0);		// 光センサの値を表示
		NumOut(10,24,Sensor(IN_4),0);		// 光センサの値を表示
		Wait(100);
	}

}

void START(){						// 後退、前進、回転を繰り返す
	int i;						// 変数宣言
		for(i=0;i<3;i++){			// 3回繰り返す
			OnRev(OUT_AC,PWM);		// 後退する
			Wait(100);			// 0.1秒待つ
			while((s_light1 > S_WH) && (s_light2 > S_WH)){
				OnFwd(OUT_AC,PWM);	// 前進
			}
			if(s_light1 < S_BR){
				Off(OUT_C);		// モータCの停止
				while(s_light2 < S_BR)OnFwd(OUT_A,PWM);
			}
			else if(s_light2 < S_BR){
				Off(OUT_A);		// モータAの停止
				while(s_light1 < S_BR)OnFwd(OUT_C,PWM);
			}
			Wait(100);			// 1秒間待つ
		}
}

void TRACE(){						// 黒線のトレース
	int nairin,mode;				// 変数宣言
	     mode=0;					// 変数の初期化

//		ResetTachoCount(OUT_AB);		// タコメータのカウント
		nairin = ((PWM*(EN_D-TY_B)*10)/(EN_D+TY_B)/10);
							// 内輪差の計算
		if((s_light1 > S_WH) && (s_light2 > S_WH)){
			OnFwd(OUT_A,nairin);		// 前進
			OnFwd(OUT_C,PWM);		// 前進
		}
		else {
			if(s_light1 < S_BR){
				OnFwd(OUT_A,nairin*0.8);// 前進
				OnFwd(OUT_C,PWM);	// 前進
				if(mode/2 == 1){
					mode++;		// モードの増加
				}
			}
			else if(s_light2 < S_BR){
				OnFwd(OUT_A,PWM);	// 前進
 				OnFwd(OUT_C,nairin*0.8);// 前進
				if(mode/2 == 0){
					mode++;
				}
			}
		}
}
↑

缶を探すプログラム（旧）†

・上手く動作しなかったプログラム

/*		2012/7/26  ITY				*/
/*		2012a_robo		robokon8	*/

while(SensorUS(IN_1) > 6){				// 超音波センサの値が6以上の時
	if(mode == 0){					// モードが0の時
		cnt++;					// カウントアップ
		if(cnt < time){				// カウントがタイムより小さい時
			if(temp0 > SensorUS(IN_1)){	// 超音波センサの値が前の値より小さい時
				temp0 = SensorUS(IN_1);	// tempにセンサの値を代入
				cnt_t0 = cnt;		// cnt_t0にcnt（時間）を代入
			}
			OnFwd(OUT_A,pw);		// モータAを正転（右回転）
			OnRev(OUT_C,pw);		// モータCを後転（右回転）
		}
		else if(cnt < time*2){			// カウントがタイムより小さい時
			OnFwd(OUT_C,pw);		// モータCを正転（左回転）
			OnRev(OUT_A,pw);		// モータAを後転（左回転）
		}
	}
	else if(mode == 1){				// モードが1の時
		cnt++;					// カウントアップ
		if(cnt < time){				// カウントがタイムより小さい時 
			if(temp1 > SensorUS(IN_1)){	// 超音波センサの値が前の値より小さい時
				temp1 = SensorUS(IN_1);	// tempにセンサの値を代入
				cnt_t1 = cnt;		// cnt_t1にcnt（時間）を代入
			}
			OnFwd(OUT_C,pw);		// モータCを正転（左回転）
			OnRev(OUT_A,pw);		// モータAを後転（左回転）
		}
		else if(cnt < time*2){			// 
			OnFwd(OUT_A,pw);		// モータAを正転（右回転）
			OnRev(OUT_C,pw);		// モータCを後転（右回転）
		}
	}
	else if(mode == 2){				// モードが2の時
		if(temp0 > temp1){			// temp0がtemp1より大きい時
			OnFwd(OUT_C,pw);		// モータCを正転（左回転）
			OnRev(OUT_A,pw);		// モータAを後転（左回転）
			cnt_t = cnt_t1;			// cnt_tに代入
		}
		else if(temp0 < temp1){			// temp1がtemp0より大きい時
			OnFwd(OUT_A,pw);		// モータAを正転（右回転）
			OnRev(OUT_C,pw);		// モータCを後転（右回転）
			cnt_t = cnt_t0;			// cnt_tに代入
		}
		if((temp0 > 20) && (temp1 > 20)){	// temp1、temp0ともに20より大きい時
			cnt_t = 0;			// cnt_tを初期化
		}
		while(cnt < cnt_t){			// cntがcnt_tより小さい時
			cnt++;				// カウントアップ
		}
		OnFwd(OUT_AC,pw);			// 0.5秒間前進
		Wait(500);				// 初期化
		mode = 0;				// 初期化
		cnt   = 0;				// 初期化
		cnt_t = 0;				// 初期化
		cnt_t0 = 0;				// 初期化
		cnt_t1 = 0;				// 初期化
		temp0 = 50;				// 初期化
		temp1 = 50;				// 初期化
	}
	if(cnt >= time*2){				// cntがtimeに2倍より大きい時
		if(mode == 0){				// モードが0の時
			mode = 1;			// モードを1にする
		}
		else if(mode == 1){			// モードが1の時
			mode = 2;			// モードを2にする
		}
		cnt = 0;				// 初期化
	}
}
↑

感想†


・なかなか思うように車体ができず、一個積み上げるのを狙いとしたロボットになってしまった。プログラムの方も詰めが甘く、車体的にタッチセンサを押したら絶対に置くことができる車体だったので、もったいなかった。
・ロボットを制御することは難しいと改めて感じ、ロボットが必ず思い通りに動いたり、絶対に安全といい切ることはできないと感じた。

添付ファイル: fileCIMG2292.JPG 335件 [詳細] fileCIMG2301.JPG 1024件 [詳細] fileCIMG2298.JPG 908件 [詳細] fileCIMG2296.JPG 940件 [詳細] fileCIMG2294.JPG 850件 [詳細] fileCIMG2291.JPG 878件 [詳細] fileCIMG2289.JPG 117件 [詳細] fileCIMG2288.JPG 874件 [詳細] file無題.png 1290件 [詳細] file無題1.png 1773件 [詳細]
トップ   編集 凍結 差分 履歴 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2012-08-11 (土) 00:48:24