[[2007a/A9/ロボコン]]
#contents
*全体のプログラム [#ie309eac]
**上のRCXのプログラム [#a1a88021]
#define TURN_TIME 237 //180度曲がるのに要する大体の時間
#define TURN_TIME2 110 //90度曲がるのに要する大体の時間
#define COLLECT_TIME1 897 //1番上の箱の回収に要する時間
#define COLLECT_TIME 667 //その他の箱の回収に要する時間
#define BACK_TIME 30 //回転の時に箱をまきこまないように後退する時間
#define BACK_TIME2 130 //搭載部分を箱側にむけたあと、回収するための後退時間
#define TURN_MOVE 70 //90度まがるときに、回転の中心を交差点上にもってくる時間
task main()
{
ClearMessage();
while(true){
if(Message()==1){ //1のメッセージを受信したら
OnRev(OUT_C);Wait(90);Off(OUT_C); //後ろの腕？を開く
Wait(BACK_TIME+BACK_TIME2+TURN_TIME-90); //BACK_TIME＋TURN_TIME+BACK_TIME2-90
OnFwd(OUT_A+OUT_C);Wait(160);Off(OUT_C);
Wait(100);
Off(OUT_A);
OnRev(OUT_C);Wait(90);Off(OUT_C);
Wait(120);
OnFwd(OUT_A+OUT_C);Wait(160);Off(OUT_C);
Wait(110);Off(OUT_A+OUT_C);
ClearMessage();
}if(Message()==2){ //2のメッセージを受信したら
OnRev(OUT_C+OUT_B); Wait(170); //BCを開いて箱を離す
Off(OUT_C+OUT_B); //本体が箱を積んだ地点から離れるのを待つ
Wait(250);
OnFwd(OUT_C+OUT_B);Wait(70);Off(OUT_C); //離れたので、腕？を閉じる
Wait(80);Off(OUT_B);
ClearMessage();
}if(Message()==3){
OnRev(OUT_B+OUT_C);Wait(90);Off(OUT_C+OUT_B); //後ろの腕？を広げる
Wait(BACK_TIME+BACK_TIME2+TURN_TIME-90); //BACK_TIME＋BACK_TIME2＋TURN_TIME-90
OnFwd(OUT_A+OUT_C);Wait(70);
OnFwd(OUT_B+OUT_A);Wait(40);Off(OUT_C);
Wait(40);Off(OUT_B);
Wait(250);
Off(OUT_A);
ClearMessage();
}if(Message()==4){
OnFwd(OUT_A+OUT_C);Wait(70);
OnFwd(OUT_B+OUT_A);Wait(40);Off(OUT_C);
Wait(40);Off(OUT_B);
Wait(250);
Off(OUT_A);
ClearMessage();
}
}
}
**下のRCXのプログラム [#tf3f9da5]
プログラム中の説明の座標の位置は↓↓で確認してください。
#ref(コース２.JPG)&br;
-移動コース&br;
(2,0)スタート→(1,0)の緑の箱を回収→(3,0)の緑の箱を回収→(4,0)の白の箱を回収&br;
(2,0)に戻って箱を置く→(3,0)経由で２つめの三段積みの箱集めに→&br;
(3,2)の緑の箱を回収→(1,2)の緑の箱を回収→(0,2)の白の箱を回収→&br;
(2,2)に戻って箱を置く＝終了！！&br;
-プログラム
/*ACは逆回転で前進し、Bは箱の上下に使うモーター*/
#define MOTOR OUT_A+OUT_C
#define THRESHOLD 40 //閾値
#define TURN_TIME 237 //180
#define TURN_TIME2 110 //90
#define COLLECT_TIME1 897 //1番上の箱の回収に要する時間
#define COLLECT_TIME 667 //その他の箱の回収に要する時間
#define BACK_TIME 30 //回転の時に箱をまきこまないように後退する時間
#define BACK_TIME2 130 //搭載部分を箱側にむけたあと、回収するための後退時間
#define BACK_TIME3 50 //箱を回収したあと、次の箱をまきこまないようにずらす時間
#define TURN_MOVE 70 //90度まがるときに、回転の中心を交差点上にもってくる時間
int search_dir=0;
int cross_count=0;
int turn_count;
int field_color; //白1、黒0
int trace_count;
int turn_count2;
sub turn_left()
{
OnRev(OUT_A);OnFwd(OUT_C);
}
sub turn_right()
{
OnFwd(OUT_A);OnRev(OUT_C);
}
-線を踏んだ数を数えて右折するtask（プログラムの説明は、[[ロボコンのページ>2007a/A9/ロボコン]]の「方向転換について」の部分をご覧下さい。）
sub turn_right2() //説明はロボコンのページの「方向転換について」をご覧ください・・・。
{
turn_count=0;
field_color=0;
while(turn_count<turn_count2){
OnFwd(OUT_A);OnRev(OUT_C);
if(field_color==0){ //最終色が黒
if(SENSOR_1>THRESHOLD){
field_color=1;
}else{
field_color=0;
}
}if(field_color==1){ //最終色が白
if(SENSOR_1>THRESHOLD){
field_color=1;
}if(SENSOR_1<THRESHOLD){
turn_count++;
field_color=0;
}
}
}
Off(OUT_A+OUT_C);
}
-線を踏んだ数を数えて左折するtask
sub turn_left2()
{
turn_count=0;
field_color=0;
while(turn_count<turn_count2){
OnFwd(OUT_C);OnRev(OUT_A);
if(field_color==0){ //最終色が黒
if(SENSOR_1>THRESHOLD){
field_color=1;
}else{
field_color=0;
}
}if(field_color==1){ //最終色が白
if(SENSOR_1>THRESHOLD){
field_color=1;
}if(SENSOR_1<THRESHOLD){
turn_count++;
field_color=0;
}
}
}
Off(OUT_A+OUT_C);
}
task main()
{
SetSensor(SENSOR_1,SENSOR_LIGHT);
SetSensor(SENSOR_2,SENSOR_LIGHT);
SetSensor(SENSOR_3,SENSOR_TOUCH);
ClearMessage();
OnFwd(MOTOR);Wait(100);Off(MOTOR);
SendMessage(4);
OnFwd(OUT_B);Wait(400);Off(OUT_B); //1個めの箱(1,0)回収終了
trace_count=10; start line_trace; //トレースで、交差点の数でとまらなくて良いときはtrace_countを１０に設定（もちろん、もっと多くても良い）
until(SENSOR_3==1); //(3.0)の緑の箱を認識
stop line_trace;
ClearTimer(0);
start box_collect; //箱の回収開始
until(FastTimer(0)>COLLECT_TIME); //箱の回収が終了したら
stop box_collect;
ClearTimer(0);
start line_trace2;
until(FastTimer(0)>200); stop line_trace2; Wait(270);
cross_count=0; //交差点の数を数えて円に向かうので、カウントリセット
ClearTimer(0);
trace_count=5;
start line_trace; //円に向かう
until(FastTimer(0)>600);
stop line_trace;
OnRev(MOTOR); Wait(TURN_MOVE+35); //90度曲がるので、回転の中心を交差点に近づける
turn_count2=1; turn_left2(); //90度左折して円の側に後ろを向ける。
OnFwd(MOTOR); Wait(50); Off(MOTOR); //円の中に確実に置けるように後退させる
ClearMessage();
SendMessage(2); //箱を離してください（通信）
Wait(300); //箱を離して一端停止ClearTimer(0);
trace_count=1;
start line_trace; //回転の中心を線上(2,0)にもってこれるように前進。
until(FastTimer(0)>200);
stop line_trace; turn_count2=1; turn_left(); //90度左折
cross_count=0; //1つめの交差点を目指す
trace_count=2;
start line_trace;
Wait(130);
stop line_trace; //1つ目の交差点(3,0)にさしかかる
OnRev(MOTOR); Wait(TURN_MOVE); //90度方向転換するので回転の中心を交差点に近づける
turn_count2=1; turn_left(); //90度左折して(3,2)に向かう
trace_count=10;
start line_trace;
until(SENSOR_3==1); //1つ目の(3,2)緑の箱を感知
ClearTimer(0);
stop line_trace;
start box_collect;
until(FastTimer(0)>COLLECT_TIME); //箱を認識してから回収が終了
stop box_collect;ClearTimer(0);
start line_trace2;
until(FastTimer(0)>200);
stop line_trace2; Wait(270);
cross_count=0; //交差点の数を数えて円に向かうので、カウントリセット
ClearTimer(0);
trace_count=5;
start line_trace; //円に向かう
until(FastTimer(0)>600);
stop line_trace;
OnRev(MOTOR); Wait(TURN_MOVE+35); //90度曲がるので、回転の中心を交差点に近づける
turn_count2=1; turn_left(); //90度左折して円の側に後ろを向ける。
OnFwd(MOTOR); Wait(50); Off(MOTOR); //円の中に確実に置けるように後退させる
ClearMessage();
SendMessage(2); //箱を離してください（通信）
Wait(300); //箱を離して一端停止
ClearTimer(0);
trace_count=2;
start line_trace; //回転の中心を線上(2,0)にもってこれるように前進。
until(FastTimer(0)>130);
stop line_trace;Off(OUT_A+OUT_B+OUT_C);
}
task box_collect() //ライントレースを停止して箱を回収する
{
ClearMessage();
stop line_trace;
SendMessage(1); //上のRCXに通信。箱の回収
OnFwd(MOTOR); Wait(BACK_TIME); Off(MOTOR); //トレースを停止して後退
turn_count2=2; turn_right(); //180度右回転
OnFwd(MOTOR); Wait(BACK_TIME2); Off(MOTOR); Wait(270); //上のBが開いた状態で後退してもう一方のRCXのほうの回収時間の経過を待つ。
}
task box_collect1() //ライントレースを停止して箱を回収する(一番上専用）
{
ClearMessage();
stop line_trace;
SendMessage(3); //上のRCXに通信。箱の回収
OnFwd(MOTOR); Wait(BACK_TIME); Off(MOTOR); //トレースを停止して後退
turn_count2=2; turn_right(); //180度右回転
OnFwd(MOTOR); Wait(BACK_TIME2); Off(MOTOR); //上のBが開いた状態で後退
OnRev(OUT_B);Wait(400); Off(OUT_B); //モーターBを動かす時間（4）秒
}
-ライントレースをしながら後退するtask（プログラムの説明については[[ライントレースのページ>2007a/A9/ロボコン/ライントレース]]をご覧下さい。）
task line_trace2() //バックライントレース
{
SetSensor(SENSOR_1,SENSOR_LIGHT);
SetSensor(SENSOR_2,SENSOR_LIGHT);OnFwd(MOTOR); //最初の動きとして直進させる。
while(true){
if(SENSOR_1<THRESHOLD) //線を見失ったら
{
if(search_dir==0){
turn_right(); Wait(5);
if(SENSOR_1>THRESHOLD){ //線をみつけたのなら直進
OnFwd(MOTOR);
search_dir=0;
}else{ //見付からないのなら反対側を探す
turn_left(); Wait(10);
if(SENSOR_1>THRESHOLD){ //見付けたのなら直進
OnFwd(MOTOR);
search_dir=1;
}else{ //見付からないのなら1秒前進
OnFwd(MOTOR);Wait(100);
search_dir=0;
}
}
}
if(search_dir==1){
turn_left(); Wait(5);
if(SENSOR_1>THRESHOLD){
OnFwd(MOTOR);
search_dir=1;
}else{
turn_right(); Wait(10);
if(SENSOR_1>THRESHOLD){
OnFwd(MOTOR);
search_dir=0;
}else{
OnFwd(MOTOR); Wait(100);
search_dir=1;
}
}
}
}
}
Off(OUT_A+OUT_C);
}
-ライントレースをするtask
task line_trace() //ライントレース
{
cross_count=0; OnRev(MOTOR); //最初の動きとして直進させる。
while(cross_count<trace_count){
if(SENSOR_2<THRESHOLD) {
cross_count++;
if(SENSOR_1<THRESHOLD) //線を見失ったら
{
if(search_dir==0){
turn_right(); Wait(5);
if(SENSOR_1>THRESHOLD){ //線をみつけたのなら直進
OnRev(MOTOR);
search_dir=0;
}else{ //見付からないのなら反対側を探す
turn_left(); Wait(10);
if(SENSOR_1>THRESHOLD){ //見付けたのなら直進
OnRev(MOTOR);
search_dir=1;
}else{ //見付からないのなら1秒前進
OnRev(MOTOR);Wait(100);
search_dir=0;
}
}
}
if(search_dir==1){
turn_left(); Wait(5);
if(SENSOR_1>THRESHOLD){
OnRev(MOTOR);
search_dir=1;
}else{
turn_right(); Wait(10);
if(SENSOR_1>THRESHOLD){
OnRev(MOTOR);
search_dir=0;
}else{
OnRev(MOTOR); Wait(100);
search_dir=1;
}
}
}
}
}else{
if(SENSOR_1<THRESHOLD) //線を見失ったら
{
if(search_dir==0){
turn_right(); Wait(5);
if(SENSOR_1>THRESHOLD){ //線をみつけたのなら直進
OnRev(MOTOR);
search_dir=0;
}else{ //見付からないのなら反対側を探す
turn_left(); Wait(10);
if(SENSOR_1>THRESHOLD){ //見付けたのなら直進
OnRev(MOTOR);
search_dir=1;
}else{ //見付からないのなら1秒前進
OnRev(MOTOR);Wait(100);
search_dir=0;
}
}
}
if(search_dir==1){
turn_left(); Wait(5);
if(SENSOR_1>THRESHOLD){
OnRev(MOTOR);
search_dir=1;
}else{
turn_right(); Wait(10);
if(SENSOR_1>THRESHOLD){
OnRev(MOTOR);
search_dir=0;
}else{
OnRev(MOTOR); Wait(100);
search_dir=1;
}
}
}
}
}
}
Off(OUT_A+OUT_C);
}
***コメントをどうぞ〜[#ud98bb0f]
#comment
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
