Tomo
Naoyoshi
写真にあるコースをライントレースして走り、コース上にある障害物の空き缶二つを撤去して一周する。右回り、左回りどちらもできるようにする。
チーム二人で作業を分担して行った。私はライントレースの右回り、左回りのプログラムの制作を担当してNaoyoshiが空き缶を撤去するためのアームとプログラムを制作した。
#define SPEED 50 #define RANGE1 3.0//缶を掴んでから少し前進する距離 #define RANGE2 6.0//缶を離した後に後退する距離 #define HAND OUT_A//アームを駆動するモーターをAに定義 #define ADDITIONAL_TIME 3000//缶を掴む動作でずれたタイマーの修正 #define CATCH_TIME 1200//アームの開閉時間 #define catch OnFwd(HAND,20);Wait(CATCH_TIME);Off(HAND);//アームを閉じるマクロ #define release OnRev(HAND,20);Wait(CATCH_TIME);Off(HAND);//アームを開くマクロ #define THRESHOLD 45//光センサの閾値 #define SPEED_L 30//走行時のスピード #define OnRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL); #define go_forward OnRL(SPEED_L,SPEED_L);//直進するマクロ #define turn_left1 OnRL(SPEED_L,-SPEED_L);//左に旋回するマクロ #define turn_left0 OnRL(SPEED_L,0);//左に曲がるマクロ #define turn_right0 OnRL(0,SPEED_L);//右に曲がるマクロ #define turn_right1 OnRL(-SPEED_L,SPEED_L);//右に旋回する #define STEP 1 #define nMAX 50 #define short_break Off(OUT_BC);Wait(1000); #define CROSS_TIME 200 #define cross_line OnRL(SPEED_L,SPEED_L);Wait(CROSS_TIME);short_break; #define FIRSTMAX 10000//第一段階終了時間 #define SECONDMAX 33000//第二段階終了時間 #define THIRDMAX 48000//第三段階終了時間 long t0,t1,t2,t3;//t0は走行開始時間、t1は缶を掴むタスクの開始時間、t2は缶を掴むタスクの終了時間、t3は缶を掴むタスクの累積の経過時間 float width;//本体幅を定義、値は後で代入 mutex moveMutex;//この変数を参照しなければタイヤを動かすことが出来ない float GetAngle(float r)//与えられた距離r(float型変数)からモーターの回転角度を導き出すマクロ { const float diameter=5.45;//タイヤの直径 const float pi=3.1415;//円周率 float ang=r/(diameter*pi)*360.0;//円周と与えられた距離から角度を導く return ang;//angを返す } task catch_can()//缶を掴むタスク { while(true) { if(SensorUS(S4)<=7)//超音波センサ値が7cm以内の時 { Acquire(moveMutex);//並列タスク同士が競合しないためのプログラム t1=CurrentTick();//t1に開始時間を記憶 Off(OUT_BC);//タイヤを停止 catch; int angleA=GetAngle(RANGE1);//3cm進むための回転角度を定義 RotateMotor(OUT_BC,SPEED,angleA);//3cm前進し缶を元の位置に戻すための調整 int angleB=GetAngle(2*width*3.1415/4);//車幅を半径として1/4周の距離を代入 RotateMotor(OUT_B,SPEED,angleB);//1/4左回転 RotateMotor(OUT_C,-SPEED,angleB);//1/4右回転 release; RotateMotor(OUT_C,-SPEED,angleB);//後ろ向きに1/4右回転 RotateMotor(OUT_B,SPEED,angleB);//後ろ向きに1/4左回転 int angleC=GetAngle(RANGE2);//6cm進むための回転角度を定義 RotateMotor(OUT_BC,-SPEED,angleC);//6cm後退 t2=CurrentTick();//t2に終了時間を記憶 t3=t3+(t2-t1)-ADDITIONAL_TIME;//今までの缶を掴むのにかかった時間の総和とタイマーのズレを勘案した時間をt3として返す Release(moveMutex);//並列タスクの抑制を解放 } } } task line_trace()//ラインをトレースするタスク { int nOnline=0; while(true) { while(nOnline<nMAX) { PlaySound(SOUND_CLICK);//第一段階開始 while(CurrentTick()-t0-t3<=FIRSTMAX) { Acquire(moveMutex); if(SENSOR_3<THRESHOLD-4) { turn_right1; } else if(SENSOR_3<THRESHOLD+4) { go_forward; } else { turn_left1; } Wait(STEP); Release(moveMutex); }//終了 PlaySound(SOUND_FAST_UP);//第二段階開始 turn_left1; Wait(700); while(CurrentTick()-t0-t3<=SECONDMAX) { Acquire(moveMutex); if(SENSOR_3<THRESHOLD-7) { turn_left0; nOnline++; } else { if(SENSOR_3<THRESHOLD+7) { go_forward; } else { turn_right1; } nOnline=0; } Wait(STEP); Release(moveMutex); }//終了 PlaySound(SOUND_UP);//第三段階開始 turn_right1; Wait(1000); while(CurrentTick()-t0-t3<=THIRDMAX) { Acquire(moveMutex); if(SENSOR_3<THRESHOLD-7) { turn_right0; nOnline++; } else { if(SENSOR_3<THRESHOLD+7) { go_forward; } else { turn_left1; } nOnline=0; } Wait(STEP); Release(moveMutex); }//終了 PlaySound(SOUND_CLICK);//第四段階開始 while(CurrentTick()-t0-t3>THIRDMAX) { Acquire(moveMutex); if(SENSOR_3<THRESHOLD-4) { turn_right1; } else if(SENSOR_3<THRESHOLD+4) { go_forward; } else { turn_left1; } Wait(STEP); Release(moveMutex); }//終了 } short_break; turn_left1; Wait(nMAX*STEP); cross_line; nOnline=0; } } task main() { t1=0; t2=0; t3=0; width=12.8; SetSensorTouch(S1); SetSensorLight(S3); SetSensorLowspeed(S4); while(SENSOR_1==0) { Wait(1); } t0=CurrentTick(); Precedes(catch_can,line_trace); }
コースを4つに分けてそれぞれのコースに合ったプログラムを作り時間で切り替わるようにした。
1番目のコースは直角カーブが二つあるので走行スピードを少し遅くして小刻みに曲がれるようにした。交差点用のプログラムなど要らないものは全て無くしシンプルなものにした。
2番目のコースでは緩やかな右カーブをまがりながら交差点を直進する場所がある。教科書の交差点を直進するプログラムでは左折回数が少なくて交差点を判断できなかった。これはプログラムの交差点と判断する左折回数を極端に減らすことで解決できた。
今まではラインの左側を走っていた3番目のコースでは右側を走った方が走行しやすいことが分かり変更した。左折のプログラムと右折のプログラムを交換するだけで左側走行と右側走行を切り替えることが出来て意外と簡単だった。
4番目のコースではまた左側走行する必要があった。しかし、今度は左側走行と右側走行を入れ替えるだけではロボットがラインを見失ってしまう。これはプログラムが切り替わった直後に少し左折するようにプログラムすることで強制的にラインを発見させて解決した。後は交差点などもなかったので1番目のコースのプログラミングで走行することができた。
上記は左周りのプログラムで右回りのプログラムは右回りのプログラムを応用することによってほぼ苦労することなくできた。
このプログラムはNaoyoshiが作ってくれたためNaoyoshiにプログラムの説明を書いてもらった。
空き缶を撤去するために考えなければならないポイントは3つあった。
・掴んだ缶をどうやって元の位置に戻すか
・缶を戻した後どうやってライン上にロボットを戻すか
・缶をつかむ間にずれたタイマーをどう補正するか
このうちどうやって掴んだ缶をもとの位置に戻すかは、下図のように片方のタイヤを使って本体を1/4周だけ回す過程を二回繰り返すことで解決した。
ライン上にロボットを戻す方法は、コースアウトしたロボットに右回転で旋回させることで、タッチセンサーが描く円状にラインがあれば通常のライントレースに戻るようにプログラミングを施した。
さらにタイマーのずれの修正は、缶を撤去する前後の時間を記録しその差をタイマーから引くことで対処した。実際には缶を撤去する動作でわずかにロボットが元の位置より前に進んでしまうので、その距離をライントレースで進むのにかかる時間を加味することでさらに正確にタイマーを修正している。
ライントレースの精度を上げるために、光センサの位置を車軸に近づけた。これにより、ヘアピンカーブでも光センサがラインから外れることなくトレースできるようになった。
一番時間がかかったのは缶をつかむアームを駆動するモータの配置だ。サイズの大きいサーボモータを前にせり出した状態で設置すると、重量バランスが悪くなる。これには、NXT本体を立てて配置することでモータのスペースを確保して対処した。さらに、アームを1本だけにすると缶を安定してつかむことが出来ないので、2本のアームで挟み込むように缶をつかまなければいけなかった。動力をアームに伝えるギアをモータの両側に置き、最小限の動力伝達機構でアームを駆動させることで、前方のアーム部の軽量化・省スペース化に成功した。
最初は全く課題がクリアできる感じがしなかった。ライントレースが全くできず、やる気もだんだん下がりプログラミングがしんどくなってきた。しかし、プログラムを時間で切り替えるという方法を試してからは出来る可能性が少し見えてきて、何とかプログラムを続けることができた。ライントレースが出来たときはとても嬉しかった。