![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
ロボコン用プログラム子機
//OUT_C is arm and OUT_A is turntable
//OnRev (OUT_C) is up and OnFwd (OUT_C); is down
//OnRev (OUT_A) is side and OnFwd (OUT_A); is back
int m; //Message No.
task main ()
{
SetSensor (SENSOR_1, SENSOR_LIGHT);
SetSensor (SENSOR_2, SENSOR_TOUCH);
ClearMessage();
start arm;
start turntable;
start color;
start touch;
}
task touch ()
{
while (true)
{
if (Message ()==7) //待機
{
SendMessage (250); //応答
Wait (10);
until (SENSOR_2==1); //タッチセンサーが当たるまで待機
//ここからメッセージ送信・応答があるまでループ
do
{
SendMessage (4);
ClearMessage ();
Wait (20);
}
while (Message ()!=249);
//ここまで
ClearMessage (); //メッセージリセット
Wait (50);
}
}
}
task arm ()
{
while (true)
{
until ((Message ()==1 || Message ()==2) || Message ()==3); //待機
//ここから応答。エラー防止のため３回送る。
repeat (3)
{
SendMessage (254);
Wait (5);
}
//ここまで
if (Message ()==1) OnRev (OUT_C); //アームを上げる
if (Message ()==2) OnFwd (OUT_C); //パックを挟む
if (Message ()==3) Off (OUT_C); //アーム電源Off
ClearMessage (); //メッセージリセット
}
}
task turntable ()
{
while (true)
{
until (Message ()==4 || Message ()==5); //待機
//ここから応答
repeat (3)
{
SendMessage (253);
Wait (5);
}
//ここまで
if (Message ()==4) OnFwd (OUT_A); //アームを車体後方へ移動
if (Message ()==5) OnRev (OUT_A); //アームを車体側方に移動
ClearMessage (); //メッセージリセット
}
}
task color () //パックの色判定
{
while (true)
{
until (Message ()==6); //待機
//ここから応答
repeat (3)
{
SendMessage (252);
Wait (5);
}
//ここまで
//ここから色判定により送信メッセージ決定
if (SENSOR_1<=34) //黒のとき
{
m=3;
}
else
{
if (SENSOR_1>=44) //白のとき
{
m=1;
}
else //緑のとき
{
m=2;
}
}
//ここまで
//ここからメッセージ送信・応答があるまでループ
do
{
SendMessage (m);
ClearMessage ();
Wait (20);
}
while (Message ()!=255);
//ここまで
ClearMessage (); //メッセージリセット
}
}
ロボコン用プログラム親機
int c=40; //光センサー感度
int c1=47; //なぜか値がおかしくなっている光センサー感度
int t=0; //交差点通過回数
int tur=0; //task turn () 起動中マーカー
int total=0; //運んだ箱の数
int goback=0; //箱を運んだ帰りマーカー
int trans=0; //task transfer () 起動中マーカー
int w=0; //輸送中の箱の色
int g=0; //輸送中の箱の色
int b=0; //輸送中の箱の色
int noturn=0; //交差点で停止マーカー
int m=0; //メッセージ番号
int re=0; //応答メッセージ番号
int hal=0; //三個運び終わったマーカー
int t1=0; //曲がる交差点の番号
int t2=0; //曲がる交差点の番号
int t3=0; //曲がる交差点の番号
int t4=0; //曲がる交差点の番号
int t5=0; //曲がる交差点の番号
int t6=0; //曲がる交差点の番号
task transfer () //パック輸送用ライントレースタスク
{
trans=1; //パック輸送用ライントレースタスク実行中宣言
t=0; //交差点通過回数リセット
while (true) //ライン挟み込み型ライントレースプログラム
{
if (SENSOR_1< c1 && SENSOR_3<c) //交差点上
{
if (noturn==1) //交差点上で停止命令が出ている
{
Off (OUT_A+OUT_C);
trans=0; //パック輸送用ライントレースタスク実行完了宣言
start sto; //パック輸送用ライントレースタスク停止
}
else
{
start turn; //交差点方向転換タスク起動
Wait (10);
until (tur==0); //交差点方向転換タスク実行完了まで待機
}
}
while (SENSOR_1< c1 && SENSOR_3>=c) //左側のセンサーがライン上
{
OnFwd (OUT_C); //左折
OnRev (OUT_A);
}
while (SENSOR_1>= c1 && SENSOR_3<c) //右側のセンサーがライン上
{
OnFwd (OUT_A); //右折
OnRev (OUT_C);
}
OnFwd (OUT_A+OUT_C); //両方のセンサーがライン脇・直進
}
}
task sto()
{
stop transfer;
}
task turn () //交差点方向転換タスク
{
tur=1; //交差点方向転換タスク実行中宣言
if (((t==t1 || t==t2) || t==t3) || t==t4) //曲がるべき交差点上
{
if (t==t5 || t==t6) //ある方向へ曲がる
{
if (goback==0) //行きのとき
{
tl ();
}
else //帰りのとき
{
tr ();
}
}
else //上とは逆方向に曲がる
{
if (goback==0) //行きのとき
{
tr ();
}
else //帰りのとき
{
tl ();
}
}
if (t==t4 && hal==0) //最後に曲がるべき交差点を通過しており、行きのときまたは運んだ箱が３個以上の帰りのとき
{
trans=0; //パック輸送用ライントレースタスク実行完了宣言
stop transfer; //パック輸送用ライントレースタスク停止
}
t++; //通過交差点数加算
}
else //直進すべき交差点上
{
if (t>t4) //最後に曲がるべき交差点を通過済みのとき
{
trans=0; //パック輸送用ライントレースタスク実行完了宣言
stop transfer; //パック輸送用ライントレースタスク停止
}
else
{
t++; //通過交差点数加算
OnFwd (OUT_A+OUT_C); //交差点通過
until (SENSOR_1>= c1 && SENSOR_3>=c);
}
}
tur=0; //交差点方向転換タスク実行完了宣言
}
sub tl () //交差点での左旋回
{
SetPower (OUT_A,1); //感知方法上の微調整
OnFwd (OUT_C);
OnRev (OUT_A);
until (SENSOR_1>= c1 && SENSOR_3>=c); //交差点を曲がりきるまで
SetPower (OUT_A,OUT_FULL);
}
sub tr () //交差点での右旋回
{
SetPower (OUT_C,1);
OnFwd (OUT_A);
OnRev (OUT_C);
until (SENSOR_1>= c1 && SENSOR_3>=c);
SetPower (OUT_C,OUT_FULL);
}
task back () //後退用ライントレースタスク
{
while (true) //センサー１個モーター２個型ライントレースプログラム
{
while (SENSOR_2>=c) //センサーがライン外
{
OnRev (OUT_A); //左折してライン上へ
Off(OUT_C);
}
OnRev (OUT_C); //センサーがライン上・右折してライン外へ
Off (OUT_A);
}
}
sub sm () //メッセージ送信ルーチン
{
//応答メッセージ(re)を受信するまでメッセージ(m)を送信し続ける
do
{
SendMessage (m);
ClearMessage ();
Wait (20);
}
while (Message ()!=re);
}
task color () //色判定および旋回交差点決定タスク
{
//ここから子機の色判定タスク起動メッセージ送信
m=6;
re=252;
sm ();
ClearMessage ();
//ここまで
until ((Message ()==1 || Message ()==2) || Message ()==3); //子機から色情報を受け取るまで待機
//ここから応答。エラー防止のため３回送る。
repeat (3)
{
SendMessage (255);
Wait (5);
}
//ここまで
//色・運んだ箱の数によりどこの角で曲がるかを決定
if (Message ()==1 && total<3 ) //箱が白で運んだ数が4個未満のとき
{
w=1; //白を輸送中。帰りにこの値を利用。
t1=0;
t2=0;
t3=0;
t4=2;
t5=5;
t6=5;
}
if (Message ()==2 && total<3 ) //箱が緑で運んだ数が4個未満のとき
{
g=1;
t1=0;
t2=0;
t3=0;
t4=1;
t5=5;
t6=5;
}
if (Message ()==3 && total<3 ) //箱が黒で運んだ数が4個未満のとき
{
b=1;
t1=0;
t2=0;
t3=0;
t4=3;
t5=5;
t6=5;
}
if (Message ()==1 && total>=3) //箱が白で運んだ数が4個以上のとき
{
w=1;
t1=0;
t2=2;
t3=3;
t4=5;
t5=3;
t6=3;
}
if (Message ()==2 && total>=3) //箱が緑で運んだ数が4個以上のとき
{
g=1;
t1=0;
t2=2;
t3=3;
t4=4;
t5=3;
t6=3;
}
if (Message ()==3 && total>=3) //箱が黒で運んだ数が4個以上のとき
{
b=1;
t1=0;
t2=2;
t3=3;
t4=6;
t5=3;
t6=3;
}
if (total<=1) //メインタスクで使った誤摩化しの辻褄合わせ
{
t4++;
t3=1;
t5=1;
t6=1;
}
start transfer; //パック輸送用ライントレースタスク起動
}
task revert () //パックを運び終わった後再びパックをとりにいくときにどこの角で曲がるかを決定
{
if (g==1 && total<3) //緑のパックを運び終わっていて、運んだパックが４個未満のとき
{
g=0; //クリア
t1=1;
t2=1;
t3=1;
t4=1;
t5=1;
t6=1;
hal=1; //停止場所調整
}
if (b==1 && total<3) //黒のパックを運び終わっていて、運んだパックが４個未満のとき
{
b=0;
t1=3;
t2=3;
t3=3;
t4=3;
t5=3;
t6=3;
hal=1;
}
if (w==1 && total<3) //白のパックを運び終わっていて、運んだパックが４個未満のとき
{
w=0;
t1=2;
t2=2;
t3=2;
t4=2;
t5=2;
t6=2;
hal=1;
}
if (g==1 && total>=3) //緑のパックを運び終わっていて、運んだパックが４個以上のとき
{
g=0;
t1=1;
t2=1;
t3=2;
t4=4;
t5=1;
t6=4;
hal=0;
}
if (b==1 && total>=3) //黒のパックを運び終わっていて、運んだパックが４個以上のとき
{
b=0;
t1=3;
t2=3;
t3=4;
t4=6;
t5=3;
t6=6;
hal=0;
}
if (w==1 && total>=3) //白のパックを運び終わっていて、運んだパックが４個以上のとき
{
w=0;
t1=2;
t2=2;
t3=3;
t4=5;
t5=2;
t6=5;
hal=0;
}
start transfer; //パック輸送用ライントレースタスク起動
}
task main () 制御用メインタスク
{
SetSensor (SENSOR_1, SENSOR_LIGHT);
SetSensor (SENSOR_2, SENSOR_LIGHT);
SetSensor (SENSOR_3, SENSOR_LIGHT);
repeat (6) //6個のパックを狙うので６回実行
{
//ここから子機にアームを上げるよう指示
m=1;
re=254;
sm ();
//ここまで
Wait (30);
//ここから子機にアームを車体側方へ移動するよう指示
m=5;
re=253;
sm ();
//ここまで
Wait (125);
start back; //ライントレースで後退開始
Wait (30);
//ここから子機にタッチセンサー情報を送るよう指示
m=7;
re=250;
sm ();
//ここまで
until (Message ()==4); //子機のタッチセンサーが押されるまで待機
stop back; //後退を停止
Off (OUT_A+OUT_C);
//ここから子機への応答
repeat (4)
{
SendMessage (249);
Wait (5);
}
//ここまで
Wait(50);
//ここから子機にパックをつかむよう指示
m=2;
re=254;
sm ();
//ここまで
//ここからライントレースプログラムの不調に対応するための誤摩化し
if (total==1)
{
OnFwd (OUT_A+OUT_C);
Wait (110);
Off (OUT_A+ OUT_C);
OnFwd (OUT_C);
OnRev (OUT_A);
Wait (20);
OnFwd (OUT_A+OUT_C);
Wait (500);
OnRev (OUT_C);
Off (OUT_A);
Wait (100);
m=4;
re=253;
sm();
until (SENSOR_2<c);
until (SENSOR_2>=c);
Off (OUT_A+ OUT_C);
OnFwd (OUT_C);
OnRev (OUT_A);
until (SENSOR_3<c);
Off (OUT_A+ OUT_C);
OnFwd (OUT_A);
until (SENSOR_1>= c1 && SENSOR_3>=c);
SetPower (OUT_A,OUT_FULL);
Off (OUT_A+ OUT_C);
}
if (total<1)
{
t1=8;
t2=8;
t3=8;
t4=8;
t5=8;
t6=8;
start transfer;
Wait (370);
stop transfer;
Off (OUT_A+ OUT_C);
Wait (30);
OnRev (OUT_C);
Off (OUT_A);
Wait (110);
m=4;
re=253;
sm();
until (SENSOR_2<c);
until (SENSOR_2>=c);
Off (OUT_A+ OUT_C);
OnFwd (OUT_C);
OnRev (OUT_A);
until (SENSOR_3<c);
Off (OUT_A+ OUT_C);
OnFwd (OUT_A);
until (SENSOR_1>= c1 && SENSOR_3>=c);
SetPower (OUT_A,OUT_FULL);
Off (OUT_A+ OUT_C);
}
//ここまで
start color; //色判定および旋回交差点決定タスク起動
//ここから子機にアームを車体後方に移動するよう指示
if (total>1)
{
m=4;
re=253;
sm();
}
//ここまで
Wait (100);
until (trans==0); //パック輸送用ライントレースタスク実行完了まで待機
//ここからパック輸送用ライントレースタスク実行・今回は普通のライントレースでかごの向こう側まで前進
start transfer;
Wait (410);
stop transfer;
Off (OUT_A+OUT_C);
//ここまで
//ここから子機にアームを車体側方に移動するよう指示
m=5;
re=253;
sm ();
//ここまで
//ここからかごの上まで後退
OnRev (OUT_A+OUT_C);
Wait (100);
Off (OUT_A+OUT_C);
//ここまで
//ここから子機にパックを離すよう指示
m=1;
re=254;
sm ();
//ここまで
//ここからパック輸送用ライントレースタスク実行・今回は普通のライントレースでかごの向こう側まで前進
noturn=1;
start transfer;
Wait (180);
stop transfer;
Off (OUT_A+OUT_C);
noturn=0;
//ここまで
//ここから子機にアームを車体後方に移動するよう指示
m=4;
re=253;
sm();
//ここまで
//ここから後退用ライントレース起動。パック輸送用ライントレースタスクを起動しやすい位置まで移動
start back;
Wait (1050);
stop back;
Off (OUT_A+OUT_C);
//ここまで
goback=1; //今から帰りだと宣言
start revert; //帰りのライントレース開始
Wait (500);
//ここから子機にアームの電源を切るよう指示
m=3;
re=254;
sm ();
//ここまで
until (trans==0); //パック輸送用ライントレースタスク実行完了まで待機
//ここから次のライントレースがやりやすいように場所を調整
OnFwd (OUT_C);
OnRev (OUT_A);
until (SENSOR_3<c);
Off (OUT_A+ OUT_C);
OnFwd (OUT_A);
until (SENSOR_1>= c1 && SENSOR_3>=c);
Off (OUT_A+OUT_C);
if (SENSOR_2>=c)
{
OnFwd (OUT_A);
OnRev (OUT_C);
Wait (30);
Off (OUT_A+ OUT_C);
}
//ここまで
hal=0; //初期化
goback=0; //行きだと宣言
total++; //運んだ箱数加算
}
}
このプログラム全体の特徴としては子機の動きをすべて親機で制御しているため、通信ミスがおこらないよう送信メッセージに対し、必ず応答メッセージを返すようにしてあります。
子機は４つのタスクを同時に起動し、常に親機からメッセージが送られてくるのを待つようにしてあります。子機が担当しているのは、
・アームの上げ下げ
・アーム位置の移動
・パックの色判定
・パックに触れたことを知らせるタッチセンサー
の４つです。上二つは親機から指示を受けるとその通りにアームを動かすだけものですが、した二つは子機に搭載されたセンサーの値を判断し、その結果を親機に送る機能がついています。
この中でも特筆すべきはタッチセンサーのタスクです。このタスクによって送られてくるメッセージによって親機は後退のタスクを停止させます。
通常は前進後退の制御はそれを担当するRCXにタッチセンサーを取り付けた方が圧倒的に簡単かつ反応も早いのですが、私たちのロボットは前進後退の制御をしている親機にライントレース用の光センサーを前進用に２つ、後退用に１つ使っているため、タッチセンサー用のポートが空いておらずやむを得ず子機から情報を転送する形をとりました。
極力転送によるディレイを減らすためにこの部分だけは親機からの起動指令に対する応答を１回のみにし、子機からタッチセンサーが押されたというメッセージを受け取った親機がまず後退を止めてから応答するようメッセージ送信方法を変えています。その結果それほど遅れることなく後退を停止することができるようになりました。
親機は子機の制御と前進後退の制御を行っています。
同じタスクですべてのパターンに対応できるように変数を多用する形になりました。
基本となるライントレースタスクは前進用がtransfer、後退用がbackです。
transferで、交差点に到達すると交差点用のタスクturnが起動しうまく曲がれるようになっています。
どの交差点で曲がるかはすべて変数でコントロールしており、その変数を決定するタスクがcolor(行き)とrevert(帰り)です。
colorは子機にパックの色情報を送るよう指示し、送られてきた色情報をもとに曲がる場所の変数を決定します。このときrevertのために今何色を運んでいるかの情報を残しておきます。
revertはcolorの残した情報をもとに帰りに曲がる場所を指定します。
あとはそれらのタスクをメインタスクでコントロールするという形になっています。
テスト終了後からできる限り時間を割いてプログラムしてきましたが、時間が足りず、turnとcolorタスクにバグが残ったままとなってしまい、メインタスク内で無理矢理誤摩化すという方法をとらざるを得なくなってしまいました。優勝したとはいえ非常に心残りです。
基本的に制御はできる限りセンサーからの情報にたより時間を指定した制御はしないようにしよう(時間制御だと電池残量により値を修正しなくてはならないため)という目標があったのですが、誤摩化した部分とかごに入れる部分以外はセンサーだけで制御することができある程度満足しています。
RCX同士の通信はタッチセンサーの部分など、ほぼ完璧にできていると思うので満足しています。
人によってプログラムの癖が違い、整合性をとるのが面倒だと思ったのですべて一人でプログラムしたのですが、バグフィクスの際に他の人からのアドバイスがもらいにくいというデメリットがありました。時間に余裕があるようならば一人がベースとなり部分を作りもう一人と一緒にチェックおよび修正していった方がうまくいくのではないかと思います。
