![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
アクセス数：今日1 総数&counter([total|today|yesterday]); 最終更新日：2006-02-14 (火) 05:01:33
//作成者：ＳＡＳ //作成日時：2/6〜7,12
#define go_st OnFwd(OUT_A+OUT_C); //前に進むマクロ
#define go_ba OnRev(OUT_A+OUT_C); //後ろに進むマクロ
#define turn_l OnFwd(OUT_C);OnRev(OUT_A); //左に旋回するマクロ
#define turn_r OnFwd(OUT_A);OnRev(OUT_C); //右に旋回するマクロ
#define float Float(OUT_A+OUT_C); //滑らかに止まるマクロ
#define SP(x,y) SetPower(OUT_A,x);SetPower(OUT_C,y); //A,Cのトルクを変えるマクロ
#define white1 SENSOR_1 >= 46 //センサー1で白は46以上と定義
#define black1 SENSOR_1 <= 45 //センサー1で黒は45以下と定義
#define white3 SENSOR_3 >= 46 //センサー3で白は46以上と定義
#define black3 SENSOR_3 <= 45 //センサー3で黒は45以下と定義
int count ; //countという変数（今通過した線が何本目かを数える変数）を使う
int r_angled; //r_amgledという変数（回転を90°にする変数）を使う
int message_upper; //message_upperという変数（上段の紙パックが何色かを記録する変数）を使う
int message_lower; //message_lowerという変数（下段の紙パックが何色かを記録する変数）を使う
int times; //timesという変数（今運んでいる動作が何回目かを数える変数）を使う
task main () //
{
r_angled = 27; //直角に曲がる時間
SetSensor(SENSOR_1, SENSOR_LIGHT); //センサー1と2を
SetSensor(SENSOR_3, SENSOR_LIGHT); //光センサーに定義
ClearMessage(); //メッセージを消す
count = 0; //変数countに0を代入
times = 1; //変数timesに1を代入
message_upper = 0; //変数message_upperに0を代入
message_lower = 0; //変数message_lowerに0を代入
SP(2,2); //左右のトルクを2にする
start pick_box; //タスク pick_box を開始する
}
task pick_box () //タスク pick_box(箱を掴むタスク)
{
stop line_trace_back; //タスク line_trace_back を終了する
SendMessage(1);Wait(100); //Message1を送信する
go_st;Wait(50);float;Wait(150); //少し前進した後止まり、150待つ
ClearTimer(0); //Timer(0)を0にする
while(Timer(0) <= 44) //Timer(0)が44以下のとき
{go_st;Wait(50);float;Wait(60); //少し前進して止まり、
if(Message() == 1) //Messageが1なら
{message_upper = 1;} //message_upperに1を代入
if(Message() == 2) //Messageが2なら
{message_upper = 2;} //message_upperに2を代入
if(Message() == 3) //Messageが3なら
{message_upper = 3;} //message_upperに3を代入
if(Message() == 4) //Messageが4なら
{message_lower = Message() - 3;} //message_lowerにMessageから3を引いたものを代入
if(Message() == 5) //Messageが5なら
{message_lower = Message() - 3;} //message_lowerにMessageから3を引いたものを代入
if(Message() == 6) //Messageが6なら
{message_lower = Message() - 3;} //message_lowerにMessageから3を引いたものを代入
}
if(Timer(0) >= 45) //Timer(0)が45以下のとき
{go_ba;until((black1)&&(black3));} //左右のセンサーが黒になるまで後退する
if((black1)&&(black3)) //左のセンサーが黒、右のセンサーが白ならば
{turn_l;Wait(r_angled);float;} //90°左に旋回し滑らかに停止する
if(times <= 2) //変数 times が2以下のとき
{repeat(2) //2回繰り返す
{go_st;Wait(5);go_st;until((black1)&&(black3));float;} //少し進んだ後左右のセンサーが黒になるまで前進する
go_st;Wait(5);float; //少し進んで滑らかに停止する
turn_r;Wait(r_angled);float; //90°右に旋回し滑らかに停止する
go_st;until((black1)&&(white3)); //左のセンサーが黒、右のセンサーが白になるまで前進する
turn_l;Wait(r_angled);float; //90°左に旋回し滑らかに停止する
go_st;Wait(10); //少し進む
start line_trace_for;} //タスク line_trace_for を開始する
if(times == 3) //変数 times が3のとき
{go_st;Wait(5);go_st;until((black1)&&(black3));float; //少し進んだ後左右のセンサーが黒になるまで前進する
go_st;Wait(5); //少し進む
turn_r;Wait(r_angled);float; //90°右に旋回し滑らかに停止する
go_st;until((black1)&&(white3)); //左のセンサーが黒、右のセンサーが白になるまで前進する
turn_l;Wait(r_angled);float; //90°左に旋回し滑らかに停止する
go_st;Wait(5); //少し進む
start line_trace_for;} //タスク line_trace_for を開始する
if(times == 4) //変数 times が2以下のとき
{repeat(3) //3回繰り返す
{go_st;Wait(5);go_st;until((black1)&&(black3));float;} //少し進んだ後左右のセンサーが黒になるまで前進する
go_st;Wait(5); //少し進む
turn_r;Wait(r_angled);float; //90°右に旋回し滑らかに停止する
go_st;until((black1)&&(white3)); //左のセンサーが黒、右のセンサーが白になるまで前進する
turn_l;Wait(r_angled);float; //90°左に旋回し滑らかに停止する
go_st;Wait(5); //少し進む
start line_trace_for;} //タスク line_trace_for を開始する
}
task line_trace_for () //タスク line_trace_for(曲がりくねったところをライントレースするプログラム)
{
stop pick_box; //タスク pick_box を終了する
while(true) //条件が真の間
{if((white1)&&(white3)) //白いところは
{SP(1,1);go_st;} // トルク1で前進する
if((black1)&&(white3)) //左のセンサーが黒なら
{SP(3,1);turn_l;} // 左右のトルクが3:1で左旋回
if((white1)&&(black3)) //右のセンサーが黒なら
{SP(1,3);turn_r;} // 左右のトルクが1:3で右旋回
if((black1)&&(black3)) //両方 緑 なら
{SP(7,7); //左右のトルクを7に戻し、
while (true) //真の間繰り返す
{if((black1)&&(black3)) //左右のセンサーが 緑 なら
{count += 1;} //変数 count を1増やす
if(message_lower == count) //メッセージの番号(message_lower)と変数 count が一致したら、
{start put_box; //タスク put_box を実行し、
stop line_trace_for;} //タスク line_trace_for を終了する
if(message_lower != count) //メッセージの番号(message_lower)と変数 count が一致しなければ、
{go_st;} //前進する
}
}
}
}
task put_box () //タスク put_box(運んできた紙パックの一個目を置くプログラム)
{
stop line_trace_for; //タスク line_trace_for を終了する
stop after_green;stop after_white;stop after_black; //タスク after_green,after_white,after_black を終了する
go_st;Wait(10);float; //前進した後滑らかに止まる
turn_r;Wait(r_angled);float; //90°右に旋回した後滑らかに止まる
go_ba;Wait(50); //後退して機体をカゴに対して真っ直ぐにする
go_st;Wait(200); //前進してカゴにぶつかる
if((message_lower == 1||2||3)&&(times == 1)) //変数 message_lower が1,2,3の何れか且つ変数 times が1なら
{SendMessage(2);Wait(200);} //Message(2)を送る
if((message_lower == 1||2||3)&&(times >= 2)) //変数 message_lower が1,2,3の何れか且つ変数 times が2なら
{SendMessage(3);Wait(200);} //Message(3)を送る
Wait(300); //300待つ
go_ba;until((black1)&&(black3)); //黒い線のところまで後退する
if(message_lower == 1) //変数 message_lower が1なら
{start after_green;} //タスク after_green を開始する
if(message_lower == 2) //変数 message_lower が2なら
{start after_white;} //タスク after_white を開始する
if(message_lower == 3) //変数 message_lower が3なら
{start after_black;} //タスク after_black を開始する
if(message_lower == 0) //変数 message_lower が0なら
{start line_trace_back;} //タスク line_trace_for を開始する
}
task line_trace_back () //タスク line_trace_back(紙パックを掴みに戻るプログラム)
{
stop put_box; //タスク put_box を終了する
times += 1; //変数 times に1を足す
go_st;Wait(5);float; //少し前進した後滑らかに止まる
turn_r;Wait(r_angled);float; //90°右に旋回した後滑らかに止まる
while(count != 0) //条件が真の間繰り返す
{go_st;until((black1)&&(black3)); //左右のセンサーが黒になるまで前進する
if((black1)&&(black3)); //左右のセンサーが黒なら、
{count -= 1;} //変数 count を1減らす
if(count == 0) //変数 count が0になったら、
{go_st;Wait(20);} //少し前進する
}
while(count == 0) //変数 count が0 の間繰り返す
{if((white1)&&(white3)) //白いところは
{SP(1,1);go_st;} // トルク1で前進する
if((black1)&&(white3)) //左のセンサーが黒なら
{SP(3,1);turn_l;} // 左右のトルクが3:1で左旋回
if((white1)&&(black3)) //右のセンサーが黒なら
{SP(1,3);turn_r;} // 左右のトルクが1:3で右旋回
if((black1)&&(black3)) //両方黒なら
{SP(7,7);count =1; //左右のトルクを7に戻し、
turn_r;Wait(r_angled);float; //90°右に旋回する
go_st;until((black1)&&(white3)); //左のセンサーが黒、右のセンサーが白になるまで前進する
if((black1)&&(white3)) //左のセンサーが黒、右のセンサーが白なら
{;turn_l;Wait(r_angled);float; //90°左に旋回する
go_st;until((black1)&&(black3));count += 1} //左右のセンサーが黒になるまで前進し、変数 count に1を足す
}
if((black1)&&(black3)&&(times <= 2)) //左右のセンサーが黒で且つ変数 times が2以下なら
{go_st;Wait(5); //少し進んでから
go_st;until((black1)&&(black3));float; //左右のセンサーが黒になるまで前進して滑らかに停止し、
go_ba;Wait(50); //少し後退した後
turn_r;Wait(r_angled); //90°右に旋回する
start pick_box;} //タスク pick_box を開始する
if((black1)&&(black3)&&(times == 3)) //左右のセンサーが黒で且つ変数 times が3なら
{go_ba;Wait(50); //少し後退した後
turn_r;Wait(r_angled);float; //90°右に旋回し、
start pick_box;} //タスク pick_box を開始する
if((black1)&&(black3)&&(times == 4)) //左右のセンサーが黒で且つ変数 times が4なら
{repeat(2) //2回繰り返す
{go_st;Wait(5);go_st;until((black1)&&(black3));float;} //少し進んだ後左右のセンサーが黒になるまで前進する
go_ba;Wait(50); //少し後退した後
turn_r;Wait(r_angled); //90°右に旋回し
start pick_box;}} //タスク pick_box を開始する
}
task after_green () //タスク after_green(一個目の紙パックが緑だったとき、二個目の紙パックを置くプログラム)
{
stop put_box; //タスク put_box を終了する
if(message_upper == 2) //変数 message_upper が2のとき
{turn_l;Wait(r_angled);float; //90°左に旋回する
go_st;until((black1)&&(black3));float; //左右のセンサーが黒になるまで前進する
go_st;Wait(30);float; //少し前進する
turn_r;Wait(r_angled);float; //90°右に旋回する
go_st;Wait(200);message_upper == 0;message_lower ==0; //200進んで変数 message_upper,message_lowerを0にする
start put_box;} //タスク put_box を開始する
if(message_upper == 3) //変数 message_upper
{turn_l;Wait(r_angled);float; //90°左に旋回する
repeat(2) //2回繰り返す
{go_st;until((black1)&&(black3));float;} //左右のセンサーが黒になるまで前進する
go_st;Wait(30);float; //少し前進する
turn_r;Wait(r_angled);float; //90°右に旋回する
go_st;Wait(200);message_upper == 0;message_lower ==0; //200進んで変数 message_upper,message_lowerを0にする
start put_box;} //タスク put_box を開始する
}
task after_white () //タスク after_green(一個目の紙パックが白だったとき、二個目の紙パックを置くプログラム)
{
stop put_box; //タスク put_box を終了する
if(message_upper == 1) //変数 message_upper
{turn_r;Wait(r_angled);float; //90°右に旋回する
go_st;until((black1)&&(black3));float; //左右のセンサーが黒になるまで前進する
go_st;Wait(30);float; //少し前進する
turn_l;Wait(r_angled);float; //90°左に旋回する
go_st;Wait(200);message_upper == 0;message_lower ==0; //200進んで変数 message_upper,message_lowerを0にする
start put_box;} //タスク put_box を開始する
if(message_upper == 3) //変数 message_upper
{turn_l;Wait(r_angled);float; //90°左に旋回する
go_st;until((black1)&&(black3));float; //左右のセンサーが黒になるまで前進する
go_st;Wait(30);float; //少し前進する
turn_r;Wait(r_angled);float; //90°右に旋回する
go_st;Wait(200);message_upper == 0;message_lower ==0; //200進んで変数 message_upper,message_lowerを0にする
start put_box;} //タスク put_box を開始する
}
task after_black () //タスク after_green(一個目の紙パッ クが黒だったとき、二個目の紙パックを置くプログラム)
{
stop put_box; //タスク put_box を終了する
if(message_upper == 2) //変数 message_upper
{turn_r;Wait(r_angled);float; //90°右に旋回する
go_st;until((black1)&&(black3));float; //左右のセンサーが黒になるまで前進する
go_st;Wait(30);float; //少し前進する
turn_l;Wait(r_angled);float; //90°左に旋回する
go_st;Wait(200);message_upper == 0;message_lower ==0; //200進んで変数 message_upper,message_lowerを0にする
start put_box;} //タスク put_box を開始する
if(message_upper == 1) //変数 message_upper
{turn_r;Wait(r_angled);float; //90°右に旋回する
repeat(2) //2回繰り返す
{go_st;until((black1)&&(black3));float;} //左右のセンサーが黒になるまで前進する
go_st;Wait(30);float; //少し前進する
turn_l;Wait(r_angled);float; //90°左に旋回する
go_st;Wait(200);message_upper == 0;message_lower ==0; //200進んで変数 message_upper,message_lowerを0にする
start put_box;} //タスク put_box を開始する
}
