2004/C1/ロボコン/source
をテンプレートにして作成
[
トップ
] [
新規
|
一覧
|
検索
|
最終更新
|
ヘルプ
|
ログイン
]
開始行:
[[2004/C1/ロボコン]]
&size(25){最終版プログラム};
-[[タスク,関数の依存関係を表示...するつもりで書きました。>#task_list]]
-[[mainタスク>#main]]
-[[boot(初動の定義)>#boot]]
-[[pick(探索タスク)>#pick]]
-[[サブルーチン>#sub]]
-[[lift(持ち上げるタスク)>#lift]]
&aname(top);
#define RIGHT 1
#define LEFT -1
#define Forward OnFwd(OUT_A+OUT_C);
#define Back OnRev(OUT_A+OUT_C);
//=================LIFT======================================
#define HOME_HEIGHT FastTimer(3) > 500
#define HOME (SENSOR_1 < 60)
#define BOX ( ((730 < SENSOR_3) && (SENSOR_3 < 745)) || ((770 < SENSOR_3) && (SENSOR_3 < 800)) )
//#define Open OnFwd(OUT_B);Wait(200);
#define Close OnRev(OUT_B);Wait(200);
//730 745 white 770 800 green 820 840 normal
//============================================
//================LINE_TRACE======================================================================
#define bGREEN ((720 < SENSOR_1) && (SENSOR_1 < 780 )) && ((720 < SENSOR_2) && (SENSOR_2 < 780 ))
#define wGREEN ((720 < SENSOR_1) && (SENSOR_1 < 780 )) || ((720 < SENSOR_2) && (SENSOR_2 < 780 ))
#define lGREEN ((720 < SENSOR_1) && (SENSOR_1 < 780 ))
#define rGREEN ((720 < SENSOR_2) && (SENSOR_2 < 780 ))
#define bBLACK ( 780 < SENSOR_1) && ( 780 < SENSOR_2)
#define wBLACK ( 780 < SENSOR_1) || ( 780 < SENSOR_2)
#define lBLACK ( 780 < SENSOR_1)
#define rBLACK ( 780 < SENSOR_2)
#define bWHITE ( 720 > SENSOR_1 ) && ( 720 > SENSOR_2 )
#define wWHITE ( 720 > SENSOR_1 ) || ( 720 > SENSOR_2 )
#define lWHITE ( 720 > SENSOR_1 )
#define rWHITE ( 720 > SENSOR_2 )
//=================================================================================================
&aname(task_list);
[[top>#top]] //================task_function_sub_LIST=============================
/*
//task music();
task main();
|_task Lift();
|_task PickUp();
|_task Grab();
|_task Zenshin();
void Tsukamu(void);
void Boot(void);
sub Forward();
void PutBox(void);
*/
//=============================================================
int bring = 0;
int swch = 1;//course
&aname(main);
[[top>#top]]
task main(){
SetSensor(SENSOR_1, SENSOR_LIGHT);//left-line_trace & home
SetSensor(SENSOR_2, SENSOR_LIGHT);//right-line_trace
SetSensor(SENSOR_3, SENSOR_LIGHT);//box-color_trace
SetSensorMode(SENSOR_1, SENSOR_MODE_RAW);//
SetSensorMode(SENSOR_2, SENSOR_MODE_RAW);//
SetSensorMode(SENSOR_3, SENSOR_MODE_RAW);//0 ~ 1023
//Boot();
PlaySound(SOUND_DOUBLE_BEEP);
start PickUp;
start Lift;
}
&aname(boot);
[[top>#top]]
void Boot(void){
Forward;
Wait(200);
}
&aname(pick);
[[top>#top]]
//int swch = 0;
task PickUp()
{
while(true){
if(swch == 1){
while(true){
Forward;
until(wGREEN || BOX);
if(rGREEN){
Forward;
until(lGREEN);
mawaruLEFT();
until(lWHITE);
Forward;
until(rWHITE);
mawaruLEFT();
until(bWHITE);
swch++; break;
}else if(lGREEN){
Forward;
until(lWHITE && rGREEN);
mawaruLEFT();
until(lGREEN);
mawaruLEFT();
until(bWHITE);
swch++; break;
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}
}//while
}else if(swch == 2){
while(true){
Forward;
until(wGREEN || wBLACK || BOX);
if(rGREEN){
mawaruRIGHT();
until(bWHITE);
}else if(lGREEN){
mawaruLEFT();
until(bWHITE);
}else if(rBLACK){
mawaruRIGHT();
until(lBLACK && rWHITE);
Forward;
until(lWHITE && rBLACK);
mawaruRIGHT();
until(bWHITE);
swch++; break;
}else if(lBLACK){
mawaruRIGHT();
until(lBLACK && rWHITE);
Forward;
until(lWHITE && rBLACK);
mawaruRIGHT();
until(bWHITE);
swch++; break;
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}
}//while
}else if(swch == 3){
while(true){
Forward;
until(wBLACK || rGREEN || BOX);
if(rBLACK){
mawaruRIGHT();
until(bWHITE);
}else if(lBLACK){
mawaruLEFT();
until(bWHITE);
}else if(rGREEN){
Forward;
until(bWHITE);
swch++; break;
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}
}//while
}else if((4 <= swch) && (swch <= 6)){
while(true){
Forward;
until(lBLACK || wGREEN);
if(lBLACK){
mawaruRIGHT();
until(lBLACK && rWHITE);
Forward;
until(lWHITE);
mawaruRIGHT();
until(bWHITE);
swch++; break;
}else if(lGREEN){
mawaruLEFT();
until(lWHITE);
}else if(rGREEN){
mawaruRIGHT();
until(bWHITE || lBLACK);
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}
}//while
}else if(swch == 7){
while(true){
Forward;
until(lBLACK || wGREEN);
if(lBLACK){
Forward;
until(lWHITE);
mawaruLEFT();
until(lBLACK);
mawaruLEFT();
until(bWHITE);
swch++; break;
}else if(lGREEN){
mawaruLEFT();
until(lWHITE);
}else if(rGREEN){
mawaruRIGHT();
until(bWHITE || lBLACK);
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}
}//while
}else if(swch == 8){
while(true){
Forward;
until(wBLACK || lGREEN);
if(lBLACK){
mawaruLEFT();
until(lWHITE);
}else if(rBLACK){
mawaruRIGHT();
until(bWHITE);
}else if(lGREEN){
Forward;
until(lWHITE);
mawaruLEFT();
until(lGREEN);
mawaruLEFT();
until(bWHITE);
swch++; break;
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}
}//while
}else if(swch == 9){
ClearTimer(1);
while(FastTimer(1) < 700){
Forward;
until(wGREEN || BOX);
if(lGREEN){
mawaruLEFT();
until(lWHITE);
}else if(rGREEN){
mawaruRIGHT();
until(bWHITE);
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}//else
}//while
swch++;
}else if(swch == 10){
mawaruRIGHT();
until(wGREEN);
if(rGREEN){
mawaruRIGHT();
until(lGREEN);
Forward;
until(bWHITE);
mawaruRIGHT();
Wait(30);
}else if(lGREEN){
mawaruRIGHT();
until(rGREEN);
}
Forward;
until(HOME);
swch = 1;
}else{
PlaySound(SOUND_DOUBLE_BEEP);
}
}//while
}//pickup
&aname(sub);
[[top>#top]]
sub mawaruRIGHT()
{
OnFwd(OUT_A);
OnRev(OUT_C);
}
sub mawaruLEFT()
{
OnFwd(OUT_C);
OnRev(OUT_A);
}
&aname(lift);
[[top>#top]]
//=======================lift====================
int z = 1;//z == 0 --> Zenshin end
task Zenshin(){
Forward;
Wait(300);
Off(OUT_A+OUT_C);
z = 0;
stop Zenshin;
}
task Open_Close(){
while(true){
if(z == 1){
OnFwd(OUT_B);
}else if(z == 0){
OnRev(OUT_B);
}
}
}
task Grab(){
OnFwd(OUT_B); Wait(100);
start Zenshin;
start Open_Close;
Back; Wait(300);
bring = 1;
start PickUp;
stop Zenshin;
stop Open_Close;
stop Grab;
}
task Lift(){
while(true){
if(bring == true){
ClearTimer(3);
OnFwd(OUT_B);
until(HOME_HEIGHT);
Off(OUT_B);
break;
}
}
while(true){
if(HOME){
OnFwd(OUT_B); Wait(500);
}
}
}
[[top>#top]]
終了行:
[[2004/C1/ロボコン]]
&size(25){最終版プログラム};
-[[タスク,関数の依存関係を表示...するつもりで書きました。>#task_list]]
-[[mainタスク>#main]]
-[[boot(初動の定義)>#boot]]
-[[pick(探索タスク)>#pick]]
-[[サブルーチン>#sub]]
-[[lift(持ち上げるタスク)>#lift]]
&aname(top);
#define RIGHT 1
#define LEFT -1
#define Forward OnFwd(OUT_A+OUT_C);
#define Back OnRev(OUT_A+OUT_C);
//=================LIFT======================================
#define HOME_HEIGHT FastTimer(3) > 500
#define HOME (SENSOR_1 < 60)
#define BOX ( ((730 < SENSOR_3) && (SENSOR_3 < 745)) || ((770 < SENSOR_3) && (SENSOR_3 < 800)) )
//#define Open OnFwd(OUT_B);Wait(200);
#define Close OnRev(OUT_B);Wait(200);
//730 745 white 770 800 green 820 840 normal
//============================================
//================LINE_TRACE======================================================================
#define bGREEN ((720 < SENSOR_1) && (SENSOR_1 < 780 )) && ((720 < SENSOR_2) && (SENSOR_2 < 780 ))
#define wGREEN ((720 < SENSOR_1) && (SENSOR_1 < 780 )) || ((720 < SENSOR_2) && (SENSOR_2 < 780 ))
#define lGREEN ((720 < SENSOR_1) && (SENSOR_1 < 780 ))
#define rGREEN ((720 < SENSOR_2) && (SENSOR_2 < 780 ))
#define bBLACK ( 780 < SENSOR_1) && ( 780 < SENSOR_2)
#define wBLACK ( 780 < SENSOR_1) || ( 780 < SENSOR_2)
#define lBLACK ( 780 < SENSOR_1)
#define rBLACK ( 780 < SENSOR_2)
#define bWHITE ( 720 > SENSOR_1 ) && ( 720 > SENSOR_2 )
#define wWHITE ( 720 > SENSOR_1 ) || ( 720 > SENSOR_2 )
#define lWHITE ( 720 > SENSOR_1 )
#define rWHITE ( 720 > SENSOR_2 )
//=================================================================================================
&aname(task_list);
[[top>#top]] //================task_function_sub_LIST=============================
/*
//task music();
task main();
|_task Lift();
|_task PickUp();
|_task Grab();
|_task Zenshin();
void Tsukamu(void);
void Boot(void);
sub Forward();
void PutBox(void);
*/
//=============================================================
int bring = 0;
int swch = 1;//course
&aname(main);
[[top>#top]]
task main(){
SetSensor(SENSOR_1, SENSOR_LIGHT);//left-line_trace & home
SetSensor(SENSOR_2, SENSOR_LIGHT);//right-line_trace
SetSensor(SENSOR_3, SENSOR_LIGHT);//box-color_trace
SetSensorMode(SENSOR_1, SENSOR_MODE_RAW);//
SetSensorMode(SENSOR_2, SENSOR_MODE_RAW);//
SetSensorMode(SENSOR_3, SENSOR_MODE_RAW);//0 ~ 1023
//Boot();
PlaySound(SOUND_DOUBLE_BEEP);
start PickUp;
start Lift;
}
&aname(boot);
[[top>#top]]
void Boot(void){
Forward;
Wait(200);
}
&aname(pick);
[[top>#top]]
//int swch = 0;
task PickUp()
{
while(true){
if(swch == 1){
while(true){
Forward;
until(wGREEN || BOX);
if(rGREEN){
Forward;
until(lGREEN);
mawaruLEFT();
until(lWHITE);
Forward;
until(rWHITE);
mawaruLEFT();
until(bWHITE);
swch++; break;
}else if(lGREEN){
Forward;
until(lWHITE && rGREEN);
mawaruLEFT();
until(lGREEN);
mawaruLEFT();
until(bWHITE);
swch++; break;
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}
}//while
}else if(swch == 2){
while(true){
Forward;
until(wGREEN || wBLACK || BOX);
if(rGREEN){
mawaruRIGHT();
until(bWHITE);
}else if(lGREEN){
mawaruLEFT();
until(bWHITE);
}else if(rBLACK){
mawaruRIGHT();
until(lBLACK && rWHITE);
Forward;
until(lWHITE && rBLACK);
mawaruRIGHT();
until(bWHITE);
swch++; break;
}else if(lBLACK){
mawaruRIGHT();
until(lBLACK && rWHITE);
Forward;
until(lWHITE && rBLACK);
mawaruRIGHT();
until(bWHITE);
swch++; break;
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}
}//while
}else if(swch == 3){
while(true){
Forward;
until(wBLACK || rGREEN || BOX);
if(rBLACK){
mawaruRIGHT();
until(bWHITE);
}else if(lBLACK){
mawaruLEFT();
until(bWHITE);
}else if(rGREEN){
Forward;
until(bWHITE);
swch++; break;
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}
}//while
}else if((4 <= swch) && (swch <= 6)){
while(true){
Forward;
until(lBLACK || wGREEN);
if(lBLACK){
mawaruRIGHT();
until(lBLACK && rWHITE);
Forward;
until(lWHITE);
mawaruRIGHT();
until(bWHITE);
swch++; break;
}else if(lGREEN){
mawaruLEFT();
until(lWHITE);
}else if(rGREEN){
mawaruRIGHT();
until(bWHITE || lBLACK);
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}
}//while
}else if(swch == 7){
while(true){
Forward;
until(lBLACK || wGREEN);
if(lBLACK){
Forward;
until(lWHITE);
mawaruLEFT();
until(lBLACK);
mawaruLEFT();
until(bWHITE);
swch++; break;
}else if(lGREEN){
mawaruLEFT();
until(lWHITE);
}else if(rGREEN){
mawaruRIGHT();
until(bWHITE || lBLACK);
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}
}//while
}else if(swch == 8){
while(true){
Forward;
until(wBLACK || lGREEN);
if(lBLACK){
mawaruLEFT();
until(lWHITE);
}else if(rBLACK){
mawaruRIGHT();
until(bWHITE);
}else if(lGREEN){
Forward;
until(lWHITE);
mawaruLEFT();
until(lGREEN);
mawaruLEFT();
until(bWHITE);
swch++; break;
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}
}//while
}else if(swch == 9){
ClearTimer(1);
while(FastTimer(1) < 700){
Forward;
until(wGREEN || BOX);
if(lGREEN){
mawaruLEFT();
until(lWHITE);
}else if(rGREEN){
mawaruRIGHT();
until(bWHITE);
}else if(BOX){
start Grab;
stop PickUp;
}else{
PlaySound(SOUND_DOUBLE_BEEP);//Error
}//else
}//while
swch++;
}else if(swch == 10){
mawaruRIGHT();
until(wGREEN);
if(rGREEN){
mawaruRIGHT();
until(lGREEN);
Forward;
until(bWHITE);
mawaruRIGHT();
Wait(30);
}else if(lGREEN){
mawaruRIGHT();
until(rGREEN);
}
Forward;
until(HOME);
swch = 1;
}else{
PlaySound(SOUND_DOUBLE_BEEP);
}
}//while
}//pickup
&aname(sub);
[[top>#top]]
sub mawaruRIGHT()
{
OnFwd(OUT_A);
OnRev(OUT_C);
}
sub mawaruLEFT()
{
OnFwd(OUT_C);
OnRev(OUT_A);
}
&aname(lift);
[[top>#top]]
//=======================lift====================
int z = 1;//z == 0 --> Zenshin end
task Zenshin(){
Forward;
Wait(300);
Off(OUT_A+OUT_C);
z = 0;
stop Zenshin;
}
task Open_Close(){
while(true){
if(z == 1){
OnFwd(OUT_B);
}else if(z == 0){
OnRev(OUT_B);
}
}
}
task Grab(){
OnFwd(OUT_B); Wait(100);
start Zenshin;
start Open_Close;
Back; Wait(300);
bring = 1;
start PickUp;
stop Zenshin;
stop Open_Close;
stop Grab;
}
task Lift(){
while(true){
if(bring == true){
ClearTimer(3);
OnFwd(OUT_B);
until(HOME_HEIGHT);
Off(OUT_B);
break;
}
}
while(true){
if(HOME){
OnFwd(OUT_B); Wait(500);
}
}
}
[[top>#top]]
ページ名: