- 追加された行はこの色です。
- 削除された行はこの色です。
[[2009b/A2/ロボコン]]
------------------
CENTER:&size(30){紙パックを移動させるロボットのプログラム};
------------------
#define eye_L SENSOR_1 //eye_LをSENSOR_1と定義
#define eye_R SENSOR_3 //eye_RをSENSOR_2と定義
#define cont 45 //contを45と定義
#define time 2 //timeを2と定義
#define new_count ClearSensor(SENSOR_2) //new_countをClearSensor(SENSOR_2)と定義
sub follow_line() //ライントレースのサブタスク
#define eye_L SENSOR_1 //eye_LをSENSOR_1と定義
#define eye_R SENSOR_3 //eye_RをSENSOR_2と定義
#define cont 45 //contを45と定義
#define time 2 //timeを2と定義
#define new_count ClearSensor(SENSOR_2) //new_countをClearSensor(SENSOR_2)と定義
sub follow_line() //ライントレースのサブタスク
{
if ((eye_L >= cont ) && (eye_R >= cont )){
OnRev(OUT_A+OUT_C); Wait(time); Off(OUT_A+OUT_C);
} //白・白で直進
OnRev(OUT_A+OUT_C); Wait(time); Off(OUT_A+OUT_C);
} //白・白で直進
else if ((eye_L >= cont ) && (eye_R <= cont )){
OnRev(OUT_A); Wait(time); Off(OUT_A);
} //白・黒で左に曲がる
OnRev(OUT_A); Wait(time); Off(OUT_A);
} //白・黒で左に曲がる
else if ((eye_L <= cont ) && (eye_R >= cont )){
OnRev(OUT_C); Wait(time); Off(OUT_C);
} //黒・白で右に曲がる
OnRev(OUT_C); Wait(time); Off(OUT_C);
} //黒・白で右に曲がる
else {
OnRev(OUT_A+OUT_C); Wait(time); Off(OUT_A+OUT_C);
} //黒・黒で直進
OnRev(OUT_A+OUT_C); Wait(time); Off(OUT_A+OUT_C);
} //黒・黒で直進
}
task main () //メインプログラム開始
task main () //メインプログラム開始
{
SetSensor(eye_L, SENSOR_LIGHT); //センサー1を光センサーにする
SetSensor(eye_R, SENSOR_LIGHT); //センサー2を光センサーにする
SetSensor(SENSOR_2,SENSOR_ROTATION); //センサー3を回転センサーにする
Wait(20); //0.2秒待ってからスタート
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 35 ) //回転センサーの絶対値が35になるまで
{
follow_line(); //サブタスクのライントレースをする
}
Off(OUT_A+OUT_C); Wait(50); //0.5秒停止
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 3 ) //紙パックのほうを向く
{OnRev(OUT_A); OnFwd(OUT_C);}
OnRev(OUT_A); OnFwd(OUT_C); Wait(5); //向きを微調節する
OnRev(OUT_A+OUT_C); Wait(45); Off(OUT_A+OUT_C); Wait(100); //紙パックをつかみに行くプログラム
OnRev(OUT_B); Wait(200); Off(OUT_B); Wait(100);
OnFwd(OUT_A); OnRev(OUT_C); Wait(15); //本体を左右にゆすりパックをきっちりつかむ
OnRev(OUT_A); OnFwd(OUT_C); Wait(15); Off(OUT_A+OUT_C);
OnFwd(OUT_A+OUT_C); Wait(60); Off(OUT_A+OUT_C); Wait(100);
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 3 ) //本体をラインと平行にする
{
SetSensor(eye_L, SENSOR_LIGHT); //センサー1を光センサーにする
SetSensor(eye_R, SENSOR_LIGHT); //センサー2を光センサーにする
SetSensor(SENSOR_2,SENSOR_ROTATION); //センサー3を回転センサーにする
Wait(20); //0.2秒待ってからスタート
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 35 ) //回転センサーの絶対値が35になるまで
{
follow_line(); //サブタスクのライントレースをする
}
Off(OUT_A+OUT_C); Wait(50); //0.5秒停止
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 3 ) //紙パックのほうを向く
{
OnRev(OUT_A); OnFwd(OUT_C);
}
OnRev(OUT_A); OnFwd(OUT_C); Wait(5); //向きを微調節する
OnRev(OUT_A+OUT_C); Wait(45); Off(OUT_A+OUT_C); Wait(100); //紙パックをつかみに行くプログラム
OnRev(OUT_B); Wait(200); Off(OUT_B); Wait(100);
OnFwd(OUT_A); OnRev(OUT_C); Wait(15); //本体を左右にゆすりパックをきっちりつかむ
OnRev(OUT_A); OnFwd(OUT_C); Wait(15); Off(OUT_A+OUT_C);
OnFwd(OUT_A+OUT_C); Wait(60); Off(OUT_A+OUT_C); Wait(100);
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 3 ) //本体をラインと平行にする
{
OnFwd(OUT_A); OnRev(OUT_C);
}
Off(OUT_A+OUT_C); Wait(100);
OnFwd(OUT_A); OnRev(OUT_C); Wait(10); //向きを微調節する
OnRev(OUT_A); OnFwd(OUT_C); Wait(10); Off(OUT_A+OUT_C);
OnRev(OUT_A+OUT_C); Wait(5); Off(OUT_A+OUT_C); Wait(100);
OnFwd(OUT_A); OnRev(OUT_C); Wait(5); Off(OUT_A+OUT_C);
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 27) //回転センサーの絶対値が27になるまで
{
follow_line(); //ライントレースする
}
PlaySound(1); Off(OUT_A+OUT_C); Wait(100);
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 10) //回転センサーの絶対値が10になるまで
{
OnFwd(OUT_A+OUT_C); //バックして右カーブに備える
}
Off(OUT_A+OUT_C); Wait(100);
OnRev(OUT_A); OnFwd(OUT_C); Wait(30); Off(OUT_A+OUT_C); //右へ曲がる
Off(OUT_A+OUT_C); Wait(20);
OnRev(OUT_A+OUT_C); Wait(30); Off(OUT_A+OUT_C); Wait(50);
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 45 ) //回転センサーの絶対値が45になるまで
{
follow_line(); //ライントレースする
}
Wait(50); OnFwd(OUT_A+OUT_C); Wait(20); Off(OUT_A+OUT_C); //バックして右カーブに備える
repeat(3) //かっこ内を三回くりかえす
{
}
Off(OUT_A+OUT_C); Wait(100);
OnFwd(OUT_A); OnRev(OUT_C); Wait(10); //向きを微調節する
OnRev(OUT_A); OnFwd(OUT_C); Wait(10); Off(OUT_A+OUT_C);
OnRev(OUT_A+OUT_C); Wait(5); Off(OUT_A+OUT_C); Wait(100);
OnFwd(OUT_A); OnRev(OUT_C); Wait(5); Off(OUT_A+OUT_C);
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 27) //回転センサーの絶対値が27になるまで
{
follow_line(); //ライントレースする
}
PlaySound(1); Off(OUT_A+OUT_C); Wait(100);
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 10) //回転センサーの絶対値が10になるまで
{
OnFwd(OUT_A+OUT_C); //バックして右カーブに備える
}
Off(OUT_A+OUT_C); Wait(100);
OnRev(OUT_A); OnFwd(OUT_C); Wait(30); Off(OUT_A+OUT_C); //右へ曲がる
Off(OUT_A+OUT_C); Wait(20);
OnRev(OUT_A+OUT_C); Wait(30); Off(OUT_A+OUT_C); Wait(50);
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 45 ) //回転センサーの絶対値が45になるまで
{
follow_line(); //ライントレースする
}
Wait(50); OnFwd(OUT_A+OUT_C); Wait(20); Off(OUT_A+OUT_C); //バックして右カーブに備える
repeat(3) //かっこ内を三回くりかえす
{
OnRev(OUT_A); OnFwd(OUT_C); Wait(5); Off(OUT_A+OUT_C); Wait(50);
OnRev(OUT_A+OUT_C); Wait(10); Off(OUT_A+OUT_C); Wait(50);
OnRev(OUT_A); OnFwd(OUT_C); Wait(10); Off(OUT_A+OUT_C); Wait(50);
} //角度を少しずつ曲げながらライン上に乗る
PlaySound(1); Off(OUT_A+OUT_C);
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 51 ) //回転センサーの絶対値が51になるまで
{
follow_line(); //ライントレースする
}
OnFwd(OUT_B); Wait(190); Off(OUT_B); Wait(100); //紙パックを離す
OnFwd(OUT_A+OUT_C); Wait(40); Off(OUT_A+OUT_C); //バックして最後のライントレースの線上に乗る
PlaySound(1); Off(OUT_A+OUT_C);
new_count;
while ( abs(SENSOR_2) <= 3 ) //ラインの方を向く
} //角度を少しずつ曲げながらライン上に乗る
PlaySound(1); Off(OUT_A+OUT_C);
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 51 ) //回転センサーの絶対値が51になるまで
{
follow_line(); //ライントレースする
}
OnFwd(OUT_B); Wait(190); Off(OUT_B); Wait(100); //紙パックを離す
OnFwd(OUT_A+OUT_C); Wait(40); Off(OUT_A+OUT_C); //バックして最後のライントレースの線上に乗る
PlaySound(1); Off(OUT_A+OUT_C);
new_count;
while ( abs(SENSOR_2) <= 3 ) //ラインの方を向く
{
OnRev(OUT_A); OnFwd(OUT_C);
}
Off(OUT_A+OUT_C);
OnRev(OUT_A); OnFwd(OUT_C); Wait(7); //向きを微調節する
Off(OUT_A+OUT_C); Wait(20);
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 45 ) //回転センサーの絶対値が45になるまで
{
follow_line(); //ライントレースする
}
OnRev(OUT_C); Wait(80); //スタート地点のほうを向く
OnRev(OUT_A+OUT_C); Wait(100); Off(OUT_A+OUT_C); //スタート地点に入り停止
}
Off(OUT_A+OUT_C);
OnRev(OUT_A); OnFwd(OUT_C); Wait(7); //向きを微調節する
Off(OUT_A+OUT_C); Wait(20);
new_count; //回転センサーをリセット
while ( abs(SENSOR_2) <= 45 ) //回転センサーの絶対値が45になるまで
{
follow_line(); //ライントレースする
}
OnRev(OUT_C); Wait(80); //スタート地点のほうを向く
OnRev(OUT_A+OUT_C); Wait(100); Off(OUT_A+OUT_C); //スタート地点に入り停止
}