蚕学部化材系の人です。基本プログラム担当。えせ林檎信者でありピアノマンもどきでもある。
工学部の人。ロボットとプログラム担当。
http://yakushi.shinshu-u.ac.jp/robotics/?2013b%2FMission1 に詳しくあります。
ライントレースと缶を丁重にどかして元の位置に戻す処理。
ライントレースのプログラムの最大の特徴はライン上をジグザグに走行する点と、またそのために交差点処理がない点です。出来るだけ多くのコースに対応できて、しかも簡単なロジックでコード量が少ないプログラムになるよう目指してみました。
一方缶の処理プログラムは時間をかけることが出来なかったためテキトーです。
1.プリプロセッサの部分
#define black 37 // この値より小さいと黒と判定します。 #define white 50 // この値より大きいと白と判定します。 #define ct1 150 // 缶をつかむときのモータの回転角です。 #define distance 20 #define go OnFwdSync(OUT_AB,p,0) // 前進。左右のタイヤが同期するようにしているのでまっすぐ進みます。 #define back OnRevSync(OUT_AB,p,0) // 後進 #define go_slowly OnFwdSync(OUT_AB,p,0) #define turn_fa OnFwdSync(OUT_AB,p,cd) // 一回目の回転です。 #define turn_fb OnFwdSync(OUT_AB,p,-cd) // 判定失敗時に機体の方向を元に戻します。 #define turn_sa turn_fb // 二回目の回転です。 #define turn_sb turn_fa // 判定失敗時。
2.変数
long t0,t1; int cd = 100; int cdp,cdm,pcd; int TurnLevel = 0; long TurnTime = 0; int p = 35;
それぞれ後で解説します。
3.main関数
task main() { while(true) { SetSensorLight(S1); // 光センサの有効化 SetSensorLowspeed(S2); // 超音波センサの有効化 if(SensorUS(S2) > 20) {lt();} // 機体前20cm以内に何もない時はライントレースします。 else if(SensorUS(S2) <= 20) {ct();} // 機体前20cm以内に缶がある場合は缶の処理をします。 } }
4.lt関数
sub lt() { while(SensorUS(S2) > distance) { if(SENSOR_1 < black) // 黒線上は直進 { go; } if(SENSOR_1 >= black || SENSOR_1 <= white) { go_slowly; // 際では徐行 } if(SENSOR_1 > white) { decide(); // 色い領域に出たら黒線を探します。 } } }
図
5.decideサブルーチン
sub decide() { while(SENSOR_1 > white) { fix(); // 白線領域に大きくはみ出して黒線を検出できなくなった時の対処です。 turn(); // ターンし始める方向を決定します。 curve(); // curve mode turntime(); // get turntime t0 = CurrentTick(); turn_fa; // turn()で決定した方向に回転します。回転時間はTurnTime間ですが、黒線を検出するとdecide()を抜けて再び直進します。 until(SENSOR_1 < black || CurrentTick()-t0 == TurnTime); if(SENSOR_1 <black) { pcd = cd; // どの方向に黒線を検出したかを記録。 TurnLevel = 0; // 初期化 } else { turn_fb; // 機体の方向をもとへ戻します。 Wait(TurnTime); t0 = CurrentTick(); turn_sa; // 2回目の回転。最初とは反対の方向に回転します。 until(SENSOR_1 < black || CurrentTick()-t0 == TurnTime); if(SENSOR_1 < black) { pcd = -cd; // 黒線を検出した方向を記録。 TurnLevel = 0; // 初期化 } else { turn_sb; // 元の位置に戻る。 Wait(TurnTime); TurnLevel ++; // TurnLevelを引き上げて、decide()の先頭から繰り返し。 } } } }
このサブルーチンは黒線を探すプログラムです。 まず最初に片側にTurnTimeに代入した時間だけ回転します。もし回転中に黒線を検出したらそこでdecideプログラムの先頭へジャンプし、またまっすぐ走行します。 回転中に黒線を検出できなかったらロボットの方向をまた元に戻し、今度は逆の方向へTurnTime分を回転します。ここでも同様に回転中に黒線を検出したらdecideの先頭に戻ります。検出できなかったらまたロボットの方向を元に戻しTurnLevelの値を増やして再度黒線の検出を再開します。
ここではいくつかの変数が出てきます。
これはTurnTimeの値を決定するのに使われます。黒線の検出に失敗したら値が増え,それに応じてTurnTimeの値が増えていきます。黒線の検出に成功したら0にリセットされます。
回転する時間を決めます。時間に応じてロボットの回転角が変化します。turntime()で値を計算します。
これは次にロボットが黒線を探すときに、どの方向から回転し始めるかを決める値として使われます。この値はOnFwdSync,OnRevSync関数の旋回パーセントです。ここでは100が右回転、−100が左回転です。
これにはどの方向に黒線を検出したかを記録します。この値を利用してcurve()を発動させます。
6.turntime()
sub turntime() { switch(TurnLevel) { case 0: TurnTime = 500; break; case 1: TurnTime = 1000; break; default: TurnTime = 1500; } }
見ての通り、TurnLevelの値に応じてTurnTimeを代入しています。defaultではTurnLevelが0でも1でもない時の値を設定しています。
7.turn()
sub turn() { cd = -cd; }
ここではロボットがdecide動作に入った時にどの方向から回転し始めるかを設定しています。 毎回cdの値を反転させることでジグザグ走行を実現しています。
8.curve()
sub curve() { if(pcd == 100) { cdp ++; // 連続で右側に黒線を検出したときに値が増加。 cdm = 0; } if(pcd == -100) { cdp = 0; cdm ++; // 連続で左側に黒線を検出したときに値が増加。 } /* 以下カーブ時の処理。値の強制書き換え */ if(cdp > 2) cd = 100; if(cdm > 2) cd = -100; if(cdm > 2 || cdp >2) p = 40; else p = 35; }
これによって連続で同じ方向へ曲がる動作をさせています。2回同じ方向に黒線を検出したらcdの値の書き換えとモータの出力の増強とを行っています。 pはモータの出力です。
9.fix()
sub fix() { if(TurnTime == 1500) { PlaySound(SOUND_UP); back; Wait(1000); turn_fa; until(SENSOR_1 < black); TurnLevel = 0; } }
コースから外れすぎた時のための処置です。バックしてまた黒線を探し始めます。
10.ct()
缶をどかします。
sub ct() { t1 = CurrentTick(); go_slowly; // 缶まで徐行 until(CurrentTick()-t1 == 300); RotateMotor(OUT_C,40,ct1); //catch RotateMotorEx(OUT_AB,p,430,100,true,true); //turn back; Wait(400); Off(OUT_AB); RotateMotor(OUT_C,-40,ct1); //put Wait(1000); back; Wait(900); t1 = CurrentTick(); OnFwdSync(OUT_AB,p,100); //turn until(SENSOR_1 < black && CurrentTick()-t1 >= 1000); }
ライントレースに関しては、自分の構想がうまく実現できたので満足している。普通のコースはだいたい上手く走れると思う。交差点処理が無いという点で面白いものが出来たと自分の中では思っている。ただ交差点を検出することが出来ないので、課題によっては難しかったり不利になったりするかもしれない。
コードに関しては少なくて見やすい様に心掛けた。ただ変数の使いまわしなどが多くて自分以外の人には理解しづらいかもしれない。
・コード書くのは楽しいけど修正作業がつらい
・自分の頭の中の構想と自分が書いたコードが一致してないのでうまく動かなくて作業がはかどらなかった。最後には一応思った通りのものができた。
・相方は神
どんなことでも良いので皆さん是非書いてくださいね(^ω^)