![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
//作成者：ＳＡＳ //作成日時：2/4
最終更新日：2006-02-14 (火) 04:58:54
#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 SPA(t) SetPower(OUT_A,t); //Aのトルクを変えるマクロ
#define SPC(t) SetPower(OUT_C,t); //Cのトルクを変えるマクロ
#define white1 SENSOR_1 >= 42
#define green1 (SENSOR_1 >= 33)&&(SENSOR_1 <= 41)
#define black1 SENSOR_1 <= 32
#define white3 SENSOR_3 >= 42
#define green3 (SENSOR_3 >= 33)&&(SENSOR_3 <= 41)
#define black3 SENSOR_3 <= 32
int count ; //countという変数を使う
int r_angled;
task main ()
{
r_angled = 86; //直角に曲がる時間
SetSensor(SENSOR_1, SENSOR_LIGHT); //センサー1と2を
SetSensor(SENSOR_3, SENSOR_LIGHT); //光センサーに定義
ClearMessage(); //メッセージを消す
count = 0; //countに0を代入
SPA(2);SPC(2);
Wait(1000);
SendMessage(1);until(Message() !=0)
if(Message() !=0)
{SendMessage(2);Wait(400);
turn_r; //右に旋回する
until(black1&&white3);} //左のセンサーが黒、右のセンサーが白になるまで
if(black1&&white3) //左のセンサーが黒、右のセンサーが白ならば
{start line_trace_for;} //タスク line_trace_for を実行する
}
task line_trace_for ()
{
if(white1&&white3) //白いところは
{SPA(1);SPC(1);go_st;} // トルク1で前進する
if(black1&&white3) //左のセンサーが黒なら
{SPA(3);SPC(1);turn_l;} // 左右のトルクが3:1で左旋回
if(white1&&black3) //右のセンサーが黒なら
{SPA(1);SPC(3);turn_r;} // 左右のトルクが1:3で右旋回
if(green1&&green3) //両方 緑 なら
{SPA(7);SPC(7); //左右のトルクを7に戻し、
while (true) //真の間繰り返す
{
if(green1&&green3) //左右のセンサーが 緑 なら
{count += 1;} //変数 count を1増やす
if(Message() == count) //メッセージの番号と変数 count が一致したら、
{start put_box;} //タスク put_box を実行し、
stop line_trace_for;} //タスク line_trace_for を終了する
if(Message() != count) //メッセージの番号と変数 count が一致しなければ、
{go_st;} //前進する
}
}
task line_trace_back ()
{
if(white1&&white3) //白いところは
{SPA(1);SPC(1);go_st;} // トルク1で前進する
if(black1&&white3) //左のセンサーが黒なら
{SPA(3);SPC(1);turn_l;} // 左右のトルクが3:1で左旋回
if(white1&&black3) //右のセンサーが黒なら
{SPA(1);SPC(3);turn_r;} // 左右のトルクが1:3で右旋回
if(green1&&green3) //両方 緑 なら
{SPA(7);SPC(7); //左右のトルクを7に戻し、
turn_r;Wait(r_angled); //90°右に旋回する
stop line_trace_back;} //タスク line_trace_back を終了する
}
task put_box ()
{
go_st;Wait(30);float; //前進した後滑らかに止まる
turn_r;Wait(r_angled);float; //90°右に旋回した後滑らかに止まる
go_ba;Wait(40); //後退して機体を真っ直ぐにする
go_st;Wait(60); //前進してカゴにぶつかる
OnFwd(OUT_B);Wait(100); //箱を置く
OnRev(OUT_B);Wait(100); //装置を元に戻す
go_ba;until(black1&&black3); //黒い線のところまで後退する
go_st;Wait(10);float; //少し前進した後滑らかに止まる
turn_r;Wait(r_angled);float; //90°右に旋回した後滑らかに止まる
while(true) //真の間繰り返す
{
go_st;until(green1&&green3); //左右のセンサーが 緑 になるまで前進する
if(green1&&green3); //左右のセンサーが 緑 なら、
{count -= 1;} //変数 count を1減らす
if(count == 0) //変数 count が0になったら、
{go_st;Wait(10); //少し前進して、
start line_trace_back; //タスク line_trace_back を実行し、
stop put_box;} //タスク put_box を終了する
else //変数 count が0でなければ、
{go_st;until(green1&&green3);} //左右のセンサーが 緑 になるまで前進する
}
}
