#contents
*課題 [#b949118c]
今回の課題は、ライントレースをしながら缶を運ぶというものであった。コースは以下の通り。
&ref(2018a/Member/Tomo/Mission2/2018a-mission2'.png,70%,全体像);
*ロボットの構造 [#i4a428f2]
ロボットは資料に載っているものを参考に、余計なパーツをオミットしてそこに缶を捕まえるための機構を取り付けた。
&ref(2018a/Member/Tomo/Mission2/full_picture_2.jpg,70%,全体像);
缶を捕まえる機構は四角い枠を上から被せるかたちとなっている。
&ref(2018a/Member/Tomo/Mission2/catch_function.jpg,70%,缶を捉える機構);
*ライントレースのプログラム [#g8fc9997]
多くの人はラインとの境界線を移動し続ける形でライントレースをしていったと思う。そちらのほうが簡単だとは思うが、あえて私は黒い線上をトレースさせようと試みた。
黒い線上をトレースする動きは、まずその場でロボットを旋回させラインの境界を検知させる。次に逆方向に旋回させて反対側の境界を検知させる。この二つの地点の間の角の二等分線上にロボットが向くように旋回させる。少し進む。以上を繰り返すことでライン上を進む。以上の流れを、境界を検知して角度を割り出す、向きを変える、進む、の３つに分割してサブルーチン化した。それぞれ以下の通りである。
**境界を検知して間の角度を割り出すプログラム [#y0955c9e]
sub check()
{
long ct=CurrentTick(); //この時点での時刻を記録
while(SENSOR_2<w_bordor){
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED); //右側の境界まで旋回
}
Off(OUT_BC);
long ct_2=CurrentTick(); //この時点での時刻を記録
while(SENSOR_2>b_bordor){
OnFwd(OUT_C,C_SPEED);OnRev(OUT_B,C_SPEED); //ライン上まで旋回
}
while(SENSOR_2<w_bordor){
OnFwd(OUT_C,C_SPEED);OnRev(OUT_B,C_SPEED); //左側の境界まで旋回
}
long ct_3=CurrentTick(); //この時点での時刻を記録
Off(OUT_BC);
ct_4=ct_3-ct_2; //ラインの端から端まで旋回するのにかかった時刻を計算
ct_1=ct_4/2-ct_2+ct; //ラインに対する車体の傾きを計算
}
角度は旋回にかかる時間で考えている。二つ目のwhileは、これがないと一つ目のwhileが終了した時点で、反対側の境界まで旋回する三つ目のwhileの条件を満たしてしまい、反対側へ動かなくなってしまう。一度二つ目のwhileでライン上にセンサーを戻すことで解決した。
**ロボットの進行方向を合わせるプログラム [#c6474b30]
sub front()
{
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED);
Wait(ct_4/2);
Off(OUT_BC);
}
境界から反対側の境界へ旋回するのにかかる時間を計測して、その半分の時間ロボットの向きを戻している。
**少しだけ進むプログラム [#kf4d101b]
sub ahead()
{
OnFwd(OUT_BC,SPEED);
Wait(ahead_time);
Off(OUT_BC);
}
[#w29f3248]
また、この３つのサブルーチンのうち「ahead」と「check」はメインプログラム中でよく一緒に使用したので、二つを合わせて１つのサブルーチンとした。
sub sequence()
{
ahead();
check();
}
**缶を捉えるプログラム [#j75555e2]
sub catch(int i)
{
RotateMotor(OUT_A, i, 45);
}
サブルーチン化し角度を変数にすることで、捉える時と放す時を同じサブルーチンで対応できるようにし、メインプログラムでの記述量の削減を図った。
**交差点の判別 [#zc648d92]
交差点の判別にはこのプログラムの特徴ともいえる旋回してラインの端を検知する動きを利用した。まず、一回目の旋回での結果を通常とは異なる変数に保存し、二回目以降の結果と比較して交差点かそうでないかを判断する。交差点では結果が通常よりも大きくなることが予想されるので一回目と二回目以降との差が一定以上となると各交差点での対応のプログラムに移る。
sub check_orig()
{
long ct=CurrentTick(); //この時点での時刻を記録
while(SENSOR_2<w_bordor){
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED); //右側の境界まで旋回
}
Off(OUT_BC);
long ct_2=CurrentTick(); //この時点での時刻を記録
while(SENSOR_2>b_bordor){
OnFwd(OUT_C,C_SPEED);OnRev(OUT_B,C_SPEED); //ライン上まで旋回
}
while(SENSOR_2<w_bordor){
OnFwd(OUT_C,C_SPEED);OnRev(OUT_B,C_SPEED); //左側の境界まで旋回
}
long ct_3=CurrentTick(); //この時点での時刻を記録
Off(OUT_BC);
ct_0=ct_3-ct_2; //ラインの端から端まで旋回するのにかかった時刻を計算
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED); //車体の角度を調整
Wait(ct_0/2);
Off(OUT_BC);
}
最初の旋回のプログラム。基本的には通常の旋回のプログラムと同じだが、旋回にかかる時間を保存する変数が異なる。また、このサブルーチンの中で車体の角度調整まで行っている。
if((ct_4-ct_0<=200)||(t==1)){ //もし結果の差が200以下またはt=1なら
front(); //車体の角度を調整
sequence();PlaySound(SOUND_CLICK); //前進のち旋回時間の計測
t=0;
}else if((ct_4-ct_0>200)&&(s<1)){ //もし結果の差が200より大きくかつs<1なら
PlaySound(SOUND_UP);
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED); //交差点を右折
Wait(ct_4/5);t++;
Off(OUT_BC);
sequence();s++;
メインプログラムでの交差点と直進の一部。これをwhileでループさせて進む。ここで登場する変数sは交差点をカウントする変数で一つの交差点または曲がり角を曲がる、または直進すると1増える。この変数の数値によって各交差点に対応したプログラムに移るように設定している。もう一つの変数tは曲がった直後の動きに関わる変数である。曲がった直後の旋回では、交差点と誤認識することが多発してしまったため、変数tを利用することで旋回による結果によらず直進のプログラムに移るようにした。
以下は定数、または変数
#define SPEED 50 //直進する際の速度
#define C_SPEED 40 //旋回する際の速度
#define w_bordor 30 //白の閾値
#define b_bordor 20 //黒の閾値
long ct_0;
long ct_4;
long ct_1;
int ahead_time=200; //直進する時間
以下、メインのプログラム
task main()
{
SetSensorTouch(S1);
SetSensorLight(S2);
long t0=CurrentTick();
int t=0;
int s=0;
ahead();
check_orig();PlaySound(SOUND_CLICK); //一度目の旋回時間計測
sequence();PlaySound(SOUND_CLICK);
while(s<7){ //６つ目の分岐まで繰り返し
if((ct_4-ct_0<=200)||(t==1)){
front(); //角度調整
sequence();PlaySound(SOUND_CLICK);
t=0;
}else if((ct_4-ct_0>200)&&(s<1)){ //一つ目の交差点で
PlaySound(SOUND_UP);
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED); //右折
Wait(ct_4/5);t++;
Off(OUT_BC);
sequence();s++;
}else if((ct_4-ct_0>200)&&(s<2)){ //２つ目の分岐(J)で
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED);PlaySound(SOUND_UP); //正面を向く
Wait(ct_4/2);
s++;
Off(OUT_BC);Wait(100); //一時停止 本来はこの後缶を捉えるサブルーチンが入る
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED); //左折
Wait(ct_4/5);
ahead_time=100;
sequence();
}else if((ct_4-ct_0>200)&&(s<5)){ //3,4つ目の分岐で
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED);PlaySound(SOUND_UP); //直進
Wait(ct_4/5);s++;
sequence();
}else if((ct_4-ct_0>200)&&(s<7)){ //5,6つ目の分岐で
Off(OUT_BC);Wait(100); //一時停止
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED);PlaySound(SOUND_UP); //左折
Wait(ct_4/5*4);t++;
Off(OUT_BC);
sequence();s++;
}
}
while(s<10){
if((t==1)&&(ct_1>-100)&&(ct_1<100)){ //つづら折りを出たら
front();
sequence();
ahead_time=300;
t=0;
s++;
}else if((ct_1>-100)&&(ct_1<100)){ //進行方向に対する左右の差が小さいとき
front(); //徐々に進む距離を短く
sequence();
ahead_time-=20;
}else if((ct_1<-100)||(ct_1<100)){ //進行方向に対する左右の差が大きいとき
ahead_time=200; //一定の距離を進む
front();
sequence();
t=1;
}
}
ahead_time=200;
while(s<16){ //分岐Dを抜けた先の曲がり角まで繰り返し
if((ct_4-ct_0<=200)||(t==1)){
front(); //角度調整
sequence();PlaySound(SOUND_CLICK);
t=0;
}else if((ct_4-ct_0>200)&&(s<11)){ //以下各分岐または曲がり角での直進、右左折
PlaySound(SOUND_UP);
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED);
Wait(ct_4/5*4);t++;
Off(OUT_BC);
sequence();s++;
}else if((ct_4-ct_0>200)&&(s<12)){
PlaySound(SOUND_UP);
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED);
Wait(ct_4/5);t++;
Off(OUT_BC);
sequence();s++;
}else if((ct_4-ct_0>200)&&(s<13)){
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED);
Wait(ct_4/5*4);
s++;
Off(OUT_BC);Wait(100);
OnFwd(OUT_C,C_SPEED);OnRev(OUT_B,C_SPEED);
Wait(ct_4/5*4);
sequence();
}else if((ct_4-ct_0>200)&&(s<14)){
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED);
Wait(ct_4/5*4);s++;
sequence();
}else if((ct_4-ct_0>200)&&(s<15)){
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED);
Wait(ct_4/5*7);s++;
Off(OUT_BC);
Wait(100); //一時停止 本来ならここで缶を放す
OnRev(OUT_BC,SPEED);Wait(100); //車体を後退
if(SENSOR_2<b_bordor){ //車体を反転
while(SENSOR_2<w_bordor){
OnFwd(OUT_C,C_SPEED);OnRev(OUT_B,C_SPEED);
}
while(SENSOR_2>b_bordor){
OnFwd(OUT_C,C_SPEED);OnRev(OUT_B,C_SPEED);
}
OnFwd(OUT_C,C_SPEED);OnRev(OUT_B,SPEED);
Wait(ct_0/2);
}else if(SENSOR_2>w_bordor){
while(SENSOR_2>b_bordor){
OnFwd(OUT_C,C_SPEED);OnRev(OUT_B,C_SPEED);
}
OnFwd(OUT_C,C_SPEED);OnRev(OUT_B,SPEED);
Wait(ct_0/2);
}
sequence();
}else if((ct_4-ct_0>200)&&(s<16)){
PlaySound(SOUND_UP);
OnFwd(OUT_B,C_SPEED);OnRev(OUT_C,C_SPEED);
Wait(ct_4/5);t++;
Off(OUT_BC);
sequence();s++;
}
}
while(s<18){ //つづら折りを進む
if((t==1)&&(ct_1>-100)&&(ct_1<100)){
front();
sequence();
t=0;
s++;
}else if((ct_1>-100)&&(ct_1<100)){
front();
sequence();
}else if((ct_1<-100)||(ct_1<100)){
front();
sequence();
t=1;
}
}
while(SENSOR_2<b_bordor){ //長い直線を進む
if((s==18)||(t==1)){
front();
sequence();
ahead_time=350;
t=0;
s++;
}else if(ct_4-ct_0<=200){ //距離を徐々に短くしながら進む
front();
sequence();
ahead_time-=20;
}else if((ct_4-ct_0>200)&&(s<20)){ //交差点を直進
ahead_time=200;
front();
Wait(100);
sequence();
t=1;
}else if((ct_4-ct_0>200)&&(s<21)){ //交差点を直進
ahead_time=200;
front();
Wait(100);
sequence();
t=1;
}
}
front();
ahead();
Off(OUT_BC);
}
**結果 [#j793f80e]
*結果 [#j793f80e]
一つ目の交差点までは曲がれたが、缶の手前で止まることができなかった。
**反省点 [#a275c0f5]
*反省点 [#a275c0f5]
試行回数が少なく十分にプログラムを見直せなかった。また、一回で進む距離が固定のためスタートの位置、角度がずれると交差点を曲がれない時もあった。よって、もっと安定性のあるプログラムへ改良する必要を感じた。
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
