[[2013b/Member]]
#contents
*メンバー [#l22be8ee]
Naoyoshi, Tomo（[[2013b/Member/Tomo/Mission1]]）
*ロボットの説明 [#e14754cd]
**主な特徴 [#t08b66bd]
&ref(2013b/Member/Naoyoshi/Mission1/DSC_0078.jpg,100%,全体像);
+ ライントレースの精度を上げるために、ドライブベースを拡張。車幅を広げて車軸の近くに光センサーを設置した。
+ 缶キャッチャー用のモータとNXT本体を前後に配置。車重バランスを改善した。
+ NXT本体を立てて組み込み、缶をつかむアームを折りたたんで収納することで省スペースを実現した。
+ 上面にタッチセンサをスタートスイッチとして置いた。
**苦労した点 [#g24788f8]
当初缶を検知するセンサはタッチセンサを使用する方針だったが、予想以上にセンサの張りが強く、バンパーを設置してもなかなか缶の重さだけでセンサを反応させることが出来なかった。超音波センサを使う方針に切り替えてからも、タッチセンサに比べサイズの大きい超音波センサを組み込むのに苦労した。最終的にセンサを上部に設置し、背の高い500ml缶を用いることで解決した。
缶を掴むアームも、2本の腕で確実に挟むためには、2つの回転数が同じで且つ回転方向が反対の出力を得なければならず、モータから動力を伝える機構の省スペース化が課題だった。モータの両端をギアで挟むという構造的な工夫で反対の回転方向を得ることができ、さらに必要最小限のギア配置を探ることで省スペース化を実現した。
*プログラムの説明 [#e8ee1ec2]
以下のプログラムはコースを半時計回りするプログラムである
**定義文 [#kd6e2aa4]
#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 //第三段階終了時間
#define SPEED 50 //缶をつかむ時のスピード
#define RANGE1 3.0 //缶を掴んでから少し前進する距離
#define RANGE2 6.0 //缶を話したあとに後退する距離
#define HAND OUT_A //アームを駆動するモータをAに定義
#define ADDITIONAL_TIME 1500 //缶をつかむ動作でずれたタイマーの補正
#define CATCH_TIME 1200 //アームの開閉時間
#define catch OnFwd(HAND,20);Wait(CATCH_TIME);Off(HAND); //アームを閉じるマクロ
#define release OnRev(HAND,20);Wait(CATCH_TIME);Off(HAND); //アームを開くマクロ
long t0,t1,t2,t3; //t0は走行開始時間、t1は缶をつかむタスクの開始時間、t2は缶をつかむタスクの終了時間、t3は缶をつかむタスクの累積の経過時間
float width; //本体幅を定義、値は後で代入
mutex moveMutex; //この変数を参照しなければタイヤを動かすことが出来ない
**缶をつかむタスク [#q9361be9]
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後退
if(SENSOR_3>THRESHOLD) //ライン上に戻っていなかった時に左に旋回してラインを探す
{
until(SENSOR_3<=THRESHOLD)
{
turn_left0;
}
}
t2=CurrentTick(); //t2に終了時間を記憶
t3=t3+(t2-t1)-ADDITIONAL_TIME; //今までの缶をつかむのにかかった時間の総和とタイマーのズレを勘案した時間をt3として返す
Release(moveMutex); //並列タスクの抑制を開放
}
}
}
私は缶をつかむプログラムを担当した。缶をつかむためには、
- 缶の戻し方
- ロボットをライン上に戻す方法
- 缶をつかむ動作でずれたタイマーの補正
を考えなければならなかった。このうち缶を戻す動作は、下図のように90度の旋回を2回繰り返すことで解決した。
#ref(2013b/Member/Naoyoshi/Mission1/ライントレース.png,100%,缶の戻し方)
ロボットをライン上に戻すためには、元の向きに戻ってから黒いラインに乗るまで左旋回を続けさせることで、光センサの描く円の中に黒いラインがあれば復帰できるようになった。これにより、直線の上だけでなくカーブの上にも缶を置くことが出来る。
タイマーのずれに対しては、缶をつかむ動作の前後で時間を記録し、経過時間をタイマーから引くことで補正している。さらに缶を戻す動作によってロボットが少し前に移動してしまうので、その距離を進むのにかかる時間を足すことで、より正確な補正を可能にした。
**ライントレース用のタスク [#t006768e]
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;
}
}
今回はライントレース用のプログラムをTomo[[2013b/Member/Tomo/Mission1]]に作ってもらった。コースを交差点の有無・時計回り・反時計回りの観点から4つに分け、タイマーによってそれぞれに合わせた走行プログラムを切り替えるという優れものである。
**メインタスク [#s7f72931]
task main() //メインタスク。ここからすべてのタスクが開始される
{
t1=0;
t2=0;
t3=0; //時間に関する変数にすべて初期値0を代入
width=12.8; //ロボット本体の幅、回転角度の算出に使う
SetSensorTouch(S1);
SetSensorLight(S3);
SetSensorLowspeed(S4); //センサ類を定義
while(SENSOR_1==0) //タッチセンサが押されるまで待機、タッチで起動
{
Wait(1);
}
t0=CurrentTick(); //開始時間を記憶
Precedes(catch_can,line_trace); //缶をつかむタスクとライントレースのタスクを同時に進行させる
}
変数類はtaskの外で定義したが、taskの外では具体的な数値を代入できないので、メインタスクの中で初期値を代入した。
命令文Precedes(a,b);は、aとbのタスクを並列して実行する。この際aとbのプログラムが同時にモータを使おうとすると想定外の動作になってしまう（競合という）。これを避けるために変数moveMutexを置き、この変数を参照しなければタイヤを駆動するモータを使えないようにプログラミングを施した。具体的にはAcquireでmoveMutexを参照し、ReleaseでmoveMutexを開放する。あるタスクでmoveMutexが参照されている間は他のタスクでmoveMutexを参照できない仕組みだ。
**苦労した点 [#t4591db9]
例えば
while(A){while(B){X;}}
Y;
というプログラムがあった時、Aが満たされなくなった時にすぐにはYは実行されない。Bの条件が満たされなくならない限りXを実行し続けるからだ。このことを理解するまでに時間がかかってしまった。この他にも、プログラムは基本的に上から下へ順に評価されていくことを考えなければ思ったようにプログラムが動作しない点が多く、苦労した。
*全体を通しての反省点 [#a475a1fc]
- ロボットが若干大きい。カーブは曲がり切れるが、せっかく戻した缶にカーブを曲がった先で接触してしまう。構造をより簡略化することが求められる。
- プログラムが大きくなりすぎた。今回は掲載しなかったが、使われない定義文があるなど無駄な部分があるので、それらを削ることが必要だ。
- 電圧によってタイマーを調整しなければならない。電圧一定の信頼できる電源を使うか、もしくはタイマーによらないプログラムを考えなければならない。
*参考にしたサイト [#s72ccc6f]
Daniele Benedettelli, 『NXC を使ったLEGO のNXT ロボットのプログラミング』
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
