課題2で作成したコースを使用してC地点,D地点に置かれた紙コップをQ地点の円の中に置き、その数や重ねた個数を競う。
詳しくは課題3より
機体の性能上、紙コップをすべて重ねることができ、最高得点を狙うことが出来ましたがコップが倒れたり置いてある場所がずれてしまうのが原因で失敗する可能性が高かったため、すべての紙コップを重ねるのをあきらめました。
紙コップ4つまではほぼ確実に回収できていたので紙コップを4つまで重ね、残り2つは放置する作戦にしました。
マイコンは1つだけを使用しました。
ライントレース用の光センサーをマイコンの真下に設置しました。(赤丸部分)
これにより、前方に紙コップを置くスペース(青丸部分)を確保することができました。
紙コップ認識用の光センサーです。(赤丸部分)
紙コップの色(白色)を判別します。
前方の2本の棒(赤丸部分)で紙コップを挟んで回収します。
↓
アームを上げて回収済み紙コップのストッパーとしての機能もあります。 基本的にはアームを上げた状態で移動します。
アームを回転させる為のギア部分に余計な力が掛からないように空回りするようにしました。
紙コップがアームの奥まで入ってしまうとうまく重ならないことや、光センサーが反応しないことがあったので、奥まで入らないようにした。(赤丸部分)
また、アームの軽量化のためにブロックから棒状の部品に変更した。(青丸部分)
#define CROSS_TIME 40 #define BALANCE_TIME 40
黒部分がCROSS_TIMEより続いたら交差点として判別する。
境界部分がBALANCE_TIMEより続いたら機体がまっすぐとして判別する。
基本動作のマクロです。
#define roll_left OnRev(OUT_A); OnFwd(OUT_C); //左回転 #define turn_left OnFwd(OUT_C); Off(OUT_A); //左旋回 #define go OnFwd(OUT_A); OnFwd(OUT_C); //前進 #define turn_right OnFwd(OUT_A); Off(OUT_C); //右旋回 #define roll_right OnFwd(OUT_A); OnRev(OUT_C); //右回転 #define back OnRev(OUT_A); OnRev(OUT_C); Wait(100); Off(OUT_AC); //後進
交差点判別後のマクロです。
#define top OnRev(OUT_C); OnFwd(OUT_A); Wait(CROSS_TIME); OnFwd(OUT_AC); Wait(10); #define left OnRev(OUT_A); OnFwd(OUT_C); Wait(20); #define right OnRev(OUT_C); OnFwd(OUT_A); Wait(80); OnFwd(OUT_AC); Wait(20);
角度を戻してそのまま直進する。
角度を戻してそのままライントレースさせる。
角度を右寄りにして外側の線に機体を移動する。
今回の課題では正確に各地点へ移動するために、左の境界部分のライントレースプログラムを作成した。
光センサーが黒部分の判定をしている時にカウントをして、0.4秒以上続いたら交差点として判別する。また、黒部分以外の時はClearTimer(0)でFastTimer(0)を初期化する。
小さなカーブを交差点として誤判別しないために指定秒数間交差点判別を行わないようにする。ここで、指定秒数を変数としてl_timeをグローバル変数として定義する。また、ライントレース開始時にClearTimer(1)でFastTimer(1)を初期化する。
sub line_l(){ int ch=0; //ループ脱出用変数 ClearTimer(0); //交差点判別用のタイマー初期化 ClearTimer(1); //交差点誤判別防止用のタイマー初期化 while(ch==0) //chが0の時はループを続ける { if(SENSOR_2 > 53) //白部分 右回転 { ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化 roll_right; } else if(SENSOR_2 > 52) //弱白部分 右旋回 { ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化 turn_right; } else if(SENSOR_2 > 44) //境界部分 直進 { ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化 go; } else if(SENSOR_2 > 41) //弱黒部分 左旋回 { ClearTimer(0); //黒部分ではないので交差点判別用のタイマーを初期化 turn_left; } else{ //黒部分 左回転 roll_left; } if(FastTimer(0)>CROSS_TIME&&FastTimer(1)>l_time){ //交差点判別 Off(OUT_AC); //車輪を止める PlaySound(SOUND_UP); //音を鳴らす Wait(100); //1秒待つ ch=1; //ループ脱出の為にchを1にする } Wait(1); //0.01秒待つ } }
上記のライントレース用プログラム内の交差点判別部分について詳しく説明する。
if(FastTimer(0)>CROSS_TIME&&FastTimer(1)>l_time) //交差点判別 { Off(OUT_AC); //車輪を止める PlaySound(SOUND_UP); //音を鳴らす Wait(100); //1秒待つ ch=1; //ループ脱出の為にchを1にする }
交差点は黒部分が長く続いた時に交差点として判別するようにした。そこでタイマーを使い黒部分以外の時は以下の関数でタイマーを初期化するようにした。また交差点と判別したことが分かるように、判別したら音を鳴らすようにした。
ClearTimer(0);
タイマーがCROSS_TIMEより長い時間が経過した時に交差点として判別する。CROSS_TIMEは40とする。
if(FastTimer(0)>CROSS_TIME)
しかし、上記のプログラムではカーブを交差点として誤判別してしまう事があるので、それを防ぐためにタイマーをもう一つ追加して次の指定秒数をカウントしなければ交差点と判別しないようにした。
if(FastTimer(0)>CROSS_TIME&&FastTimer(1)>l_time)
l_timeを上記の指定秒数として、メインプログラムで指定する。
C,D地点に置いた紙コップを正確に取るために、ラインを使い機体の姿勢をまっすぐにするプログラムを作成しました。
光センサーが境界部分の判定をしている時にカウントをして、タイマーがBALANCE_TIME以上になったら機体の姿勢をまっすぐとして判別する。また、境界部分以外の時はClearTimer(0)でFastTimer(0)を初期化する。
ライントレースをする時間を指定しなければ、姿勢がまっすぐにならない場合ライントレースをし続けてしまい、C,D地点に置いてある紙コップをとることが出来なくなってしまうので追加しました。また、姿勢制御プログラム開始時にClearTimer(1)でFastTimer(1)を初期化する。
姿勢制御プログラムは、交差点判別プログラムを応用して作成しました。
交差点判別プログラムとの違いは、黒部分の判定ではなく境界部分の判定であることです。
また、ラインの長さは限られているので、一定時間が経過しても姿勢がまっすぐにならなければ、一度処理を止めて後退してからまた姿勢制御プログラムを開始するようにしました。
このプログラムにより、C,D地点にますっぐ侵入することが出来るようになりました。
sub balance_l(){ int ch=0; //ループ脱出用変数 ClearTimer(0); //姿勢判別用のタイマー初期化 while(ch==0) //chが0の時はループを続ける { ClearTimer(1); //ライントレース時間判別用のタイマー初期化 while(FastTimer(1)<200&&ch==0) //FastTimer(1)が2秒未満かつchが0の時はループを続ける { if(SENSOR_2 > 53) //白部分 右回転 { ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化 roll_right; } else if(SENSOR_2 > 52) //弱白部分 右旋回 { ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化 turn_right; } else if(SENSOR_2 > 44) //境界部分 直進 { go; } else if(SENSOR_2 > 41) //弱黒部分 左旋回 { ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化 turn_left; } else{ //黒部分 左回転 ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化 roll_left; } if(FastTimer(0)>BALANCE_TIME) //姿勢判別 { Off(OUT_AC); //車輪を止める PlaySound(SOUND_UP); //音を鳴らす Wait(100); //1秒待つ ch=1; //ループ脱出の為にchを1にする } Wait(1); //0.01秒待つ } if(ch==0) { back; //約2秒分の後退 back; } } }
sub balance_r(){ int ch=0; //ループ脱出用変数 ClearTimer(0); //姿勢判別用のタイマー初期化 while(ch==0) //chが0の時はループを続ける { ClearTimer(1); //ライントレース時間判別用のタイマー初期化 while(FastTimer(1)<200&&ch==0)//FastTimer(1)が2秒未満かつchが0の時はループを続ける { if(SENSOR_2 > 53) //白部分 左回転 { ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化 roll_left; } else if(SENSOR_2 > 52) //弱白部分 左旋回 { ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化 turn_left; } else if(SENSOR_2 > 44) //境界部分 直進 { go; } else if(SENSOR_2 > 41) //弱黒部分 右旋回 { ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化 turn_right; } else{ //黒部分 右回転 ClearTimer(0); //境界部分ではないので姿勢判別用のタイマーを初期化 roll_right; } if(FastTimer(0)>BALANCE_TIME) //姿勢判別 { Off(OUT_AC); //車輪を止める PlaySound(SOUND_UP); //音を鳴らす Wait(100); //1秒待つ ch=1; //ループ脱出の為にchを1にする } Wait(1); //0.01秒待つ } if(ch==0) { back; //約2秒分の後退 back; } } }
上記の姿勢制御プログラム内の姿勢判別部分について詳しく説明する。
if(FastTimer(0)>BALANCE_TIME) //姿勢判別 { Off(OUT_AC); //車輪を止める PlaySound(SOUND_UP); //音を鳴らす Wait(100); //1秒待つ ch=1; //ループ脱出の為にchを1にする }
このプログラムでは、境界部分が長く続いた時に機体がますっぐであると判別するようにした。
そこでタイマーを使い境界部分以外の時は以下の関数でタイマーを初期化するようにした。また機体がますっぐであると判別したことが分かるように、判別したら音を鳴らすようにした。
ClearTimer(0);
タイマーがBALANCE_TIME(0.4秒)より長い時間が経過した時に機体がますっぐであると判別する。
if(FastTimer(0)>BALANCE_TIME)
また、機体が一定時間(2秒)経過してもまっすぐにならない場合、一度後退してから姿勢制御プログラムを再開する。
if(ch==0) //姿勢がまっすぐと判別された場合は後退しない。 { back; //約2秒分の後退 back; }
今回の課題で最も重要である紙コップの取得プログラムです。
コップを二つのアームの間に挟んでからアームを上げることによって紙コップを持ち上げて機体の手前に置くことが出来ます。同じ動作で最大8つまでの紙コップを重ねることが出来ます。
光センサーを使い紙コップを認識したら前進を停止します。
紙コップを二つのアームの間に入れなくてはいけないので、上記の姿勢制御プログラムが重要です。
sub cup(){ OnRev(OUT_B); //アームを下す Wait(50); Off(OUT_B); OnFwd(OUT_AC); //前進 while(SENSOR_1<42); //光センサーが紙コップを認識するまで前進する Off(OUT_AC); //停止 OnFwd(OUT_B); //アームを上げる Wait(50); Off(OUT_B); }
C地点の紙コップを取得後、左回転し右境界部分で姿勢制御を開始する。
赤い点:光センサー
D地点の紙コップを取得後、後退をし交差点を超えてから左回転し右境界部分で姿勢制御を開始する。
赤い点:光センサー
また右回転ではなく左回転をする理由は、左側にはアームだけしかストッパーがなく紙コップが転倒する可能性が上がるたので、左回転にした。
sub turn(){ OnFwd(OUT_C); //左回転 OnRev(OUT_A); Wait(100); //黒部分から白部分に出るための時間(D地点からC地点への移動時に特に必要) while(SENSOR_2>41); //白部分である場合は回転し続ける Off(OUT_AC); //停止 }
光センサーで判別して回転するので、電池の消費による回転不足をなくすことができた。
設置場所の輪の中へ紙コップを置くために、最後の交差点を判別したら以下の処理を実行する。
1:交差点判別後、機体の姿勢を戻さずにそのまま前進する。
2:前進停止後、アームを下して左側のストッパーをなくす。
3:右回転させ、紙コップを外に出す。
4:後進させ、紙コップが機体に触れないようにする。
sub last(){ OnFwd(OUT_AC); //前進 Wait(50); //約0.5秒待つ Off(OUT_AC); OnRev(OUT_B); //アームを下す Wait(50); //約0.5秒待つ Off(OUT_B); OnFwd(OUT_A); //右回転 OnRev(OUT_C); Wait(100); //約1秒待つ back; //約2秒間後進する(1秒ではアームが紙コップに触れている時があるため) back; }
task main () { SetSensor(SENSOR_1, SENSOR_LIGHT); //光センサー1の初期設定(紙コップ認識用) SetSensor(SENSOR_2, SENSOR_LIGHT); //光センサー2の初期設定(ライントレース用) OnFwd(OUT_AC); //スタート地点の枠から脱出 Wait(60); //C地点までの移動 line_l(); //左側境界部分をトレース left; //P地点を左折 l_time=400; //次の交差点までの秒数を4秒に設定 line_l(); //左側境界部分をトレース right; //S地点を右折 balance_l(); //左境界部分での姿勢制御 //紙コップ回収作業 cup(); //1つ目の紙コップ取得 back; //後進 //D地点までの移動 turn(); //左回転 balance_r(); //右境界部分での姿勢制御 //紙コップ回収作業 cup(); //2つ目の紙コップ取得 back; //後進 //C地点までの移動 turn(); //左回転 OnFwd(OUT_AC); //前進 Wait(20); balance_r(); //右境界部分での姿勢制御 //紙コップ回収作業 cup(); //3つ目の紙コップ取得 back; //後進 //D地点までの移動 turn(); //左回転 OnFwd(OUT_AC); //前進 Wait(20); balance_r(); //右境界部分での姿勢制御 //紙コップ回収作業 cup(); //4つ目の紙コップ取得 back; //約4秒間後進 back; back; back; l_time=200; //次の交差点までの秒数を2秒に設定 //左境界部分へ移動 OnFwd(OUT_C); //左回転 OnRev(OUT_A); Wait(50); OnFwd(OUT_AC); //前進 Wait(30); //Q地点までの移動 line_l(); //左側境界部分をトレース left; //S地点を左折 line_l(); //左側境界部分をトレース left; //P地点を左折 line_l(); //左側境界部分をトレース //紙コップの設置 last(); }
紙コップ認識用光センサーの不具合が原因
成功
20点中5.2点でした。1回目の挑戦の基本点が0点だったことと他のチームに比べて機体の機構がシンプルすぎたのが理由として考えられます。
2回目の挑戦で基本点が30点になったことで、合計点数が35.2点になりロボコン優勝という良い結果で終わることが出来ました。
今回の課題では課題1、課題2に比べてロボット本体に工夫することが出来ました。チームメイトと考えて何回か作り直しを行い意見を出し合いながら今回のロボットを完成させることが出来ました。
また今回のプログラムは、課題1のマクロや課題2のサブルーチン、ライントレースを応用した姿勢制御など今までに学んできたものの集大成となりました。
この授業で一番大変であったのは、スケジュール管理でした。特に課題3は一人ではできないので、自分だけではなくチームメイトの予定も考慮しながら作業しなくてはいけなく、授業終わりから夜遅くまで残ることが多かったです。しかし、少ない時間を有効に使い最後のロボコンでは優勝という良い結果で終わることができとてもよかったです。