![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
#define blackline 40 //ライトセンサーが黒線を認識するときの値を40とする
int frag; //変数frag(黒線を認識した本数)の設定
sub linetrace (){ //ライントレースを定義
if ((SENSOR_1 <= blackline) && (SENSOR_3 <= blackline)){ //センサー１とセンサー３が共に黒線を認識したとき
OnFwd(OUT_A + OUT_C); //直進
Wait(030);
frag++; //fragを１足す
}
else{
if ((SENSOR_1 <= blackline) && (SENSOR_3 >= blackline)){ //センサー１のみが黒線を認識したとき
OnFwd (OUT_A);OnRev (OUT_C); //左折
}
if ((SENSOR_1 >= blackline) && (SENSOR_3 <= blackline)){ //センサー３のみが黒線を認識したとき
OnFwd (OUT_C);OnRev (OUT_A); //右折
}
if ((SENSOR_1 >= blackline) && (SENSOR_3 >= blackline)) { //センサー１、センサー３共に黒線を認識していないとき
OnFwd (OUT_A + OUT_C); //直進
}
}
}
sub hold_can (){ //缶を受け取るときの動作を定義
ClearMessage();
until (Message() != 0);
if ( Message() == 1 ){ //メッセージ１を受け取ったとき
Wait(1000); //10秒静止
OnFwd(OUT_B);Wait(200); //アームを下ろす
OnRev(OUT_A+OUT_C);Wait(350); //3,5秒後進
}
}
sub port_can (){ //缶を離す時の動作を定義
OnFwd(OUT_A+OUT_C);Wait(30); //0,3秒直進
OnRev(OUT_B);Wait (200); //アームを上げる
Off (OUT_B);
OnFwd(OUT_A+OUT_C);
Wait(50); //0,5秒直進して缶を入れる
OnRev(OUT_A+OUT_C);
Wait(250); //2,5秒後進
｝
sub turn_R (){ //右折を定義
Off (OUT_A + OUT_C);OnRev (OUT_A+OUT_C);Wait (007);OnFwd (OUT_A);Wait (150);Off (OUT_A+OUT_C);
}
sub turn_r(){ //缶を持っていないときの右折を定義
Off(OUT_A+OUT_C);OnRev(OUT_A+OUT_C);Wait(007);OnFwd (OUT_A);Wait(130);
}
sub turn_L (){ //左折を定義
Off (OUT_A + OUT_C);OnRev (OUT_C+OUT_A);Wait (007);OnFwd (OUT_C);Wait (130);Off (OUT_A+OUT_C);
}
task main(){
SetSensor (SENSOR_1, SENSOR_LIGHT);
SetSensor (SENSOR_3, SENSOR_LIGHT);
frag=0; //黒線の数を０とする
while (true){
if(frag==0){ //黒線を０本認識しているとき
hold_can(); //缶を受け取る
turn_R(); //右折
frag=1; //黒線の数を１とする
}
if (frag ==1||2||6||7||9){ //黒線を1,2,6,7,9本認識しているとき
linetrace (); //ライントレース
}
if (frag == 3){ //黒線を３本認識したとき
OnFwd(OUT_A+OUT_C);Wait(30); //0,3秒直進
turn_L (); //左折
frag = 4; //黒線の数を４本とする
}
if (frag == 4){ //黒線を４本認識しているとき
port_can(); //缶を離す
turn_L(); //左折
frag=6; //黒線の数を６本とする
}
if (frag == 8){ //黒線を８本認識しているとき
OnFwd(OUT_A+OUT_C);Wait(050); //0,5秒直進
turn_r (); //右折
frag =9; //黒線の数を９本とする
}
if (frag == 10){ //黒線を10本認識しているとき
OnFwd(OUT_A+OUT_C);Wait(120); //1,2秒直進
Off(OUT_A+OUT_C);
Wait(80); //0,8秒静止
frag = 0; //黒線の数を０本とする
}
}
}
黒線を認識した本数で場合わけし、繰り返しを防ぐために一つの動作が終わるごとに本数を１ずつ足した。
