#contents *メンバー [#kb656250] Tomo ~Naoyoshi *課題 [#qf85e42b] 写真にあるコースをライントレースして走り、コース上にある障害物の空き缶二つを撤去して一周する。右回り、左回りどちらもできるようにする。 *進行計画 [#p1312521] チーム二人で作業を分担して行った。私はライントレースの右回り、左回りのプログラムの制作を担当してNaoyoshiが空き缶を撤去するためのアームとプログラムを制作した。 *プログラム [#pef1c497] **右回り用 [#r1277ce5] #define SPEED 50 #define RANGE1 3.0 #define RANGE2 6.0 #define HAND OUT_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_H 40 #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; float width; mutex moveMutex; float GetAngle(float r) { const float diameter=5.45; const float pi=3.1415; float ang=r/(diameter*pi)*360.0; return ang; } task catch_can() { while(true) { if(SensorUS(S4)<=7) { Acquire(moveMutex); t1=CurrentTick(); Off(OUT_BC); catch; int angleA=GetAngle(RANGE1); RotateMotor(OUT_BC,SPEED,angleA); int angleB=GetAngle(2*width*3.1415/4); RotateMotor(OUT_B,SPEED,angleB); RotateMotor(OUT_C,-SPEED,angleB); release; RotateMotor(OUT_C,-SPEED,angleB); RotateMotor(OUT_B,SPEED,angleB); int angleC=GetAngle(RANGE2); RotateMotor(OUT_BC,-SPEED,angleC); t2=CurrentTick(); t3=t3+(t2-t1)-ADDITIONAL_TIME; 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); } **左周り用 [#v23bd663] #define SPEED 50 #define RANGE1 3.0 #define RANGE2 6.0 #define HAND OUT_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_H 40 #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 13000 #define SECONDMAX 33000 #define THIRDMAX 47000 long t0,t1,t2,t3; float width; mutex moveMutex; float GetAngle(float r) { const float diameter=5.45; const float pi=3.1415; float ang=r/(diameter*pi)*360.0; return ang; } task catch_can() { while(true) { if(SensorUS(S4)<=7) { Acquire(moveMutex); t1=CurrentTick(); Off(OUT_BC); catch; int angleA=GetAngle(RANGE1); RotateMotor(OUT_BC,SPEED,angleA); int angleB=GetAngle(2*width*3.1415/4); RotateMotor(OUT_B,SPEED,angleB); RotateMotor(OUT_C,-SPEED,angleB); release; RotateMotor(OUT_C,-SPEED,angleB); RotateMotor(OUT_B,SPEED,angleB); int angleC=GetAngle(RANGE2); RotateMotor(OUT_BC,-SPEED,angleC); t2=CurrentTick(); t3=t3+(t2-t1)-ADDITIONAL_TIME; 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_left1; } else if(SENSOR_3<THRESHOLD+4) { go_forward; } else { turn_right1; } Wait(STEP); Release(moveMutex); }//終了 PlaySound(SOUND_UP);//第二段階開始 while(CurrentTick()-t0-t3<=SECONDMAX) { Acquire(moveMutex); if(SENSOR_3<THRESHOLD-15) { turn_left0; nOnline++; } else { if(SENSOR_3<THRESHOLD+7) { go_forward; } else if(SENSOR_3<THRESHOLD+15) { turn_right0; } else { turn_right1; } nOnline=0; } Wait(STEP); Release(moveMutex); }//終了 PlaySound(SOUND_FAST_UP);//第三段階開始 while(CurrentTick()-t0-t3<=THIRDMAX) { Acquire(moveMutex); if(SENSOR_3<THRESHOLD-7) { turn_right0; nOnline++; } else { if(SENSOR_3<THRESHOLD+7) { go_forward; } else if(SENSOR_3<THRESHOLD+15) { turn_left0; } else { turn_left1; } nOnline=0; } Wait(STEP); Release(moveMutex); }//終了 PlaySound(SOUND_CLICK);//第四段階開始 turn_left1; Wait(1000); while(CurrentTick()-t0-t3>THIRDMAX) { Acquire(moveMutex); if(SENSOR_3<THRESHOLD-4) { turn_left1; } else if(SENSOR_3<THRESHOLD+4) { go_forward; } else { turn_right1; } Wait(STEP); Release(moveMutex); }//終了 } short_break; turn_right1; 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); } *プログラム制作 [#b7b79eaf] **ライントレースプログラム [#yd82e320] ~ コースを4つに分けてそれぞれのコースに合ったプログラムを作り時間で切り替わるようにした。 ~ 1番目のコースは直角カーブが二つあるので走行スピードを少し遅くして小刻みに曲がれるようにした。交差点用のプログラムなど要らないものは全て無くしシンプルなものにした。 ~ 2番目のコースでは緩やかな右カーブをまがりながら交差点を直進する場所がある。教科書の交差点を直進するプログラムでは左折回数が少なくて交差点を判断できなかった。これはプログラムの交差点と判断する左折回数を極端に減らすことで解決できた。 ~ 今まではラインの左側を走っていた3番目のコースでは右側を走った方が走行しやすいことが分かり変更した。左折のプログラムと右折のプログラムを交換するだけで左側走行と右側走行を切り替えることが出来て意外と簡単だった。 ~ 4番目のコースではまた左側走行する必要があった。しかし、今度は左側走行と右側走行を入れ替えるだけではロボットがラインを見失ってしまう。これはプログラムが切り替わった直後に少し左折するようにプログラムすることで強制的にラインを発見させて解決した。後は交差点などもなかったので1番目のコースのプログラミングで走行することができた。 ~ 上記は左周りのプログラムで右回りのプログラムは右回りのプログラムを応用することによってほぼ苦労することなくできた。 **空き缶撤去用プログラム [#f6a530ef] このプログラムはNaoyoshiが作ってくれたためNaoyoshiにプログラムの説明を書いてもらった。 ~ 空き缶を撤去するために考えなければならないポイントは3つあった。 ~ ・掴んだ缶をどうやって元の位置に戻すか ~ ・缶を戻した後どうやってライン上にロボットを戻すか ~ ・缶をつかむ間にずれたタイマーをどう補正するか ~ このうちどうやって掴んだ缶をもとの位置に戻すかは、下図のように片方のタイヤを使って本体を1/4周だけ回す過程を二回繰り返すことで解決した。 #ref:http://yakushi.shinshu-u.ac.jp/robotics/?plugin=attach&pcmd=open&file=DSCN1271%B2%C3%B9%A9.jpg&refer=2013b%2FMember%2FTomo%2FMission1 ~ ライン上にロボットを戻す方法は、コースアウトしたロボットに右回転で旋回させることで、タッチセンサーが描く円状にラインがあれば通常のライントレースに戻るようにプログラミングを施した。 ~ さらにタイマーのずれの修正は、缶を撤去する前後の時間を記録しその差をタイマーから引くことで対処した。実際には缶を撤去する動作でわずかにロボットが元の位置より前に進んでしまうので、その距離をライントレースで進むのにかかる時間を加味することでさらに正確にタイマーを修正している。 *ロボット制作 [#gfffcac2] ~ ライントレースの精度を上げるために、光センサの位置を車軸に近づけた。これにより、ヘアピンカーブでも光センサがラインから外れることなくトレースできるようになった。 ~ 一番時間がかかったのは缶をつかむアームを駆動するモータの配置だ。サイズの大きいサーボモータを前にせり出した状態で設置すると、重量バランスが悪くなる。これには、NXT本体を立てて配置することでモータのスペースを確保して対処した。さらに、アームを1本だけにすると缶を安定してつかむことが出来ないので、2本のアームで挟み込むように缶をつかまなければいけなかった。動力をアームに伝えるギアをモータの両側に置き、最小限の動力伝達機構でアームを駆動させることで、前方のアーム部の軽量化・省スペース化に成功した。 *感想 [#vd483e08] 最初は全く課題がクリアできる感じがしなかった。ライントレースが全くできず、やる気もだんだん下がりプログラミングがしんどくなってきた。しかし、プログラムを時間で切り替えるという方法を試してからは出来る可能性が少し見えてきて、何とかプログラムを続けることができた。ライントレースが出来たときはとても嬉しかった。