- 追加された行はこの色です。
- 削除された行はこの色です。
[[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