2005/C4

2005/C4/ロボコン

目次

NQCのプログラム

プログラムの解説

プログラム作成の苦労した点

ソースコード

ソースコードを見るときの注意

コード : trace-cource.nqc

#define LINETRACE

#define LIGHT_L SENSOR_3
#define LIGHT_C SENSOR_2
#define LIGHT_R SENSOR_1
#define L OUT_C
#define R OUT_A

#define LIGHT_THRESHOLD 40

#if 1
#define FWD OnFwd
#define REV OnRev
#else
#define FWD OnRev
#define REV OnFwd
#endif




int pcounter=0;
int c_point1=1,c_point2=0,c_point3=0;
int place=1;  //put first position!!!!!!!!
int tmp;
int startplace;
int endplace;

#include "command-proc.h"
#include "turn.nqc"
#include "command.nqc"

task main(){
	ClearMessage();
	
	SetSensor(LIGHT_L,SENSOR_LIGHT);
	SetSensor(LIGHT_R,SENSOR_LIGHT);
	SetSensor(LIGHT_C,SENSOR_LIGHT);
	
	while(1){
		recive();
		
		if(command==COMMAND_GO){
			startplace=place;
			recive();
			endplace=command;
			
			if(startplace<endplace){
				//PlaySound(SOUND_CLICK);
				//PlaySound(SOUND_CLICK);
				while(place!=3 && place<endplace){
					//gostrait();
					linetrace();
					find_bar();
					place++;
				}
				
				if(place<endplace){
					linetrace();
					find_light();
					place++;
				}
				
				if(place<endplace){
					//godiffzone3to5();
					//SetPower(L,4);
					//SetPower(R,4);
					
					Off(L);
					FWD(R);
					
					until(LIGHT_R<LIGHT_THRESHOLD);
					
					REV(L);
					//FWD(R);
					
					until(LIGHT_R>LIGHT_THRESHOLD);
					
					Off(R);
					REV(L);
					until(LIGHT_L>LIGHT_THRESHOLD);
					
					
					//
					
					 
					REV(L);
					REV(R);
					until(LIGHT_R<LIGHT_THRESHOLD);
					 
					REV(L);
					REV(R);
					until(LIGHT_R>LIGHT_THRESHOLD);
					
					REV(L);
					REV(R);
					until(LIGHT_L<LIGHT_THRESHOLD || LIGHT_R<LIGHT_THRESHOLD);
					
					REV(L);
					REV(R);
					
					until(LIGHT_R>LIGHT_THRESHOLD);
					
					
					
					/*REV(L+R);
					Wait(15);
					
					Off(L);
					FWD(R);
					until(LIGHT_C<LIGHT_THRESHOLD);
					
					REV(L+R);
					until(LIGHT_C>LIGHT_THRESHOLD);
					
					until(LIGHT_C>LIGHT_THRESHOLD);
					
					REV(R);FWD(L);
					until(LIGHT_R>LIGHT_THRESHOLD && LIGHT_L>LIGHT_THRESHOLD);
					*/
					Off(L+R);
					
					//SetPower(L,OUT_FULL);
					//SetPower(R,OUT_FULL);
					
					place++;
				}
				if(place<endplace){
					//SetPower(L,3);
					//SetPower(R,3);
					linetrace();
					find_bar();
					//SetPower(L,OUT_FULL);
					//SetPower(R,OUT_FULL);
					place++;
				}
				
				while(place!=9 && place<endplace){
					//gostrait();
					linetrace();
					find_bar();
					place++;
				}
				
				//place--;
				
			}else if(endplace<startplace){
				//PlaySound(SOUND_CLICK);
				while(place!=5 && place>=endplace){
					//gostrait();
					linetrace();
					find_bar();
					place--;
				}
				
				if(place>=endplace){
					//SetPower(L,3);
					//SetPower(R,3);
					linetrace();
					find_light();
					
					FWD(L+R);
					until(LIGHT_L<LIGHT_THRESHOLD || LIGHT_R<LIGHT_THRESHOLD);
					Off(L+R);
					find_light();
					
					//SetPower(L,OUT_FULL);
					//SetPower(R,OUT_FULL);
					place--;
				}
				
				if(place>=endplace){
					//godiffzone5to3();
					
					//SetPower(L,4);
					//SetPower(R,4);
					
					REV(L);Off(R);
					until(LIGHT_C<LIGHT_THRESHOLD);
					
					//REV(L);Off(R);
					//until(LIGHT_C>LIGHT_THRESHOLD);
					
					
					REV(L+R);
					until(LIGHT_L<LIGHT_THRESHOLD);
					
					REV(L+R);
					until(LIGHT_L>LIGHT_THRESHOLD);
					
					FWD(L);REV(R);
					//Wait(25);
					//Wait(88);
					until(LIGHT_C>LIGHT_THRESHOLD);
					until(LIGHT_C<LIGHT_THRESHOLD);
					until(LIGHT_C>LIGHT_THRESHOLD);
					
					//REV(L+R);
					//Wait(20);
					
					//until(LIGHT_C<LIGHT_THRESHOLD);
					
					
					Off(L+R);
					
					//SetPower(L,OUT_FULL);
					//SetPower(R,OUT_FULL);
					
					place--;
				}
				if(endplace==3){
					//SetPower(L,OUT_HALF);
					//SetPower(R,OUT_HALF);
					
					linetrace();
					FWD(L+R);
					
					//until(LIGHT_L>LIGHT_THRESHOLD && LIGHT_R>LIGHT_THRESHOLD);
					//until(LIGHT_L<LIGHT_THRESHOLD && LIGHT_R<LIGHT_THRESHOLD);
					until(LIGHT_L>LIGHT_THRESHOLD && LIGHT_R>LIGHT_THRESHOLD);
					until(LIGHT_L<LIGHT_THRESHOLD && LIGHT_R<LIGHT_THRESHOLD);
					REV(L+R);
					until(LIGHT_L>LIGHT_THRESHOLD && LIGHT_R>LIGHT_THRESHOLD);
					
					//SetPower(L,OUT_FULL);
					//SetPower(R,OUT_FULL);
					
					Off(L+R);
				}
				
				while(place!=0 && place>endplace){
					//gostrait();
					linetrace();
					find_bar();
					place--;
				}
				
				//place++;
			}
			place=endplace;
			
		}else if(command==COMMAND_GET_READY){
			if(place==1){
				c_point1++;
			}else if(place==2){
				c_point2++;
			}else if(place==3){
				c_point3++;
			}
			
			turn_back=0;
			turn();
		}else if(command==COMMAND_TURN_LINE){
			turn_back=1;
			turn();
		}
	
	}
}

