A地点から出発 → M → K(直進) → L(ピンポン玉をつかむ) → K(右折) → J(一時停止の後、左折) → I(直進) → H(直進) → G(左折) → F → E → D(一時停止の後、直進) → C(直進) → B(一時停止) → シュート→ A地点に入る(ゴール)
ベースはNXTキットの基本形ですが、それを改良しました。光センサーの位置をより近づけコンパクト化を目指しました。
下から見た図です、そこまで大きな改良点はありませんが、センサーと車軸までの距離を短くしました。
ロボット本体は小回りが利くようなるべく小さくしました。さらに光センサーと本体の間隔を短くし,センサーと本体の動きの差をなるべく小さくしました。
アームはギアで動力を伝え,両側から閉じる機構にしました.
ピンポン玉を掴めるだけの最小限のアームと本体によってかなり小さくすることが出来ました。
左端をトレースするには白を検知したとき右に,黒を検知したとき左に曲がればよい. 右側はそれと逆のことをする.
交差点を判断するには時間を利用します.交差点ではセンサーがずっと黒を認識し続けます.それを黒をある一定の時間以上認識し続けると止まる.というプログラムを書きます. 具体的には黒以外を認識するとタイマーが0にリセットされ、黒を検知した時にタイマーがカウントされ続け定められた値を超えると停止するプログラムです。
#define turn_right OnFwd(OUT_C,25);OnRev(OUT_B,15);//右折 #define turn_left OnFwd(OUT_B,25);OnRev(OUT_C,15);//左折 #define rotate_right OnFwd(OUT_C,20);OnRev(OUT_B,25);//右旋回 #define rotate_left OnFwd(OUT_B,20);OnRev(OUT_C,25);//左旋回 #define go_straight OnFwd(OUT_BC,20);//直進 #define go_straight2 OnFwd(OUT_BC,15);//ゆっくり直進 #define turn OnRev(OUT_C,25);OnFwd(OUT_B,25);Wait(1500);Off(OUT_BC);//180°旋回 #define catch OnRev(OUT_A,25);Wait(500);Off(OUT_A);//ピンポン玉をつかむ #define release OnFwd(OUT_A,25);Wait(500);Off(OUT_A);//ピンポン玉を離す #define osidasi OnFwd(OUT_BC,100);Wait(100);Off(OUT_BC);//加速してピンポン玉を押し出す #define black 20//黒の値は20 #define white 50//白の値は50 #define lightgray 40//灰白色は40 #define darkgray 28//灰黒色は28
まず,右左折,右旋回,左旋回,直進,ピンポン玉をつかむ,離す,センサーの値をそれぞれ定義する
交差点まで左側に沿ってトレースするサブルーチン
sub followline_L(int stop_time) { long t0=CurrentTick(); while(CurrentTick()-t0<stop_time){//現在の時刻が一定の値を超えるまで if(SENSOR_1<black){//センサーが黒の時 rotate_left;//左旋回 }else if(SENSOR_1<darkgray){//センサーが黒灰なら turn_left;//左折 }else if(SENSOR_1>lightgray){//センサーが白灰なら turn_right;//右折 t0=CurrentTick();//時間をリセット }else if(SENSOR_1<35){//明るさ35以下で go_straight;//直進 t0=CurrentTick();//時間をリセット }else{//それ以外は rotate_right;//右旋回 t0=CurrentTick();//時間をリセット } } Off(OUT_BC);//時刻が一定の値を超えると停止 Wait(1000);//1秒間停止 }
交差点までラインの右側をトレースするサブルーチン
sub followline_R(int stop_time) { long t0=CurrentTick(); while(CurrentTick()-t0<stop_time){//現在の時刻が一定の値を超えるまで if(SENSOR_1<black){//センサーが黒の時 rotate_right;//右旋回 }else if(SENSOR_1<darkgray){//センサーが黒灰なら turn_right;//右折 }else if(SENSOR_1>lightgray){//センサーが白灰なら turn_left;//左折 t0=CurrentTick();//時間をリセット }else if(SENSOR_1<35){//センサーが35以下なら go_straight;//直進 t0=CurrentTick();//時間をリセット }else{//それ以外は rotate_left;//左旋回 t0=CurrentTick();//時間をリセット } } Off(OUT_BC);//一定の時間を超えると停止 Wait(1000);//一秒間停止 }
J地点でのT字路まで右側をトレースするサブルーチン
sub followline_R2(int stop_time) { long t0=CurrentTick(); while(CurrentTick()-t0<stop_time){//時刻がある一定の値より小さいとき if(SENSOR_1<black){//黒なら rotate_right;//右旋回 t0=CurrentTick();//時間をリセット }else if(SENSOR_1<darkgray){//灰黒色なら turn_right;//右折 t0=CurrentTick();//時間をリセット }else if(SENSOR_1>lightgray){//灰白色なら turn_left;//左折 }else if(SENSOR_1<35){//明るさ35以下で go_straight2;//ゆっくり直進 t0=CurrentTick();時間をリセット }else{//それ以外なら rotate_left;//左旋回 } } Off(OUT_BC);//停止 Wait(2000);//2秒間停止 }
黒から白,白から黒など,交差点での方向の微調整をするサブルーチンを作ります.
sub Blackto_White()//黒から白へ { while(SENSOR_1<darkgray){灰黒色の間 OnFwd(OUT_BC,15);//直進 } Off(OUT_BC);//停止 }
sub Whiteto_Black()//白から黒まで直進 { while(SENSOR_1>darkgray){//灰黒色まで OnFwd(OUT_BC,15);//直進 } Off(OUT_BC);//停止 }
sub Whiteto_Black_R()//白から黒まで右旋回 { while(SENSOR_1>darkgray){//灰黒色まで OnFwd(OUT_C,20); OnRev(OUT_B,20);}//右旋回 Off(OUT_BC); }
sub Blackto_White_R()//黒から白へ右旋回 { while(SENSOR_1<darkgray){//灰黒の間 OnFwd(OUT_C,20); OnRev(OUT_B,20);//右旋回 } Off(OUT_BC); }
sub Blackto_White_L()//黒から白へ左旋回 { while(SENSOR_1<lightgray){//薄灰のとき OnFwd(OUT_B,20); OnRev(OUT_C,20);//左に旋回 } Off(OUT_BC); }
sub Whiteto_Black_L()白から黒へ左旋回 { while(SENSOR_1>black){//黒の時 OnFwd(OUT_B,20); OnRev(OUT_C,20);//左旋回 } Off(OUT_BC); }
task main() { SetSensorLight(S1); followline_R(150);//右側をトレース(B→K) Blackto_White_L();//黒から白へ followline_L(200);// Blackto_White_R();//位置調節 followline_R2(200);//右側をトレース(K→L) catch;//ピンポン玉をつかむ turn;//180°回転 followline_R(150);右側をトレース(K→J) Blackto_White();//位置調節 followline_L(150);//左側をトレース(J→I) Blackto_White_L();//位置調節 followline_L(200);//左側をトレース(I→H) Blackto_White();//位置調節 followline_L(200);//左側をトレース(H→G) Blackto_White();//位置調節 followline_L(200);//左側をトレース(G→F) Blackto_White();//位置調節 followline_R(150);//右側をトレース(F→D) Blackto_White();//位置調節 followline_R(150);//右側をトレース(D→B) release;//ピンポン玉を離す osidasi;//ピンポン玉を押し出す }