2004/C3/ロボコンまでのアイデアの変遷と製作過程

//ロボコン用プログラム

//白44 緑39 黒32
//////////////////値の定義//////////////////////

#define wht_con 37
#define gre_con 30 //白色と緑色の光量の差
#define blk_con 27 //白色と黒色の光量の差

#define pai_2 1 //90度回転時間
#define pai_ 2  //180度回転時間
#define pai_32 162 //270度回転時間
#define pai2_ 212  //360度回転時間

#define wht_box 53 //無搭載時と白箱搭載時の光量の差
#define gre_box 45 //同緑
#define blk_box 36 //同黒

#define RUN_TIME 150
#define RAISE_TIME 1
#define PUSH_TIME 4


/////////////////変数の定義/////////////////////

int cur_light; //現在の光量
int cur_box; //箱がないときの光量
int gre_light; //緑の光量
int blk_light;  //黒の光量



/////////////////関数の定義/////////////////////


void green_trace()  //緑のラインをトレースする
{       
        if(SENSOR_1 <= gre_con +2 && SENSOR_1 >=gre_con -2){
  OnFwd(OUT_A);Wait(2);
  Off(OUT_C);
 }else{
  Off(OUT_A);
  OnFwd(OUT_C); Wait(2);
 }
   
}

void black_trace()  //黒のラインをトレースする
{     
 if(SENSOR_1 <= blk_con +2){
  OnFwd(OUT_A);Wait(2); 
  Off(OUT_C); 
 }else{
  Off(OUT_A); 
  OnFwd(OUT_C);Wait(2);
   
 } 
}

void left()
{
 Off(OUT_C); OnFwd(OUT_A); Wait(2); off(); Wait(1);//右に旋回
}

void Rleft()
{
 Off(OUT_C); OnRev(OUT_A); Wait(50); off(); Wait(1);//右に旋回
}

void right()
{
 Off(OUT_A); OnFwd(OUT_C); Wait(2); off(); Wait(1);//左に旋回
}

void Rright()
{
 Off(OUT_A); OnRev(OUT_C); Wait(150); off(); Wait(5);//左に旋回
}

void Hright()
{
 OnFwd(OUT_A); OnRev(OUT_C); Wait(15); off(); Wait(5); //右に急旋回
}

void Hleft()
{
OnFwd(OUT_C); OnRev(OUT_A); Wait(15); off(); Wait(5); //左に急旋回
}

void go()
{
 OnFwd(OUT_A+OUT_C); Wait(6); Off(OUT_A+OUT_C);  Wait(3);  //前進
}

void back()
{
 OnRev(OUT_A+OUT_C); Wait(40); Off(OUT_A+OUT_C); Wait(3); //後進
}

void off()
{
 Off(OUT_A+OUT_C); //停止
}

void turn()
{
 while(SENSOR_2 > wht_con -3 || SENSOR_3 > wht_con -3){
 if(SENSOR_2 > wht_con -3){OnFwd(OUT_A);} else {Off(OUT_A);}
 if(SENSOR_3 > wht_con -3){OnFwd(OUT_C);} else {Off(OUT_C);}
}
}

/////////////////////メインプログラム//////////////////////////

task main()
{
 SetSensor(SENSOR_2,SENSOR_LIGHT); //左ライントレース用光センサーの定義
 SetSensor(SENSOR_3,SENSOR_LIGHT); //右ライントレース用光センサーの定義
 SetSensor(SENSOR_1,SENSOR_LIGHT); //箱の色識別用光センサーの定義

 SetPower(OUT_C,7);
 //降段動作
 repeat(4){ back();} Wait(100);cur_light = SENSOR_2;
 PlaySound(SOUND_DOUBLE_BEEP);

 //緑線まで右回転
 while(SENSOR_2 > cur_light -3){ right();}
 
 PlaySound(SOUND_DOUBLE_BEEP);

 while(true){
 ClearTimer(0);
 while(Timer(0) < 1000){
 if(SENSOR_2 > cur_light -3){ go(); }
 else{ go(); }}

 repeat(20){ right(); }
 
 repeat(10){ go(); }
 ClearTimer(1);
 while(Timer(1) < 3500){
 go(); OnFwd(OUT_B); go(); Off(OUT_B);}

 repeat(5){ back();}

 while(SENSOR_2 > cur_light -3){ right();}
 ClearTimer(3);
 while(Timer(3) < 300){
 if(SENSOR_2 > cur_light -3){ left(); }
 else{ right(); }
 
}
}
}

トップ   編集 凍結 差分 バックアップ 添付 複製 名前変更 リロード   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS
Last-modified: 2005-03-01 (火) 02:59:55