チョウ カイドウ(自分) 横川 友也 大谷 風吾 カ デンエン
1.フィールドは課題2で使用した紙を使用する。 2.図のマジェンタの円に逆さまにした紙コップを置き、その中にピンポン玉を2つずつ入れておく。 3.図の黄色い円に紙コップを通常の向きに置き、障害物とする。直径6cmの円の外周をペンまたは鉛 筆でマークしておく。 4.紙コップには色をつけたり文字や記号を書いてもよい。 5.割り箸を使って一辺が14cmの正方形を作り、X地点を中心とした円に接するように置く。
1.競技時間は審判が続行不能と判断するまで、あるいはリタイアするまで。 2.図のA地点または(および)D地点からスタートする。ただし接地している部分はそれぞれの領域 内に収まるものとする(線上はOK)。上空部分は領域からはみ出していてもよい。 3.ピンポン玉をX地点に運び、割り箸で正方形に囲まれた領域に入れる。 4.紙コップをY地点がある黒線に囲まれた領域に運ぶ。 5.開始の合図から5秒以内にスタートボタンを押す作業を完了すること。 6.競技が終了するまで、ロボットに触ったり人間が遠隔で操作してはならない。 7.途中でうまく動かなくなった場合、1回限り再スタートすることができる(再スタートの際に 別プログラムで起動してよい)
まずは超音波センサが降ろしている状態でコップを見つける。その後超音波センサを上げて、ロボットが前に進むと共に、コップをロボットの中押し詰める
そして、コップを掴んでまま、超音波センサを下げる、また新しいコップを見つけて、超音波センサを上げて、コップを掴む手も上げる
そして、そのまま上に置く
こういう流れでやっていこうと思っていた
#define SPEED 100
#define SPEED_SLOW 60
#define turn_left1 OnFwd(OUT_A,80 * x); OnRev(OUT_C,35 * x);
#define turn_left2 OnFwd(OUT_A,100 * x); OnFwd(OUT_C,40 * x);
#define turn_left3 OnFwd(OUT_A,80 * x); OnRev(OUT_C,90 * x);
#define turn_left5 OnFwd(OUT_A,80 * x); OnRev(OUT_C,1 * x);
#define go_forward OnFwd(OUT_A,80 * x); OnFwd(OUT_C,87 * x);
#define go_back OnRev(OUT_A,100 * x); OnRev(OUT_C,85 * x);
#define turn_right1 OnRev(OUT_A,35 * x); OnFwd(OUT_C,80 * x);
#define turn_right2 OnFwd(OUT_A,30 * x); OnFwd(OUT_C,100 * x);
#define turn_right3 OnRev(OUT_A,80 * x); OnFwd(OUT_C,80 * x);
#define AD SendRemoteNumber(1,MAILBOX1,01); //アームを閉じて締める
#define AU SendRemoteNumber(1,MAILBOX1,02); //アームを上げる
#define TD OnRev(OUT_B,45 * x); Wait(1000);
#define TU OnFwd(OUT_B,45 * x); Wait(1000);
#define CupOff SendRemoteNumber(1,MAILBOX1,03); //コップを離す
#define turn_right4 OnRev(OUT_A,80 * x); OnFwd(OUT_C,60 * x);
#define turn_right5 OnRev(OUT_A,80 * x); OnFwd(OUT_C,1 * x);
#define turn_right6 OnRev(OUT_A,1 * x); OnFwd(OUT_C,80 * x);
#define DO 523
#define x 0.65 //かけ数
#define z 1.0
sub IKK() { int BP = 0; while (BP < 125){ if (SENSOR_2 < 45) { turn_right1; BP++; }else{ if (SENSOR_2 < 49 ) { turn_right2; } else if (SENSOR_2 < 56 ) { go_forward; } else if (SENSOR_2 < 58) { turn_left2; } else { turn_left1; } BP = 0; } Wait(1); } Off(OUT_AC); PlayTone(DO,100); Wait(100); Wait(10); BP = 0; }
sub DKK() { int BP = 0; while (BP < 170){ if (SENSOR_2 < 45) { turn_left1; BP++; }else{ if (SENSOR_2 < 49 ) { turn_left2; } else if (SENSOR_2 < 54 ) { go_forward; } else if (SENSOR_2 < 55) { turn_right3; } else { turn_right4; } BP = 0; } Wait(1); } Off(OUT_AC); PlayTone(DO,100); Wait(100); BP = 0; }
sub DKK2() { int BP = 0; while (BP < 170){ if (SENSOR_3 < 45) { turn_left1; BP++; }else{ if (SENSOR_3 < 49 ) { turn_left2; } else if (SENSOR_3 < 54 ) { go_forward; } else if (SENSOR_3 < 55) { turn_right3; } else { turn_right4; } BP = 0; } Wait(1); } Off(OUT_AC); PlayTone(DO,100); Wait(100); BP = 0; }
const float diameter = 5.45; //タイヤ直径 const float track = 10.35; //タイヤトレッド幅 const float pi = 3.1415; //円周率
void fwdDist(float d) //距離 dcm前進 { long angle = d/(diameter*pi)*360.0; //必要なタイヤの回転角度 RotateMotorEx(OUT_AC, SPEED_SLOW, angle, 0, true ,true); }
void turnAng(long ang) //角度ang度の時計回りの旋回 { long angle = track/diameter * ang; RotateMotorEx(OUT_AC, SPEED_SLOW, angle, 100, true, true); }
int searchDirection(long ang) //現在の方向を中心にang度の範囲で探し //障害物までの距離を返す { long tacho_min; //最も近い距離を実現するタイヤの回転数 int d_min = 300; //最も近い距離の仮の最小値(十分大きくとっておく) long angle = (track/diameter)*ang; //旋回角度からタイヤの回転を計算 turnAng(ang/2); //指定された角度の半分を旋回 ResetTachoCount(OUT_AC); //角度計測をリセット
OnFwdSync(OUT_AC,SPEED_SLOW,-100); //半時計回りに旋回 while(MotorTachoCount(OUT_A)<=angle) { if (SensorUS(S1)<d_min){ //現在の距離が借りの最小値より小さい場合 d_min = SensorUS(S1); //仮の最小値を更新 tacho_min = MotorTachoCount(OUT_A); //この時のタイヤの回転数を記録 } } OnFwdSyncEx(OUT_AC,SPEED_SLOW,100,RESET_NONE); until(MotorTachoCount(OUT_A)<=tacho_min || SensorUS(S1)<=d_min);
Wait(14); //微調整 Off(OUT_AC);Wait(500); return d_min; }
task main() { SetSensorLowspeed(S1); SetSensorLight(S2); SetSensorLight(S3); go_forward; Wait(500); //少し前進 turn_left3; Wait(200); while (SENSOR_2 > 45) { turn_left3; }
turn_right1; Wait(100); IKK(); //Fまでライントレース turn_left3; Wait(1000); //左向く
int d = searchDirection(70);//コップ探す if (d > 10){ fwdDist(d-10.0);//コップに近づく }
TU; //超音波センサーをあげる
go_forward; Wait(2900); AD; //コップを取る Off(OUT_AC); Wait(3000); go_back; Wait(500);
turn_left3; Wait(700);
while (SENSOR_2 > 45) { go_back; } go_back; Wait(250); Off(OUT_AC); Wait(200);
while (SENSOR_2 > 45) { go_back; }
go_back; Wait(450);
Off(OUT_AC); Wait(450);
while (SENSOR_2 > 45) { go_back; }
go_back; Wait(250);
Off(OUT_AC); Wait(200);
turn_right5; Wait(700);
while (SENSOR_3 > 45) { turn_right5; }
DKK2(); //Sを左折 //Qに到着 turn_right6; Wait(1100); while (SENSOR_2 > 45) { turn_right6; }
turn_right6; Wait(300);
Off(OUT_AC); Wait(1000);
go_forward; //Xに入る Wait(2500); AU; //Xでコップを上げる Off(OUT_AC); Wait(3000);
go_back; Wait(1200); Off(OUT_AC); Wait(2000); while (SENSOR_2 > 45) { go_back; } go_back; Wait(500);
while (SENSOR_2 > 45) { go_back; }
Off(OUT_AC); Wait(1000); turn_right5; Wait(500);
while (SENSOR_3 > 45) { turn_right5; } turn_right5; Wait(600);
while (SENSOR_3 > 45) { turn_right5; }
Off(OUT_AC); Wait(1000);
go_forward; Wait(1000);
while (SENSOR_2 > 45) { go_forward; } Off(OUT_AC); Wait(1000);
while (SENSOR_3 > 45) { turn_left5; } turn_left5; Wait(300);
go_forward; Wait(500);
TD; //超音波下げる
Off(OUT_AC); Wait(2000); d = searchDirection(120);//コップ探す if (d > 10){ fwdDist(d-10.0);//コップに近づく } TU; //超音波センサーをあげる Off(OUT_AC); Wait(1000); go_forward; Wait(1300);
Off(OUT_AC); Wait(1000);
turn_left3; Wait(1400);
Off(OUT_AC); Wait(2000); go_forward; Wait(2300); go_back; Wait(100); Off(OUT_AC); Wait(500); AD; //コップを取る(重ねる) Off(OUT_AC); Wait(3000);
go_back; //kaiki Wait(1900); turn_right3; Wait(1400); go_back; Wait(1300);
while (SENSOR_2 > 45) { go_back; }
turn_left5; Wait(1200); while (SENSOR_3 > 45) { turn_left5; }
turn_right5; Wait(300); go_forward; Wait(1000); while (SENSOR_2 > 45) { go_forward; //Xまで直進 }
AU; //Xでコップを上げる
turn_left3; Wait(200); go_back; Wait(300);
CupOff; while (SENSOR_2 > 45) { go_back; } //黒い線までバックする
turn_right3; Wait(1000); //右折する
go_forward; Wait(1000); //少し直進する
d = searchDirection(120); //コップ探す if (d > 10){ fwdDist(d-10.0);//コップに近づく } TU; //超音波センサーをあげる go_forward; Wait(1000); AD; //コップを取る
go_back; Wait(1000); //バックしてコースにもどる DKK(); //Bまでライントレースする
turn_left3; //左折する Wait(1000); DKK(); //Pに到着
turn_right3; Wait(200); go_forward; Wait(1000); //Xにはいる
AU; //コップを上げる
go_back; Wait(1000); //Xから出る turn_left3; //左を向く Wait(1000);
DKK(); //PからQまで円周上をライントレース turn_left3; Wait(1000); DKK(); //Sに到着
turn_right3; Wait(200); go_forward; //Yに入る Wait(1000); AD; //コップを置く CupOff; //コップ離す } //終わり
#define ArmDown OnRev(OUT_A,50); Wait(500); #define ArmUp OnFwd(OUT_A,70); Wait(300); #define ArmClose OnRev(OUT_C,70); Wait(1000); #define ArmOpen OnFwd(OUT_C,30); Wait(500); #define ArmStay OnRev(OUT_AC,1);
task main () { int msg; while(true) { ReceiveRemoteNumber(MAILBOX1,true,msg); if (msg == 01) { Off(OUT_AC); ArmDown; ArmClose; msg = 04; } if (msg == 02) { ArmStay; ArmUp; msg = 04; } if (msg == 03) { Off(OUT_AC); ArmUp; ArmOpen; } if (msg == 04) { ArmStay; Off(OUT_A); } } }
詳しい部分はプログラミングの作者:大谷 風吾のページを参考ください。 http://yakushi.shinshu-u.ac.jp/robotics/?2017b%2FMember%2Fwaitu%2FMission3
結果は0点だった、コップを仕組みはそんなに精確ではなかった。それと、時間が足りなく、三つ 目のコップを探すプログラミングはできなかった。
もっと早めに完成すべきだった、ロボット本体はもっと頑丈に作るべきだった