[[2005/C4/ロボコン]]

* NQCのプログラム [#u8e1028a]

- 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_NULL && 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


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