sub linetrace(){
	//PlaySound(SOUND_DOUBLE_BEEP);
	REV(L+R);
	
	while(true){
		if(LIGHT_R<LIGHT_THRESHOLD && LIGHT_L<LIGHT_THRESHOLD && LIGHT_C<LIGHT_THRESHOLD){
			Off(L+R);
			//SetPower(L+R,OUT_FULL);
			break;
		}
		
		
		if(LIGHT_C<LIGHT_THRESHOLD && LIGHT_L<LIGHT_THRESHOLD){
			Off(L);
			REV(R);
			
		}else if(LIGHT_C<LIGHT_THRESHOLD && LIGHT_R<LIGHT_THRESHOLD){
			Off(R);
			REV(L);
		}else{
			if(LIGHT_R<LIGHT_THRESHOLD){
				Off(R);
				////SetPower(L,OUT_HALF);
			}else{
				////SetPower(L,OUT_FULL);
				REV(R);
			}
			
			if(LIGHT_L<LIGHT_THRESHOLD){
				Off(L);
				////SetPower(R,OUT_HALF);
			}else{
				REV(L);
				////SetPower(R,OUT_FULL);
			}
			
		}
		
	}
	
}

/*sub linetrace(){
	//PlaySound(SOUND_DOUBLE_BEEP);
	REV(L+R);
	
	while(true){
		if(LIGHT_R<LIGHT_THRESHOLD && LIGHT_L<LIGHT_THRESHOLD && LIGHT_C<LIGHT_THRESHOLD){
			Off(L+R);
			//SetPower(L+R,OUT_FULL);
			break;
		}else if(LIGHT_C<LIGHT_THRESHOLD && LIGHT_L>LIGHT_THRESHOLD &&LIGHT_R>LIGHT_THRESHOLD){
			REV(L+R);
			
		}else if(LIGHT_C<LIGHT_THRESHOLD && LIGHT_L<LIGHT_THRESHOLD){
			//Float(L);
			//FWD(L);
			Off(L);
			REV(R);
			
		}else if(LIGHT_C<LIGHT_THRESHOLD && LIGHT_R<LIGHT_THRESHOLD){
			//Float(R);
			//FWD(R);
			Off(R);
			REV(L);
			
		}else if(LIGHT_C>LIGHT_THRESHOLD){
			if(LIGHT_R<LIGHT_THRESHOLD && LIGHT_L>LIGHT_THRESHOLD){
				FWD(R);
				REV(L);
				////SetPower(L,OUT_HALF);
			}else if(LIGHT_R>LIGHT_THRESHOLD && LIGHT_L<LIGHT_THRESHOLD){
				////SetPower(L,OUT_FULL);
				REV(R);
				FWD(L);
				//Off(L);
			}
			
		}else{
			//
			PlaySound(SOUND_DOWN);
		}
		
	}
	
}*/

