![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
#define go OnFwd(OUT_A+OUT_C); //前進
#define back(t) OnRev(OUT_A+OUT_C);Wait(t);Off(OUT_A+OUT_C);//後退
#define tomaru Off(OUT_A+OUT_C);//停止
#define turn_l OnFwd(OUT_C); //左折
#define turn_r OnFwd(OUT_A); //右折
#define shine 45 //ライントレースするための光の値
#define up(t) OnFwd(OUT_B);Wait(t);Off(OUT_B); //アームを上げる
#define down(t) OnRev(OUT_B);Wait(t);Off(OUT_B);//アームを下げる
int n=0;
task follow_line_1(){ //交差点を直進するプログラム
while(true){
if((SENSOR_1 >= shine)&&(SENSOR_3 >= shine)){
go;
until((SENSOR_1 < shine)||(SENSOR_3 < shine));
tomaru;
}
else if((SENSOR_1 < shine)&&(SENSOR_3 >= shine)){
turn_l;
until((SENSOR_1 >= shine)&&(SENSOR_3 >= shine));
tomaru;
}
else if((SENSOR_1 >= shine)&&(SENSOR_3 < shine)){
turn_r;
until(((SENSOR_1 >= shine)&&(SENSOR_3 >= shine))||((SENSOR_1 < shine)&&(SENSOR_3 < shine)));
tomaru;
}
else if((SENSOR_1 < shine)&&(SENSOR_3 < shine)){
PlaySound(SOUND_UP);
turn_l;
until((SENSOR_1 >= shine)&&(SENSOR_3 >= shine));
tomaru;
}
}
}
task follow_line_2(){ //交差点を右折するプログラム（玉をとってからゴールまで向かうプログラム）
while(true){
if((SENSOR_1 >= shine)&&(SENSOR_3 >= shine)){
go;
until((SENSOR_1 < shine)||(SENSOR_3 < shine));
tomaru;
}
else if((SENSOR_1 < shine)&&(SENSOR_3 >= shine)){
turn_l;
until((SENSOR_1 >= shine)&&(SENSOR_3 >= shine));
tomaru;
}
else if((SENSOR_1 >= shine)&&(SENSOR_3 < shine)){
turn_r;
until(((SENSOR_1 >= shine)&&(SENSOR_3 >= shine))||((SENSOR_1 < shine)&&(SENSOR_3 < shine)));
tomaru;
}
else if((SENSOR_1 < shine)&&(SENSOR_3 < shine)){
PlaySound(SOUND_UP);
OnFwd(OUT_A);OnRev(OUT_C);
ClearTimer(0); //交差点を曲がるとき曲がりすぎないように曲がる時間の最大を決める
until(((SENSOR_1 >= shine)&&(SENSOR_3 >= shine))||(FastTimer(0) > 70));
tomaru;
ClearTimer(1);n++;PlaySound(3); //誤差を少なくするために交差点（下図の交差点２）を曲がったらタイマーをリセットする。
//ちょうどゴールの横に来るまでタイマーの時間を決める。
//そして交差点を曲がってからタイマーの時間が来るまでライントレースする。
}
}
}
task follow_line_3(){ //狭い交差点を左折するプログラム
while(true){
if((SENSOR_1 >= shine)&&(SENSOR_3 >= shine)){
go;
until((SENSOR_1 < shine)||(SENSOR_3 < shine));
tomaru;
}
else if((SENSOR_1 < shine)&&(SENSOR_3 >= shine)){
turn_l;
until((SENSOR_1 >= shine)&&(SENSOR_3 >= shine));
tomaru;
}
else if((SENSOR_1 >= shine)&&(SENSOR_3 < shine)){
turn_r;
until(((SENSOR_1 >= shine)&&(SENSOR_3 >= shine))||((SENSOR_1 < shine)&&(SENSOR_3 < shine)));
tomaru;
}
else if((SENSOR_1 < shine)&&(SENSOR_3 < shine)){
PlaySound(SOUND_UP);
back(50); //アームが長くて（アームが壁に当たって）曲がりきれないので少し後退してから曲がる
turn_l;Wait(100);tomaru;
go;Wait(40);tomaru;
go;
until(SENSOR_1 < shine);
tomaru;
}
}
}
task follow_line_4(){ //狭い交差点を右折するプログラム
while(true){
if((SENSOR_1 >= shine)&&(SENSOR_3 >= shine)){
go;
until((SENSOR_1 < shine)||(SENSOR_3 < shine));
tomaru;
}
else if((SENSOR_1 < shine)&&(SENSOR_3 >= shine)){
turn_l;
until((SENSOR_1 >= shine)&&(SENSOR_3 >= shine));
tomaru;
}
else if((SENSOR_1 >= shine)&&(SENSOR_3 < shine)){
turn_r;
until(((SENSOR_1 >= shine)&&(SENSOR_3 >= shine))||((SENSOR_1 < shine)&&(SENSOR_3 < shine)));
tomaru;
}
else if((SENSOR_1 < shine)&&(SENSOR_3 < shine)){
PlaySound(SOUND_UP);
back(50); //アームが長くて（アームが壁に当たって）曲がりきれないので少し後退してから曲がる
turn_r;Wait(100);tomaru;
go;Wait(40);tomaru;
go;
until(SENSOR_3 < shine);
tomaru;
}
}
}
task main(){
SetSensor(SENSOR_1,SENSOR_LIGHT);//センサー1を光センサーに決める
SetSensor(SENSOR_2,SENSOR_TOUCH);//センサー2をタッチセンサーに決める
SetSensor(SENSOR_3,SENSOR_LIGHT);//センサー3を光センサーに決める
Wait(1500); //紙パックをとるロボットと衝突しないように15秒待機
OnFwd(OUT_C);OnRev(OUT_A); //スタート地点から出てライントレースを始めるためのプログラム
Wait(90); //（下図の黒い矢印の部分）
tomaru;
start follow_line_1; //スタート地点の枠の線をライントレースする
Wait(200);
stop follow_line_1;
tomaru;
turn_l;
Wait(40);
tomaru;
go;
Wait(160);
tomaru;
start follow_line_1; //ライントレースを開始
Wait(230);
stop follow_line_1;
tomaru;
turn_r; //ピンポン玉をとるために右に90度方向変換
Wait(170);
tomaru;
go; //ピンポン玉がありそうな所まで前進
Wait(60);
tomaru;
down(160); //アームを下げてピンポン玉をとる
OnFwd(OUT_A);OnRev(OUT_C); //機体を横に振ってピンポン玉をとりやすくする
Wait(20);
tomaru;
OnFwd(OUT_C);OnRev(OUT_A);
Wait(20);
tomaru;
OnFwd(OUT_A);OnRev(OUT_C);
Wait(20);
tomaru;
OnFwd(OUT_C);OnRev(OUT_A);
Wait(20);
tomaru;
up(165); //ゴールの枠を越えられる高さまでアームを上げる
OnRev(OUT_A+OUT_C); //ライン上に戻るまで後退
until((SENSOR_1 < shine)&&(SENSOR_3 < shine));
tomaru;
OnFwd(OUT_C);OnRev(OUT_A);
Wait(70);
tomaru;
start follow_line_2; //ゴールまでライントレースをする
until((FastTimer(1) > 290)&&(n > 0)); //follow_line_2の最後のところでも説明したように交差点２を曲がったら
stop follow_line_2; //タイマーをリセットして、ここでゴールの手前まで行く時間を
tomaru; //設定することでより正確にゴールまで行くことができる
turn_l; //ゴールの手前まできたら左に90度方向変換
Wait(180);
tomaru;
go; //ゴールの枠まで前進
until(SENSOR_2 == 1);
tomaru;
up(165); //アームをさらに上げて上のストッパーにピンポン玉を当ててゴールに玉を落とす
back(170); //ライン上まで後退
OnFwd(OUT_C);OnRev(OUT_A);
Wait(80);
tomaru;
down(155); //次に備えてアームをピンポン玉をとりやすい位置まで下げる
repeat(3){ //あと三回玉をとりにいく
n=0; //（下図の赤い矢印の部分がrepeat）
start follow_line_3; //ゴールの所からピンポン玉がある所までライントレースをする
Wait(715);
stop follow_line_3;
tomaru;
turn_l; //玉をとるために左に90度方向変換
Wait(170);
tomaru;
go; //玉がありそうな所まで前進
Wait(200);
tomaru;
down(160); //アームを下げて玉をとる
OnFwd(OUT_A);OnRev(OUT_C); //機体を横に振って玉をとりやすくする
Wait(20);
tomaru;
OnFwd(OUT_C);OnRev(OUT_A);
Wait(20);
tomaru;
OnFwd(OUT_A);OnRev(OUT_C);
Wait(20);
tomaru;
OnFwd(OUT_C);OnRev(OUT_A);
Wait(20);
tomaru;
up(165); //ゴールの枠を越えられる高さまでアームを上げる
OnRev(OUT_A+OUT_C); //ライン上まで後退
until((SENSOR_1 < shine)&&(SENSOR_3 < shine));
tomaru;
OnFwd(OUT_C);OnRev(OUT_A);
Wait(70);
tomaru;
start follow_line_2; //ゴールまでライントレースする
until((FastTimer(1) > 290)&&(n > 0));
stop follow_line_2;
tomaru;
turn_l; //ゴールの横まできたら左に90度方向変換
Wait(180);
tomaru;
go; //ゴールの枠まで前進
until(SENSOR_2 == 1);
tomaru;
up(165); //アームを上げて玉をゴールに入れる
back(170);
OnFwd(OUT_C);OnRev(OUT_A); //ライン上まで後退
Wait(80);
tomaru;
down(155);
}
start follow_line_4; //玉をとり終わったらスタート地点まで戻る
Wait(700); //（下図の青い矢印の部分）
stop follow_line_4;
tomaru;
OnRev(OUT_A+OUT_C); //後退してスタート地点に入ったら停止
until((SENSOR_1 < shine)&&(SENSOR_3 < shine));
tomaru;
}
ロボットの進む経路
○左図はピンポン玉をゴールへ運ぶときの経路
○右図はスタート地点まで戻るときの経路
無題42.jpg
無題43.jpg
