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(); }
}
}
}