2013a/Member/math5/Mission2

2013a/Member

ロボット†

プログラム†

#define BLACK_01 27
#define BLACK_02 35
#define WHITE_01 50
#define WHITE_02 60
#define SPEED 30
#define go_forward OnFwd(OUT_AC,SPEED);
#define turn_right OnRev(OUT_A,SPEED);OnFwd(OUT_C,SPEED);
#define turn_left OnFwd(OUT_A,SPEED);OnRev(OUT_C,SPEED);
#define right Off(OUT_A);OnFwd(OUT_C,SPEED);
#define left OnFwd(OUT_A,SPEED);Off(OUT_C);
#define STEP 1
#define CENTER 45
#define CCC 14000
#define SPEED_H 45
#define SPEED_L 25
#define turn_R Off(OUT_A); OnFwd(OUT_C,SPEED_L);
#define turn_L Off(OUT_C); OnFwd(OUT_A,SPEED_L);
#define turn_SR OnRev(OUT_A,SPEED_L); OnFwd(OUT_C,SPEED_L);
#define turn_SL OnRev(OUT_C,SPEED_L); OnFwd(OUT_A,SPEED_L);
#define go_F OnFwd(OUT_AC,SPEED_H);
#define nMAX 250
#define short_break Off(OUT_AC);Wait(400);
#define CROSS_TIME 400
#define cross_line OnFwd(OUT_A,SPEED_L);OnFwd(OUT_C,SPEED_L);Wait(CROSS_TIME);short_break;
#define cross OnFwd(OUT_C,SPEED_L);OnFwd(OUT_A,SPEED_L);Wait(CROSS_TIME+200);short_break;
#define go_b OnFwd(OUT_A,-40);OnFwd(OUT_C,-40);
#define catch OnFwd(OUT_B,33);Wait(600);Off(OUT_B);
#define throw OnRev(OUT_B,80);Wait(1000);Off(OUT_B);

sub right_n()
{	
	if (SENSOR_1<BLACK_01) {turn_left;}
	else if (SENSOR_1<BLACK_02) {left;}
	else if (SENSOR_1<WHITE_01) {go_forward;}
	else if (SENSOR_1<WHITE_02) {right;}
	else {turn_right;}Wait(STEP);
}

sub trace_R()
{
	int n;
	while(true) {
		while(n < nMAX) {
			if(SENSOR_1 < CENTER-12) {
				turn_SL;
				n++;
			} else {
				if(SENSOR_1 < CENTER-6) {
					turn_L;
				} else if(SENSOR_1 < CENTER+6) {
					go_F;
				} else if(SENSOR_1 < CENTER+12) {
		turn_SL			turn_R;
				} else {
					turn_SR;
				}
				n=0;
			}
			Wait(STEP);
		}
		short_break;
		turn_SR;Wait(nMAX*STEP*2);
		cross;
		n=0;
		break;
	}
}

sub trace_L()
{
	int n = 0;
	while(true) {
		while(n < nMAX) {
			if(SENSOR_1 < CENTER-12) {
				turn_SR;
				n++;
			} else {
				if(SENSOR_1 < CENTER-6) {
					turn_R;
				} else if(SENSOR_1 < CENTER+6) {
					go_F;
				} else if(SENSOR_1 < CENTER+12) {
					turn_L;
				} else {
					turn_SL;
				}
				n=0;
			}
			Wait(STEP);
		}
		short_break;
		turn_SL;Wait(nMAX*STEP);
		cross;
		n=0;
		break;
	}
}

sub left_n()
{
	if (SENSOR_1<BLACK_01) {
		turn_right;
	}
	else if (SENSOR_1<BLACK_02) {
		right;
	}
	else if (SENSOR_1<WHITE_01) {
		go_forward;
	}
	else if (SENSOR_1<WHITE_02) {
		left;
	}
	else {
		turn_left;
	}
	Wait(STEP);
}

task main()
{
	SetSensorLight(S1);
	SetSensorLight(S2);
	long timess  = 0;
	/////////////////////////////start///////////////////////////////////
	trace_L();
	Wait(1000);
	go_b;
	Wait(950);
	turn_SL;
	Wait(800);
	PlaySound(SOUND_CLICK);//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>ブザー１
	go_F;
	Wait(550);
	go_F;
	catch;
	while(SENSOR_1 >  CENTER+8 )
	{
		Wait(STEP);
	}
	PlaySound(SOUND_CLICK);//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>ブザー２
	Off(OUT_AC);
	/////////////////////////////ボールキャッチ後///////////////////////////////
	go_F;
	Wait(90);
	turn_SR;
	Wait(750);
	PlaySound(SOUND_CLICK);//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>ブザー３
	/////////////////////////////////////////////////////////////////////////////////
	Wait(1000);
	trace_R();
	turn_SR;
	Wait(700);
	trace_L();//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>左側トレース
	Wait(1000);
	PlaySound(SOUND_CLICK);
	PlaySound(SOUND_CLICK);
	//////////////////////////////二回目の交差点終了////////////////////////////////
	//////////////////////////////////三回目の交差点認識/////////////////////////
	go_b;
	Wait(800);
	Off(OUT_AC);
	Wait(100);
	RotateMotor(OUT_A,-70,410);
	throw;
	RotateMotor(OUT_A,70,470);
	go_F;
	Wait(200);
	///////////////////////////////////一回目の発射/////////////////////////////////(向き直っている)
	timess  =  CurrentTick();
	while(CurrentTick() - timess < 4000) {
		 left_n();
	}
	Off(OUT_AC);
	Wait(2000);
	RotateMotor(OUT_A,-70,440);
	Off(OUT_AC);
	Wait(10000);
	catch;
	catch;
	RotateMotor(OUT_A,70,440);
	go_F;
	Wait(1700);
	timess  =  CurrentTick();
	Off(OUT_AC);
	RotateMotor(OUT_A,-70,460);
	Wait(2000);
	throw;	
}

ロボット†

プログラム†

目次

全般

教材・資料

Debian関連

過去の授業

2003年度以前

最新の20件