[[2012a/MemberOnly/進行状況A]]

[[ロボティクス入門ゼミ]]  
[[ロボティクス入門ゼミ]]
[[2012a/A5/Liwon7/M3]] 
#contents
*プログラム [#vde8e72f]
 /*		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++;
 				}
 			}
 		}
 }

**以前の缶を探すプログラム [#y922efc4]
 /*		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;				// 初期化
 	}
 }



トップ   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS