今回の課題はライントレースをしながら、十字路を指定されたように曲がるというものである。
ロボット本体は元となる車の状態に、光センサを取り付けた簡単な造りである。
まずは普通にライントレースをしていき、交差点に差し掛かったところで左折・右折・直進のいずれかの動作をするので、その繰り返しのようなプログラムになっている。
交差点の判定は、連続で何回黒を認識したかによって行う。
以下のように定義する
#define THRESHOLD (66+36)/2 //白と黒の取りうる値の最大値の平均値(しきい値) #define SPEED_H 35 #define SPEED_L 20 #define OnRL(speed_R,speed_L) OnFwd(OUT_B,speed_R);OnFwd(OUT_C,speed_L); #define go_forward OnRL(SPEED_H,SPEED_H); //直進 #define turn_left0 OnRL(SPEED_L,0); //左旋回 #define turn_left1 OnRL(SPEED_L,-SPEED_L); //左回転 #define turn_right0 OnRL(0,SPEED_L); //右旋回 #define turn_right1 OnRL(-SPEED_L,SPEED_L); //右回転 #define STEP 1 //1ステップの待ち時間 #define nMAX 270 //十字路と判別する連続ステップ数 #define ds 7 //しきい値との差(小) #define dl 12 //しきい値との差(大)
センサの設定
task main() { SetSensorLight(S1); //センサ1は光センサ int nOnline=0; //nOnlineを定義、初期値は0
while(nOnline < nMAX){ if(SENSOR_1 < THRESHOLD-dl){ turn_left1; nOnline++; }else{ if(SENSOR_1 < THRESHOLD-ds){ turn_left0; }else if(SENSOR_1 < THRESHOLD+ds){ go_forward; }else if(SENSOR_1 < THRESHOLD+dl){ turn_right0; }else{ turn_right1; } nOnline=0; } Wait(STEP); } //十字路までライントレース turn_left0; until(SENSOR_1 > THRESHOLD+(dl+ds)/2); //白を検出するまで左旋回 nOnline=0; //nOnlinoの初期化
while(nOnline < nMAX){ if(SENSOR_1 < THRESHOLD-dl){ turn_left1; nOnline++; }else{ if(SENSOR_1 < THRESHOLD-ds){ turn_left0; }else if(SENSOR_1 < THRESHOLD+ds){ go_forward; }else if(SENSOR_1 < THRESHOLD+dl){ turn_right0; }else{ turn_right1; } nOnline=0; } Wait(STEP); } //十字路までライントレース repeat(2){ turn_right0; until(SENSOR_1 > THRESHOLD+(dl+ds)/2); turn_right0; until(SENSOR_1 < THRESHOLD-(dl+ds)/2); } //白→黒→白→黒の順に検出するまで右旋回 nOnline=0; //nOnlineの初期化
while(nOnline < nMAX){ if(SENSOR_1 < THRESHOLD-dl){ turn_left1; nOnline++; }else{ if(SENSOR_1 < THRESHOLD-ds){ turn_left0; }else if(SENSOR_1 < THRESHOLD+ds){ go_forward; }else if(SENSOR_1 < THRESHOLD+dl){ turn_right0; }else{ turn_right1; } nOnline=0; } Wait(STEP); } //十字路を検出するまでライントレース turn_left1; until(SENSOR_1 > THRESHOLD+(dl+ds)/2); //白を検出するまで左旋回 nOnline=0; //nOnlineの初期化
repeat(2){ while(nOnline < nMAX){ if(SENSOR_1 < THRESHOLD-dl){ turn_left1; nOnline++; }else{ if(SENSOR_1 < THRESHOLD-ds){ turn_left0; }else if(SENSOR_1 < THRESHOLD+ds){ go_forward; }else if(SENSOR_1 < THRESHOLD+dl){ turn_right0; }else{ turn_right1; } nOnline=0; } Wait(STEP); } //十字路までライントレース turn_left0; //誤作動を減らすための調整 Wait(100); turn_right0; until(SENSOR_1 > THRESHOLD+(dl+ds)/2); turn_right0; until(SENSOR_1 < THRESHOLD-(dl+ds)/2); //白→黒の順に検出するまで右旋回 nOnline=0; //nOnlineの初期化 }
while(true){ if(SENSOR_1 < THRESHOLD-dl){ turn_left1; }else if(SENSOR_1 < THRESHOLD-ds){ turn_left0; }else if(SENSOR_1 < THRESHOLD+ds){ go_forward; }else if(SENSOR_1 < THRESHOLD+dl){ turn_right0; }else{ turn_right1; } Wait(STEP); } } //最後は普通のライントレース
サブルーチン等を用いて短いプログラムにしようと試みたが、上手くいかず、動作の割に長いプログラムになってしまった。
しっかり完走することはできるが、若干電池残量に影響を受けてしまうことが分かった。
次回は、簡潔で外的要因の影響を受けにくいプログラムを作りたい。