2006a/C2/課題３左
光源を見失った時には曲を演奏しながらぐるっと旋回して光源を探すプログラム
カウンタ772
プログラム内容†
#define timer 200
//music nomber
#define Do 523
#define Re 587
#define Mi 659
#define Fa 698
#define So 784
#define Ra 880
#define Si 988
#define Do2 1047
int touch;
task music(){
while(1){ //曲目：ねこふんじゃった
Wait(20);
PlayTone(Ra,5); Wait(10);
PlayTone(So,5); Wait(10);
PlayTone(Do,15); Wait(20);
PlayTone(Do2,5); Wait(20);
PlayTone(Do2,5); Wait(20);
PlayTone(Ra,5); Wait(10);
PlayTone(So,5); Wait(10);
PlayTone(Do,15); Wait(15);
PlayTone(Do2,5); Wait(20);
PlayTone(Do2,5); Wait(20);
PlayTone(Ra,5); Wait(10);
PlayTone(So,5); Wait(10);
PlayTone(Do,15); Wait(20);
PlayTone(Do2,15); Wait(20);
PlayTone(So,15); Wait(20);
PlayTone(Do2,15); Wait(20);
PlayTone(Ra,15); Wait(20);
PlayTone(Si,5); Wait(20);
PlayTone(Si,5); Wait(20);
PlayTone(Ra,5); Wait(10);
PlayTone(So,5); Wait(10);
PlayTone(Re,15); Wait(20);
PlayTone(Si,5); Wait(20);
PlayTone(Si,5); Wait(20);
PlayTone(Ra,5); Wait(10);
PlayTone(So,5); Wait(10);
PlayTone(Re,15); Wait(20);
PlayTone(Si,5); Wait(20);
PlayTone(Si,5); Wait(20);
PlayTone(Ra,5); Wait(10);
PlayTone(So,5); Wait(10);
PlayTone(Re,15); Wait(20);
PlayTone(Si,15); Wait(20);
PlayTone(Ra,15); Wait(20);
PlayTone(Si,15); Wait(20);
PlayTone(Do2,5); Wait(20);
PlayTone(Do2,5); Wait(20);
PlayTone(Do2,5); Wait(20);
}
}
task main(){
SetSensor(SENSOR_1,SENSOR_LIGHT); //センサー１を光センサーと設定
SetSensor(SENSOR_3,SENSOR_LIGHT); //センサー３を光センサーと設定
touch = (SENSOR_1 + SENSOR_3) * 3 / 5; //閾値をセンサー１と３の３／５の値に設定
while(1){ //無限ループ
if((SENSOR_1 < touch) && (SENSOR_3 < touch)){ //センサー１と３が閾値より低い場合
if(SENSOR_1 > SENSOR_3){ //センサー３より１が高い場合
trun_r(); //サブルーチンtrun_rへ移動
}else if(SENSOR_1 < SENSOR_3){ //センサー１が３より高い場合
trun_l(); //サブルーチンtrun_lへ移動
}
}else{ //両センサーが閾値より低い場合以外
stop music; //音楽停止
straight(); //サブルーチンstraightへ移動
}
}
}
void trun_r(void){ //右旋回
start music; //音楽再生
OnFwd(OUT_C); //モーターＣを正転
OnRev(OUT_A); //モーターＡを逆転
until((SENSOR_1 >= touch) || (SENSOR_3 >= touch)); //センサーの一方がライトに反応するまで足止め
}
void trun_l(void){ //左旋回
start music; //音楽再生
OnFwd(OUT_A); //モーターＡを正転
OnRev(OUT_C); //モーターＣを逆転
until((SENSOR_1 >= touch) || (SENSOR_3 >= touch)); //センサーの一方がライトに反応するまで足止め
}
void straight(void){ //前進運動
if((SENSOR_1 >= touch) && (SENSOR_3 >= touch)){ //両センサーがライトに反応している場合
OnFwd(OUT_A+OUT_C); //両モーター正転
}else if((SENSOR_1 >= touch) && (SENSOR_3 < touch)){ //センサー１のみがライトに反応している場合
OnFwd(OUT_C);
OnRev(OUT_A);
}else if((SENSOR_1 < touch) && (SENSOR_3 >= touch)){ //センサー３のみがライトに反応している場合
OnFwd(OUT_A);
OnRev(OUT_C);
}
}
プログラム説明†
- このプログラムは、光源を見失った時には曲を演奏しながらぐるっと旋回して光源を探すプログラムです。マシンの状態はセンサー二つの時を想定して作りました。曲目は「ねこふんじゃった」です。
- このプログラムでの「旋回」というのは、一度完全に一周するのでなく、一定値以上の明るさの部分を見つけたらその方向にむかって進むようになっています。また、今回閾値は定数を用いずに初回起動時にセンサーが受信したセンサー値を基準にしています。これは、そのプログラムを起動する場所というのは常に同じ明るさの場所で起動することはなく、必ず周囲の明るさが変化するためです。場所によって変化する明るさに対応出来るように一度周囲の明るさをセンサーを介して入力し、左右のセンサーで平均を取ることでより安定した動きを出来るだろう、と考えたためです。しかし、プログラムをみると「３／５」と平均になっていません。これは、完全に平均にしてしまうと、周囲の明るさでほんの少し明るくなったところをライトの明かりと勘違いしないようにするために平均より多少高いところに閾値を設定しようと思ったからです。
- センサー反応の条件は、今回多少複雑化してしまいました。まずセンサーがどちらも閾値以下になった場合、三つの可能性があります。それは、ライトが左側に逃げたか、上に逃げたか、右側に逃げたか、の三つです。上に逃げたのは追いかけようがありません。ですが、もし左側にライトが逃げたとしたら左側に旋回したほうがより早くライトの居場所を見つけられるはずです。同じように右側に逃げたライトは右側に旋回した方がより早く見つけられます。そのため、どちらのセンサーも閾値以下になった瞬間、一度左右の値を比較して大きい方に旋回するようにしました。また、旋回しないときでも直進以外に左旋回・右旋回をする必要があります。そのため、直進するサブルーチン内でもう一度どちらにライトがあるか認識品柄進むようにしています。
- このプログラムは旋回時のみ音楽を流すプログラムなので、旋回するサブルーチン内で再生するように設定して、どちらか一方のセンサーがライトに反応するまで、サブルーチンから抜けないよう、until文で足止めしています。
コメント・感想・アドバイス†
- 本題と関係のないコメントで申し訳ありませんが、trun_l や trun_r は turn_l や turn_r のほうが多分いいですね。 -- 松本(教員)
- スペルミスのご指摘ありがとうございます。前回のプログラムでもチームメイトに指摘されておりましたが、また間違えたようです。あと、プログラムに関してご指摘頂けるとうれしいのでお願いします。 -- H・L・ニコ
[[2006a/C2/課題３左に戻る>2006a/C2/課題３左]#g1294642]]
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
