- 追加された行はこの色です。
- 削除された行はこの色です。
#define THRESHOLD 46
#define go(t) OnRev(OUT_AC);Wait(t);Off(OUT_AC); //t秒前進
#define back(t) OnFwd(OUT_AC);Wait(t);Off(OUT_AC); //t秒後進
#define roll_left OnFwd(OUT_C);OnRev(OUT_A); //左旋回
#define turn_left OnRev(OUT_A);Off(OUT_C); //左折
#define roll_right OnFwd(OUT_A);OnRev(OUT_C); //右旋回
#define turn_right OnRev(OUT_C);Off(OUT_A); //右折
#define roll_left2(t) OnFwd(OUT_C);OnRev(OUT_A);Wait(t); //t秒左旋回
#define turn_left2(t) OnRev(OUT_A);Off(OUT_C);Wait(t); //t秒左折
#define roll_right2(t) OnFwd(OUT_A);OnRev(OUT_C);Wait(t); //t秒右旋回
#define turn_right2(t) OnRev(OUT_C);Off(OUT_A);Wait(t); //t秒右折
#define roll_left3 OnRev(OUT_A);OnFwd(OUT_C);until(SENSOR_2<=43);Off(OUT_AC); //ラインを見つけるまで左折
#define roll_right3 OnRev(OUT_C);OnFwd(OUT_A);until(SENSOR_2<=43);Off(OUT_AC); //ラインを見つけるまで右折
#define close SetPower(OUT_B,1);OnFwd(OUT_B);Wait(50);Off(OUT_B);//アームを閉じる
#define open SetPower(OUT_B,1);OnRev(OUT_B);Wait(70);Off(OUT_B);//アームを開ける
#define cross_stop Off(OUT_AC);Wait(100); //交差点で1秒停止
#define STEP 2
sub LINE_R() //線の右側をトレース
{
SetSensor(SENSOR_2,SENSOR_LIGHT);
ClearTimer(0);
while(FastTimer(0)<=23)//0.23秒以内の場合はループ
{
if (SENSOR_2 > THRESHOLD +5) {
roll_left;
ClearTimer(0);
}else if (SENSOR_2 > THRESHOLD +3){
turn_left;
ClearTimer(0);
}else if (SENSOR_2 > THRESHOLD){
OnRev(OUT_AC);
ClearTimer(0);
}else if (SENSOR_2 > THRESHOLD -3){
turn_right;
ClearTimer(0);
}else{
roll_right;
}
Wait(STEP);
}
Off(OUT_AC);
}
sub LINE_L() //線の左側をトレース
{
SetSensor(SENSOR_2,SENSOR_LIGHT);
ClearTimer(0);
while(FastTimer(0)<=21)//0.20秒以内の場合はループ
{
if (SENSOR_2 > THRESHOLD +5) {
roll_right;
ClearTimer(0);
}else if (SENSOR_2 > THRESHOLD +3){
turn_right;
ClearTimer(0);
}else if (SENSOR_2 > THRESHOLD){
OnRev(OUT_AC);
ClearTimer(0);
}else if (SENSOR_2 > THRESHOLD -3){
turn_left;
ClearTimer(0);
}else{
roll_left;
}
Wait(STEP);
}
Off(OUT_AC);
}
sub CATCH() //コップをつかむ
{
roll_right2(70);
go(30);
close;
roll_left2(80);
go(30);
roll_right3;
}
sub RELEASE() //コップを離す
{
roll_right2(90);
open;
turn_left2(30);
back(100)
turn_right2(120);
}
task main()
{
LINE_R();//A~B
go(20);
roll_left3;
LINE_R();//B~C
roll_right2(20);
LINE_R();//C~F
go(25);
roll_left3;
LINE_R();//F~第2ヘアピンカーブ
go(25);
LINE_L();//第2ヘアピンカーブ~R
cross_stop;//R
roll_left2(20);
LINE_L();//R~P
CATCH();
LINE_L();//P~Q
go(30);
LINE_R();//Q~S
cross_stop;//S
go(15
);
LINE_R();//S~S
cross_stop;//S
go(20);
roll_left3;
LINE_R();//S~F
cross_stop;//F
RELEASE();
LINE_R();
cross_stop;//C
roll_right2(50);//Dへ
go(120);
}