目次
詳細はこちら2019a/Mission2
ピンポン玉はロボット前部のU字型の部分で掬うようにして持ち上げられるようにした.またプログラム上ロボットは線の中央を走らないため玉を持ち上げる部分は光センサにずらして設置してある.
ピンポン玉を持ち上げるには
この2つの動作が必要である.よってアームが上下するプログラムを以下のように組む.
#define up OnRev(OUT_C,20);Wait(500);Off(OUT_C); #define down OnFwd(OUT_C,20);Wait(500);Off(OUT_C);
まずロボットの基本的な動きを以下のように定義する
#define left0 OnFwd(OUT_A,20);OnRev(OUT_B,20); #define left1 OnFwd(OUT_A,20);Off(OUT_B); #define right0 OnFwd(OUT_B,20);OnRev(OUT_A,20); #define right1 OnFwd(OUT_B,20);Off(OUT_A); #define forward OnFwd(OUT_AB,20);
定義した動作をまとめると下の通りになる.
プログラム名 | 内容 |
left0 | 左旋回 |
left1 | 左折 |
right0 | 右旋回 |
right1 | 右折 |
forward | 直進 |
またコース上の光センサの値は線の黒い(暗い)箇所で最低値41,また白紙の箇所で最高値71だった.よって閾値を以下のように定義し,暗い側からstep1とした
#define step1 45 #define step2 53 #define step3 61 #define step4 69 #define step5 75
線に沿って移動させるため,ロボットの光センサが常に線の淵を沿うようにプログラムする.この時,線の境界上にある時は直進,線状や線から外れた時は右左折またはどちらかに旋回するようにする.交差点では1秒間停止させなければならないため,発進して線を辿り交差点で停止するサブルーチンを同時にプログラムする.交差点は光センサが線上の黒い部分を一定時間以上認識した時とする.このため交差点で停止した時,ロボットは線に沿って斜めに停止することになる.
線の左右の淵に沿って移動するプログラムは以下のとおりである.ミッションより丁字路でロボットは一時停止しなければならないため,コース上ロボットは場所によって線の左右どちらの境界も辿れるようにしなければならない.ここでは線の左側を辿るサブルーチンをline_left,右側を辿るサブルーチンをline_rightとした.
sub line_left() //線の左側を走行するプログラム { SetSensorLight(S3);//光センサを起動 long t0=0,t1=0,tm=1;//交差点認識の為に時間を定義 while(t1-t0<tm){ //交差点でない時は実行するプログラム if(SENSOR_3<step1){ //線中央にあるとき left0;t0=CurrentTick(); //左旋回し時間を計測 until(SENSOR_3<step3); //境界を認識するまで続ける t1=CurrentTick(); //旋回した時間を計測 }else if(SENSOR_3<step2){ //線上に入ったとき left1; //左折する }else if(SENSOR_3<step3){ //境界上にあるとき forward; //直進する }else if(SENSOR_3<step4){ //線から外れたとき right1; //右折する }else if(SENSOR_3<step5){ //完全に線から外れたとき right0; //右折する } } //交差点に入ったとき Off(OUT_AB); //停止する(1秒間) Wait(1000); }
sub line_right() //線の右側を走行するプログラム { SetSensorLight(S3);//光センサを起動 long t0=0,t1=0,tm=1;//交差点認識の為に時間を定義 while(t1-t0<tm){ //交差点でない時は実行するプログラム if(SENSOR_3<step1){ //線中央にあるとき right0; t0=CurrentTick(); //右旋回し時間を計測 until(SENSOR_3<step3); //境界を認識するまで続ける t1=CurrentTick(); //旋回した時間を計測 }else if(SENSOR_3<step2){ //線上にあるとき right1; //右折する }else if(SENSOR_3<step3){ //境界上にあるとき forward; //直進する }else if(SENSOR_3<step4){ //線から外れたとき left1; //左折する }else if(SENSOR_3<step5){ //完全に線から離れたとき left0; //左旋回する } } //交差点にある時 Off(OUT_AB); //停止する(1秒間) Wait(1000); }
コースはA→M→B→Aとした.コースを走るためのプログラムは以下のとおりである.
#define step1 45 #define step2 53 #define step3 61 #define step4 69 #define step5 75 #define left0 OnFwd(OUT_A,20);OnRev(OUT_B,20); #define left1 OnFwd(OUT_A,20);Off(OUT_B); #define right0 OnFwd(OUT_B,20);OnRev(OUT_A,20); #define right1 OnFwd(OUT_B,20);Off(OUT_A); #define forward OnFwd(OUT_AB,20); #define up OnRev(OUT_C,20);Wait(500);Off(OUT_C); #define down OnFwd(OUT_C,20);Wait(500);Off(OUT_C); sub line_left() { SetSensorLight(S3); long t0=0,t1=0,tm=15; while(t1-t0<tm){ if(SENSOR_3<step1){ t0=CurrentTick(); left0;until(SENSOR_3<step3); t1=CurrentTick(); }else if(SENSOR_3<step2){ left1; }else if(SENSOR_3<step3){ forward; }else if(SENSOR_3<step4){ right1; }else if(SENSOR_3<step5){ right0; } } Off(OUT_AB); Wait(1000); } sub line_right() { SetSensorLight(S3); long t0=0,t1=0,tm=15; while(t1-t0<tm){ if(SENSOR_3<step1){ t0=CurrentTick(); right0; until(SENSOR_3<step3); t1=CurrentTick(); }else if(SENSOR_3<step2){ right1; }else if(SENSOR_3<step3){ forward; }else if(SENSOR_3<step4){ left1; }else if(SENSOR_3<step5){ left0; } } Off(OUT_AB); Wait(1000); } task main() { long t=1000; //交差点で停止後に進む時間をt=1秒とする forward; //Aから出発 Wait(1000); line_left(); //MからKまで進みKで停止 forward; //交差点を直進 Wait(t); line_left(); //KからLまで進みKで停止 up; //ピンポン玉を拾う left0; //Lで回転し方向転換 Wait(2000); line_right(); //LからKまで進みKで停止 right0; //Lで回転し方向転換 Wait(t); line_right(); //KからJまで進みJで停止 left1; //Jで回転 Wait(t); line_left(); //JからIまで進みIで停止 repeat(2){ forward; //丁字路を直進 Wait(t); line_left(); //コースを進む } //IからH,HからGまで進みGで停止 forward; //丁字路を直進 Wait(t); line_right(); //GからF,Eを通りDまで進みDで停止 forward; //交差点を直進 line_right(); //DからCを通りBまで進みBで停止 down; //ピンポン玉を置く OnRev(OUT_AB,20); //後進 Wait(500); OnFwd(OUT_AB,100); //勢いをつけてボールに接触,シュート Wait(t); Off(OUT_AB);//静止 }
試走させるたびに光センサの値が変化したため毎回の調整が必要だった.また一切のプログラムとロボットの構造を変更していないいもかかわらず唐突に交差点を認識しなくなったり,逆にカーブで停止してしまうことが多々ありこれも閾値やtmの数値の調整が毎回必要であった.交差点とカーブの認識のズレは光センサの数値の変化のように明確な原因がわからずこの対応に多くの時間がかかった.