sub find_light(){
	REV(L+R);
	until((LIGHT_R>LIGHT_THRESHOLD) && (LIGHT_L>LIGHT_THRESHOLD) && (LIGHT_C>LIGHT_THRESHOLD));
	
	Off(L+R);
	
}

sub find_bar(){
	REV(L+R);
	while((LIGHT_R<LIGHT_THRESHOLD) && (LIGHT_L<LIGHT_THRESHOLD) && (LIGHT_C<LIGHT_THRESHOLD));
	
	Off(L+R);
	
}

コード : turn.nqc

#define GO_STRAIGHT REV(OUT_A+OUT_C);
#define TURN_LEFT FWD(OUT_C);REV(OUT_A);
#define BACK_STRAIGHT FWD(OUT_A+OUT_C);
#define TURN_RIGHT FWD(OUT_A);REV(OUT_C);

#define BATT_POWER 8/12
//  8/13

int turn_back;
sub turn()
{
	int tmp=0,tmp2;
	if(place==1){
		tmp2=c_point1;
	}else if(place==2){
		tmp2=c_point2;
	}else if(place==3){
		tmp2=c_point3;
	}else{
		tmp2=0;
	}
	if(turn_back){
		PlaySound(SOUND_DOWN);
		
		BACK_STRAIGHT
		if(tmp2==1){
			tmp=5*BATT_POWER;
			
		}else if(tmp2==2){
			tmp=60*BATT_POWER;
			
		}else{
			tmp= 90*BATT_POWER;
		}
		Wait(tmp);
		
		if(place<4){
			TURN_LEFT
		}else{
			TURN_RIGHT
		}
		
		//Wait(95*BATT_POWER);
		until(LIGHT_C<LIGHT_THRESHOLD);
		
		Off(OUT_A+OUT_C);
		
	}else{
		PlaySound(SOUND_UP);
		
		int sec_first;
		int sec_second;
		
		SetPower(L,2);
		SetPower(R,2);
		
		FWD(L+R);
		until(LIGHT_L<LIGHT_THRESHOLD || LIGHT_R<LIGHT_THRESHOLD);
		Off(L+R);
		if(LIGHT_L<LIGHT_THRESHOLD){
			REV(L);
			until(LIGHT_L>LIGHT_THRESHOLD);
			Off(L);
			
		}else if(LIGHT_R<LIGHT_THRESHOLD){
			REV(R);
			until(LIGHT_R>LIGHT_THRESHOLD);
			Off(R);
			
		}
		REV(L+R);
		until(LIGHT_L>LIGHT_THRESHOLD &&LIGHT_R>LIGHT_THRESHOLD);
		Off(L+R);
		
		SetPower(L,OUT_FULL);
		SetPower(R,OUT_FULL);
		
		
		if(place==2){
			sec_first=250;
			sec_second=80;
			
		}else if(place==3){
			sec_first=250;
			sec_second=80;
			
		}else if(place==1){
			sec_first=250;
			sec_second=80;
			
		}else if(place>4){
			sec_first=239;
			sec_second=109;
			
		}
		
		//if(place==3){
			//
		//}
		
		GO_STRAIGHT
		Wait(sec_first*BATT_POWER);
		
		if(place<4){
			TURN_LEFT
		}else{
			TURN_RIGHT
		}
		Wait(140*BATT_POWER);
		
		if(tmp2==1){
			tmp=sec_second*BATT_POWER*1/16;
			
		}else if(tmp2==2){
			tmp=sec_second*BATT_POWER*7/10;
			
		}else{
			tmp=sec_second*BATT_POWER;
		}
		GO_STRAIGHT
		Wait(tmp);
		Off (OUT_A+OUT_C);
	}
	
}

コード : cacher.nqc (変なファイル名)

/*#define GREEN 44
#define BLACK 30
#define WHITE 50
*/
#include "command.nqc"
#include "command-proc.h"

#define LIGHT SENSOR_1
		
#define ARM OUT_A
#define HAND OUT_B


task main()
{
	ClearMessage();
	
	SetSensor(LIGHT,SENSOR_LIGHT);
	
	Wait(10);
	
	//SendMessage(COMMAND_TURN_LINE);
	//command=COMMAND_TURN_LINE;
	//send();
	
	int i,dest,src,tmp;
	int c_place1=0,c_place2=0,c_place3=0;
	
	src=1;/////
	
	for(i=0;i<9;i++){
		
		Wait(450);
		
		if(src==1){
			c_place1++;
			tmp=c_place1;
		}else if(src==2){
			c_place2++;
			tmp=c_place2;
		}else{
			c_place3++;
			tmp=c_place3;
		}
		
		//tsukamu();
		if(src==1 || src ==3){
			//SetPower(ARM,OUT_FULL);
			OnFwd(ARM) ;
			Wait(30); 
			OnRev(HAND);
			Wait(150);
			SetPower(ARM,OUT_FULL);
			OnRev(ARM);
			Wait(90);
			//SetPower(HAND,OUT_FULL);
			Off(ARM);
			//Off(HAND);
		}else{
			OnFwd(ARM) ;
			//SetPower(ARM,OUT_FULL);
			Wait(50);
			Off(ARM);
			//SetPower(HAND,OUT_FULL);
			OnRev(HAND);
			Wait(150);
			SetPower(ARM,OUT_FULL); 
			OnRev(ARM);
			Wait(90);
			Off(ARM);
			//Off(HAND);
		}
		//
		/*
		Wait(40);
		OnRev(HAND);
		Wait(80);
		OnRev(ARM);
		//SetPower(ARM,OUT_LOW); 
		Wait(130);
		SetPower(HAND,OUT_HALF);
		
		OnRev(ARM);    
		SetPower(ARM,OUT_FULL);
		
		Wait(50);
		
		Off(ARM);*/
		//Off(B);
		//
		
		if(LIGHT>48){
			//white
			dest=7;
			
		}else if(LIGHT>39){
			//green
			dest=6;
		}else{
			//brack
			dest=8;
		}
		
		PlaySound(SOUND_LOW_BEEP);
		
		command=COMMAND_TURN_LINE;
		send();
		
		command=COMMAND_GO;
		send();
		
		command=dest;
		send();
		
		
		command=COMMAND_GET_READY;
		send();
		
		Wait(450);
		//hanasu();
		OnFwd(ARM) ;
		SetPower(ARM,OUT_HALF);
		Wait(30);
		OnRev(ARM);
		SetPower(ARM,OUT_LOW); 
		OnFwd(HAND);
		SetPower(HAND,OUT_FULL);
		Wait(50);
		Off(HAND); 
		OnRev(ARM);    
		SetPower(ARM,OUT_FULL);
		Wait(40);
		Off(ARM);
		//
		
		command=COMMAND_TURN_LINE;
		send();
		PlaySound(SOUND_CLICK);
		
		if(i==3-1){
			src++;
			PlaySound(SOUND_FAST_UP);
		}else if(i==6-1){
			src++;
			PlaySound(SOUND_FAST_UP);
		}
		command=COMMAND_GO;
		send();
		
		command=src;
		send();
		
		command=COMMAND_GET_READY;
		send();
		
	}

}

コード : command-proc.h

#define COMMAND_GO 110
#define COMMAND_GET_READY 111
#define COMMAND_TURN_LINE 112

コード : command.h

#define COMMAND_START 201
#define COMMAND_END 202
#define COMMAND_RECIVED 203
#define COMMAND_READY 204
#define COMMAND_CONTENT 205
#define COMMAND_LISNING 206
#define COMMAND_NULL 255

コード : command.nqc

#include "command.h"

int command=0;

#ifdef LINETRACE

sub recive(){
	int count,tmp;
	
	until(Message()==COMMAND_START || Message()==COMMAND_CONTENT);
	
	until(Message()==COMMAND_CONTENT){
		Wait(10);
		SendMessage(COMMAND_LISNING);
	}
	
	while(1){
		Wait(2);
		//PlaySound(SOUND_CLICK);
		
		tmp=Message();
		if(tmp==COMMAND_CONTENT){
			tmp=COMMAND_NULL;
			until(tmp!=COMMAND_CONTENT){
				tmp=Message();
			}
			command=tmp;
			Wait(3);
			SendMessage(command);
			Wait(13);
			
		}else if(tmp==COMMAND_END){
			break;
		}
	}
	Wait(20);
	
	until(Message()==COMMAND_END){
		SendMessage(COMMAND_RECIVED);
		Wait(7);
	}
	
	Wait(10);
}

#else
sub send(){
	int i;
	
	until(Message()==COMMAND_READY || Message()==COMMAND_LISNING){
		SendMessage(COMMAND_START);
		Wait(15);
	}
	
	until(Message()==command){
		Wait(23);
		SendMessage(COMMAND_CONTENT);
		Wait(5);
		SendMessage(command);
	}
	Wait(10);
	
	for(i=0;i<15;i++){
		SendMessage(COMMAND_END);
		Wait(5);
	}
}

#endif

コメント欄

どうぞ。



添付ファイル: filecource-number.png 468件 [詳細] filerobots-prog-chart-img.png 487件 [詳細] filerobots-prog-chart.odg 651件 [詳細] filerobots-ir-connect-chart-img.png 476件 [詳細] filerobots-ir-connect-chart.odg 560件 [詳細]

トップ   編集 凍結 差分 履歴 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2006-05-15 (月) 18:06:01