![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
#author("2020-02-14T18:40:00+09:00","Hayashi","Hayashi") #author("2020-02-14T18:43:35+09:00","Hayashi","Hayashi") [[2019b/Member]] *目次 [#m04647f0] #contents *課題3 [#z24163fc] 青と赤のボールを運搬して、空き缶の上に載せる。 #ref(kadai.png) -空き缶を6本用意し、そのうち3本をC,E,Fの交差点に描いた半径4cmの円内に置き、 その上にボールを載せておく。 -同様に残りの空き缶をC', E', F' 地点に置く。 **ボールを運ぶ側(自分担当） [#oc7024b6] +まず、Cでボールをつかみ、C→G→H→Iの順番で、缶を運ぶほうの機械に渡す。 +次に、I→H→G→Fの道を通り、Fでボールをつかんだ後、F→G→H→Iに行き、ボールを渡す。 +最後に、I→H→G→D→Eの道を通り、Eでボールをつかんだ後、E→D→G→H→Iに行き、ボールを渡して終了。 *ロボットの説明 [#y32fba97] **ボールを運ぶ側（自分担当） [#d83cded7] 下のような、ロボットを作りました。 #ref(5.jpeg) 缶よりも高い位置にある、ボールをとれるように、アームの高さを、真ん中にある、黒い歯車の大きさを大きくすることで、調節しました。また、光センサーを用いて、ライントレースするようにし、超音波センサーを用いて、缶までの距離を測り、近くに来ると、アームがと閉じてボールを掴む仕組みです。 #ref(2.jpeg) 歯車を回している間や、車体が移動している間に、固定をしないと、アームがぶれるので、固定をしました。 #ref(4.jpeg) 左から見ると、こんな感じです。 **缶を運ぶ側（相手担当） [#o658b4bd] #ref(9.jpeg) コントローラーを上部に持っていき、ロボットの土台とコントローラーの間に缶を掴む用のアームを動かすためのモーターを組み込みました．また、コントローラーから超音波センサーを吊り下げるように下向きに取り付け，これによって，缶がアームに入っている状態では超音波センサーの値が小さくなります。 #ref(10.jpeg) 缶を掴むためのアームは、上下で缶を支えるようにしてあります。そして，４つあるアームのうち一つを長くすることで、万が一，缶が完全にアーム内に入っていない状態でアームを閉じたとしても引き寄せられるようになります。また，光センサーを中心にうまく収め、アームと干渉しないようにアームとモーターの位置も調節しました。 *プログラムの説明 [#g9e0166e] **ボールを運ぶ側（自分担当） [#hd03e177] #define THRESHOLD 52 //閾値 #define SPEED_H 35 //ハイスピード #define SPEED_L 25 //ロウスピード #define onRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL); //右のタイヤと左のタイヤの速度定義 #define go_forward onRL(SPEED_L,SPEED_L); //前に進む #define turn_left1 onRL(SPEED_H,-SPEED_H); //両方の車輪を回して左に曲がる #define turn_left0 onRL(SPEED_H,0); //片方の車輪を回して左に曲がる #define turn_right0 onRL(0,SPEED_H); //片方の車輪を回して右に曲がる #define turn_right1 onRL(-SPEED_H,SPEED_H); //両方の車輪を回して右に曲がる #define STEP 1 //一回の判断で動作させる時間 #define nMAX 140 //通常のカーブとして許容できる繰り返しの最大値 #define short_break Off(OUT_BC);Wait(1000); //休憩 #define cross_line onRL(SPEED_L,SPEED_L);Wait(1000); //交差点を渡る #define u_turnl1 OnRev(OUT_BC,SPEED_L);Wait(200);RotateMotorEx(OUT_BC,SPEED_H,angle2,-100,true,true); //Uターンする #define u_turnr1 OnRev(OUT_BC,SPEED_L);Wait(1400);RotateMotorEx(OUT_BC,SPEED_H,angle1,100,true,true); //Uターンする #define u_turnl2 OnRev(OUT_BC,SPEED_L);Wait(1200);RotateMotorEx(OUT_BC,SPEED_H,angle2,-100,true,true); //Uターンする #define u_turnr2 OnRev(OUT_BC,SPEED_L);Wait(1000);RotateMotorEx(OUT_BC,SPEED_H,angle2,100,true,true); //Uターンする float GetAngle(float d) { const float diameter = 5.5; const float distance=12; float ang = (distance*d)/diameter; return ang; } sub catch_balll() { SetSensorLight(S4); SetSensorLowspeed(S1); int catch=0; while(catch<1){ while(SensorUS(S1)>=6.1){ if(SENSOR_4<THRESHOLD-15){ turn_left1; }else{ if(SENSOR_4<THRESHOLD-7){ turn_left0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_right0; }else{ turn_right1; } } Wait(STEP); } short_break; OnFwd(OUT_A,-22); Wait(2000); Off(OUT_A); catch++; } } sub follow_liner() { SetSensorLight(S4); int nOnline=0; int cross=0; while(cross<2){ while(nOnline<nMAX){ if(SENSOR_4<THRESHOLD-15){ turn_right1; nOnline++; }else{ if(SENSOR_4<THRESHOLD-7){ turn_right0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_left1; }else{ turn_left0; } nOnline=0; } Wait(STEP); } short_break; turn_right1; Wait(1000); nOnline=0; cross++; } } sub catch_ballr() { SetSensorLight(S4); SetSensorLowspeed(S1); int catch=0; while(catch<1){ while(SensorUS(S1)>=6.2){ if(SENSOR_4<THRESHOLD-15){ turn_right1; }else{ if(SENSOR_4<THRESHOLD-7){ turn_right0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_left1; }else{ turn_left0; } } Wait(STEP); } short_break; OnFwd(OUT_A,-22); Wait(2000); Off(OUT_A); catch++; } } sub follow_linel() { SetSensorLight(S4); int nOnline=0; int cross=0; while(cross<2){ while(nOnline<nMAX){ if(SENSOR_4<THRESHOLD-15){ turn_left1; nOnline++; }else{ if(SENSOR_4<THRESHOLD-7){ turn_left0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_right1; }else{ turn_right0; } nOnline=0; } Wait(STEP); } short_break; turn_left1; Wait(800); nOnline=0; cross++; } } sub catch_releaser() { SetSensorLight(S4); SetSensorLowspeed(S1); int catch=0; while(catch<1){ while(SensorUS(S1)>=6.2){ if(SENSOR_4<THRESHOLD-15){ turn_right1; }else{ if(SENSOR_4<THRESHOLD-7){ turn_right0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_left1; }else{ turn_left0; } } Wait(STEP); } short_break; OnFwd(OUT_A,20); Wait(500); Off(OUT_A); catch++; } } sub catch_releasel() { SetSensorLight(S4); SetSensorLowspeed(S1); int catch=0; while(catch<1){ while(SensorUS(S1)>=6.2){ if(SENSOR_4<THRESHOLD-15){ turn_left1; }else{ if(SENSOR_4<THRESHOLD-7){ turn_left0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_right1; }else{ turn_right0; } } Wait(STEP); } short_break; OnFwd(OUT_A,20); Wait(500); Off(OUT_A); catch++; } } sub follow_stop() { SetSensorLight(S4); int nOnline=0; int cross=0; while(cross<1){ while(nOnline<nMAX){ if(SENSOR_4<THRESHOLD-15){ turn_right1; nOnline++; }else{ if(SENSOR_4<THRESHOLD-7){ turn_right0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_left1; }else{ turn_left0; } nOnline=0; } Wait(STEP); } short_break; cross++; } } sub follow_line1() { SetSensorLight(S4); int nOnline=0; int cross=0; while(cross<1){ while(nOnline<nMAX){ if(SENSOR_4<THRESHOLD-15){ turn_right1; nOnline++; }else{ if(SENSOR_4<THRESHOLD-7){ turn_right0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_left1; }else{ turn_left0; } nOnline=0; } Wait(STEP); } short_break; turn_left1;Wait(nMAX*STEP); cross_line; nOnline=0; cross++; } } sub follow_line2() { SetSensorLight(S4); int nOnline=0; int cross=0; while(cross<1){ while(nOnline<nMAX){ if(SENSOR_4<THRESHOLD-15){ turn_left1; nOnline++; }else{ if(SENSOR_4<THRESHOLD-7){ turn_left0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_right1; }else{ turn_right0; } nOnline=0; } Wait(STEP); } short_break; cross_line; nOnline=0; cross++; } } task main() { int angle1 = GetAngle(90); int angle2 = GetAngle(60); OnFwd(OUT_BC,30); Wait(500); catch_balll(); u_turnl2; follow_stop(); OnFwd(OUT_BC,25); Wait(200); catch_releaser(); Wait(7500); u_turnl1; Wait(500); OnFwd(OUT_BC,25); Wait(100); follow_liner(); catch_ballr(); u_turnr1; follow_line2(); catch_releaser(); Wait(10000); u_turnl2; Wait(500); OnFwd(OUT_BC,25); Wait(100); follow_stop(); turn_right1; Wait(1000); follow_line1(); follow_stop(); turn_right1; Wait(1000); catch_ballr(); u_turnr1; follow_line2(); follow_line2(); catch_releaser(); } ***関数 [#f490d1ca] float GetAngle(float d) { const float diameter = 5.5; const float distance=12; float ang = (distance*d)/diameter; return ang; } この関数は、上で定義した"u-turn"での回る角度を計算するために書きました。 sub catch_balll() { SetSensorLight(S4); SetSensorLowspeed(S1); int catch=0; while(catch<1){ while(SensorUS(S1)>=6.1){ if(SENSOR_4<THRESHOLD-15){ turn_left1; }else{ if(SENSOR_4<THRESHOLD-7){ turn_left0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_right0; }else{ turn_right1; } } Wait(STEP); } short_break; OnFwd(OUT_A,-22); Wait(2000); Off(OUT_A); catch++; } } 左トレースして、ボールを取りに行くときに、この関数を使いました。 sub follow_liner() { SetSensorLight(S4); int nOnline=0; int cross=0; while(cross<2){ while(nOnline<nMAX){ if(SENSOR_4<THRESHOLD-15){ turn_right1; nOnline++; }else{ if(SENSOR_4<THRESHOLD-7){ turn_right0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_left1; }else{ turn_left0; } nOnline=0; } Wait(STEP); } short_break; turn_right1; Wait(1000); nOnline=0; cross++; } } 右トレースして、交差点を2回渡るときに、使いました。 sub catch_ballr() { SetSensorLight(S4); SetSensorLowspeed(S1); int catch=0; while(catch<1){ while(SensorUS(S1)>=6.2){ if(SENSOR_4<THRESHOLD-15){ turn_right1; }else{ if(SENSOR_4<THRESHOLD-7){ turn_right0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_left1; }else{ turn_left0; } } Wait(STEP); } short_break; OnFwd(OUT_A,-22); Wait(2000); Off(OUT_A); catch++; } } 右トレースして、ボールを取りに行くときに使いました。 sub follow_linel() { SetSensorLight(S4); int nOnline=0; int cross=0; while(cross<2){ while(nOnline<nMAX){ if(SENSOR_4<THRESHOLD-15){ turn_left1; nOnline++; }else{ if(SENSOR_4<THRESHOLD-7){ turn_left0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_right1; }else{ turn_right0; } nOnline=0; } Wait(STEP); } short_break; turn_left1; Wait(800); nOnline=0; cross++; } } 左トレースして、交差点を2回渡るときに、使いました。 sub catch_releaser() { SetSensorLight(S4); SetSensorLowspeed(S1); int catch=0; while(catch<1){ while(SensorUS(S1)>=6.2){ if(SENSOR_4<THRESHOLD-15){ turn_right1; }else{ if(SENSOR_4<THRESHOLD-7){ turn_right0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_left1; }else{ turn_left0; } } Wait(STEP); } short_break; OnFwd(OUT_A,20); Wait(500); Off(OUT_A); catch++; } } 右トレースして、ボールを真ん中で、缶に乗せるときに、使いました。 sub catch_releasel() { SetSensorLight(S4); SetSensorLowspeed(S1); int catch=0; while(catch<1){ while(SensorUS(S1)>=6.2){ if(SENSOR_4<THRESHOLD-15){ turn_left1; }else{ if(SENSOR_4<THRESHOLD-7){ turn_left0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_right1; }else{ turn_right0; } } Wait(STEP); } short_break; OnFwd(OUT_A,20); Wait(500); Off(OUT_A); catch++; } } 左トレースして、ボールを真ん中で、缶に乗せるときに使いました。 sub follow_stop() { SetSensorLight(S4); int nOnline=0; int cross=0; while(cross<1){ while(nOnline<nMAX){ if(SENSOR_4<THRESHOLD-15){ turn_right1; nOnline++; }else{ if(SENSOR_4<THRESHOLD-7){ turn_right0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_left1; }else{ turn_left0; } nOnline=0; } Wait(STEP); } short_break; cross++; } } 交差点で、右トレースを、左トレースにするときに、使いました。 sub follow_line1() { SetSensorLight(S4); int nOnline=0; int cross=0; while(cross<1){ while(nOnline<nMAX){ if(SENSOR_4<THRESHOLD-15){ turn_right1; nOnline++; }else{ if(SENSOR_4<THRESHOLD-7){ turn_right0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_left1; }else{ turn_left0; } nOnline=0; } Wait(STEP); } short_break; turn_left1;Wait(nMAX*STEP); cross_line; nOnline=0; cross++; } } 右トレースで、交差点を一回だけ、渡るときに使いました。 sub follow_line2() { SetSensorLight(S4); int nOnline=0; int cross=0; while(cross<1){ while(nOnline<nMAX){ if(SENSOR_4<THRESHOLD-15){ turn_left1; nOnline++; }else{ if(SENSOR_4<THRESHOLD-7){ turn_left0; }else if(SENSOR_4<THRESHOLD+7){ go_forward; }else if(SENSOR_4<THRESHOLD+15){ turn_right1; }else{ turn_right0; } nOnline=0; } Wait(STEP); } short_break; cross_line; nOnline=0; cross++; } } 左トレースで、交差点を1回渡るときに、使いました。 ***task main()の中身 [#te133ba1] int angle1 = GetAngle(90); int angle2 = GetAngle(60); 上のように、90度回転する場合と、60度回転する場合の2つを定義しました。 ****1個目 [#w3ba509f] OnFwd(OUT_BC,30); Wait(500); Aを出発 catch_balll(); J→H→G→Cまで左トレースして、缶を認識して、つかむ u_turnl2; follow_stop(); Cで左に回転して、空白のところを行き、線GH上に行く、そして、Hで止まる。 OnFwd(OUT_BC,25); Wait(200); catch_releaser(); Hの場所で、渡り、右トレースして、Iの場所で運んできた缶にボールを乗せる。 Wait(7500); 向こうの機械が、ボールが乗った缶を持って去るのを、待つ。 ****2個目 [#c839102a] u_turnl1; Wait(500); OnFwd(OUT_BC,25); Wait(100); Iの位置で、左回転し、右トレースをする。（曲がる間に交差点と認識させないために、少しだけ進ませる。） follow_liner(); catch_ballr(); HとGの交差点で、右に曲がるようにする。そして、G→Fでトレースして、ボールを掴むようにする。 u_turnr1; follow_line2(); catch_releaser(); Fで右回転し、空白の場所に行ったあと、線GHでライントレースするようにし、Hで渡り、右トレースして、Iでボールを運んできた缶に乗せる。 Wait(7500); ボールを乗せた管を運ぶ機械が、去るまで待つ。 ****3個目 [#s6cd99ae] u_turnl2; Wait(500); OnFwd(OUT_BC,25); Wait(100); Iで左回転し、右トレースするようにする。（曲がる間に交差点と認識させないために、少しだけ進ませる。） follow_stop(); turn_right1; Wait(1000); Hの交差点で止まり、右に曲がる。 follow_line1(); Gの交差点を一回だけ渡る。 follow_stop(); turn_right1; Wait(1000); Dの交差点で止まり、右に曲がる。 catch_ballr(); u_turnr1; D→Eでボールをつかんだ後、右に回転して、空白のところを行き、線DG上に行く。 follow_line2(); Gを渡る。 follow_line2(); catch_releaser(); Hを渡り、H→Iで右トレースして、運んできた缶にボールを乗せて、終了。 **缶を運ぶ側（相手担当） [#k7e2de8d] #define WHITE 56 // 白色 #define GWHITE 48 // 白灰色 #define GBLACK 40 // 黒灰色 #define BLACK 34 // 黒色 #define SPEED_H 47 // ハイスピード #define SPEED 43 // ノーマルスピード #define SPEED_L 40 // ロースピード #define STEP 1 #define OnRL(speedR,speedL) OnFwd(OUT_B,speedR);OnFwd(OUT_C,speedL); #define go OnFwdSync(OUT_BC,SPEED_H,0); // 直進 #define gofwd OnFwdSync(OUT_BC,SPEED_L,0); // 直進（スロー） #define left_rotation_s OnRL(SPEED,-SPEED); // 高速左回転 #define left_rotation OnRL(SPEED_L,-SPEED_L); // 左回転 #define turn_left Off(OUT_C);OnFwd(OUT_B,SPEED); // 左旋回 #define turn_right Off(OUT_B);OnFwd(OUT_C,SPEED); // 右旋回 #define right_rotation OnRL(-SPEED_L,SPEED_L); // 右回転 #define right_rotation_s OnRL(-SPEED,SPEED); // 高速右回転 float GetAngle(float d) { const float diameter = 5.45; const float pi=3.1415; const float distance=12.1; float ang = (distance*d)/diameter; return ang; } void turnR(long u) { int angle = GetAngle(u); RotateMotorEx(OUT_BC,SPEED,angle,100,true,true); } void turnL(long l) { int angle = GetAngle(l); RotateMotorEx(OUT_BC,SPEED,angle,-100,true,true); } void back(long b) { RotateMotorEx(OUT_BC,-SPEED,b,0,true,true); } void follow_line(long q) { SetSensorLight(S3); long t0; t0 = CurrentTick(); while(CurrentTick()-t0 < q){ if(SENSOR_3 <= BLACK){ left_rotation }else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){ turn_left t0 = CurrentTick(); }else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){ go t0 = CurrentTick(); }else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){ turn_right t0 = CurrentTick(); }else if(WHITE <= SENSOR_3){ right_rotation t0 = CurrentTick(); } Wait(STEP); } Off(OUT_BC); } void follow_lineR(long p) { SetSensorLight(S3); long t0; t0 = CurrentTick(); while(CurrentTick()-t0 < p){ if(SENSOR_3 <= BLACK){ right_rotation }else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){ turn_right t0 = CurrentTick(); }else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){ go t0 = CurrentTick(); }else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){ turn_left t0 = CurrentTick(); }else if(WHITE <= SENSOR_3){ left_rotation t0 = CurrentTick(); } Wait(STEP); } Off(OUT_BC); } void follow_line_for_intersection(long t) { SetSensorLight(S3); long t0; t0 = CurrentTick(); while(CurrentTick()-t0 < t){ if(SENSOR_3 <= BLACK){ left_rotation_s }else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){ turn_left }else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){ go }else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){ turn_right }else if(WHITE <= SENSOR_3){ right_rotation } Wait(STEP); } Off(OUT_BC); } void follow_line_for_intersectionR(long y) { SetSensorLight(S3); long t0; t0 = CurrentTick(); while(CurrentTick()-t0 < y){ if(SENSOR_3 <= BLACK){ right_rotation_s }else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){ turn_right }else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){ go }else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){ turn_left }else if(WHITE <= SENSOR_3){ left_rotation } Wait(STEP); } Off(OUT_BC); } void fetch_ball() { SetSensorLowspeed(S2); long t0; t0 = CurrentTick(); while(CurrentTick()-t0 < 200){ if(SensorUS(S1) <= 15){ Off(OUT_BC); RotateMotor(OUT_A,-SPEED,70); }else{ gofwd t0=CurrentTick(); } } Off(OUT_BC); } void open() { OnFwd(OUT_A,30); Wait(400); Off(OUT_A); } void close() { OnRev(OUT_A,30); Wait(700); Off(OUT_A); } void followC() { SetSensorLowspeed(S2); SetSensorLight(S3); while(true){ int s=SensorUS(S2); if(s<15){ Off(OUT_BC); Wait(300); close(); int tenkan = GetAngle(70); RotateMotorEx(OUT_BC,SPEED,tenkan,100,true,true); break; }else{ if(SENSOR_3<BLACK){ RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true); }else if(SENSOR_3<GBLACK){ OnFwd(OUT_B,70); Off(OUT_C); }else if(SENSOR_3<GWHITE){ OnFwd(OUT_BC,100); Wait(10); Off(OUT_BC); }else if(SENSOR_3<WHITE){ Off(OUT_B); OnFwd(OUT_C,70); }else if(SENSOR_3>=WHITE){ RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true); } } } } void followF() { SetSensorLowspeed(S2); SetSensorLight(S3); while(true){ int s=SensorUS(S2); if(s<15){ Off(OUT_BC); Wait(300); close(); back(120); turnR(66); break; }else{ if(SENSOR_3<BLACK){ RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true); }else if(SENSOR_3<GBLACK){ OnFwd(OUT_B,70); Off(OUT_C); }else if(SENSOR_3<GWHITE){ OnFwd(OUT_BC,100); Wait(10); Off(OUT_BC); }else if(SENSOR_3<WHITE){ Off(OUT_B); OnFwd(OUT_C,70); }else if(SENSOR_3>=WHITE){ RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true); } } } } void followE() { SetSensorLowspeed(S2); SetSensorLight(S3); while(true){ int s=SensorUS(S2); if(s<15){ Off(OUT_BC); Wait(300); close(); back(120); turnR(50); break; }else{ if(SENSOR_3<BLACK){ RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true); }else if(SENSOR_3<GBLACK){ Off(OUT_B); OnFwd(OUT_C,70); }else if(SENSOR_3<GWHITE){ OnFwd(OUT_BC,100); Wait(10); Off(OUT_BC); }else if(SENSOR_3<WHITE){ OnFwd(OUT_B,70); Off(OUT_C); }else if(SENSOR_3>=WHITE){ RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true); } } } } void catch() { SetSensorLowspeed(S2); long tO; tO = CurrentTick(); while(CurrentTick()-tO < 10000){ int O = SensorUS(S2); if(O <= 11){ break; }else if(11 < O){ Wait(STEP); } Wait(STEP); Wait(2500); } } task main() { RotateMotorEx(OUT_BC,SPEED_H,180,0,true,true); follow_line_for_intersection(10000); follow_line(200); follow_line_for_intersection(1000); follow_line(200); follow_line_for_intersection(1500); followC(); // C開始 follow_line_for_intersection(750); follow_line(150); RotateMotor(OUT_C,SPEED,150); turnR(40); RotateMotorEx(OUT_BC,SPEED_L,60,0,false,true); follow_line_for_intersection(1000); follow_line(200); follow_line_for_intersection(3000); Wait(10000); RotateMotor(OUT_C,-SPEED,330); RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true); turnL(40); follow_lineR(135); RotateMotor(OUT_C,SPEED,70); follow_line_for_intersection(1000); follow_line(180); follow_line_for_intersection(1000); follow_line(180); back(120); open(); // C終了 back(160); turnR(130); RotateMotorEx(OUT_BC,SPEED_L,40,0,false,true); follow_line_for_intersection(2000); followF(); // F開始 follow_line_for_intersection(1000); follow_line(200); follow_line_for_intersection(3000); Wait(9000); RotateMotor(OUT_C,-SPEED,330); RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true); turnL(40); follow_lineR(140); follow_line_for_intersectionR(1000); follow_lineR(140); RotateMotor(OUT_C,SPEED_H,90); follow_line(160); back(120); open(); //F 終了 back(150); turnL(77); RotateMotorEx(OUT_BC,SPEED_L,50,0,false,true); follow_line_for_intersectionR(1000); follow_lineR(140); follow_line_for_intersectionR(2000); followE(); // E開始 follow_line_for_intersection(1500); follow_line(160); RotateMotor(OUT_C,SPEED,75); follow_line_for_intersection(500); follow_line(160); follow_line_for_intersection(3000); Wait(15000); RotateMotor(OUT_C,-SPEED,330); RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true); turnL(40); follow_lineR(150); follow_line_for_intersectionR(1500); follow_lineR(160); RotateMotorEx(OUT_BC,SPEED_L,65,0,false,true); RotateMotor(OUT_B,SPEED,30); follow_line_for_intersectionR(1000); follow_lineR(150); follow_line_for_intersectionR(1750); follow_lineR(140); back(120); turnL(15); open(); // E終了 back(300); } ***関数 [#vd84cf21] float GetAngle(float d) { const float diameter = 5.45; const float pi=3.1415; const float distance=12.1; float ang = (distance*d)/diameter; return ang; } 上と同様です． void turnR(long u) { int angle = GetAngle(u); RotateMotorEx(OUT_BC,SPEED,angle,100,true,true); } void turnL(long l) { int angle = GetAngle(l); RotateMotorEx(OUT_BC,SPEED,angle,-100,true,true); } void back(long b) { RotateMotorEx(OUT_BC,-SPEED,b,0,true,true); } 上から順に，入力した数値だけ右回転，左回転，後退する関数． void follow_line(long q) { SetSensorLight(S3); long t0; t0 = CurrentTick(); while(CurrentTick()-t0 < q){ if(SENSOR_3 <= BLACK){ left_rotation }else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){ turn_left t0 = CurrentTick(); }else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){ go t0 = CurrentTick(); }else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){ turn_right t0 = CurrentTick(); }else if(WHITE <= SENSOR_3){ right_rotation t0 = CurrentTick(); } Wait(STEP); } Off(OUT_BC); } void follow_lineR(long p) { SetSensorLight(S3); long t0; t0 = CurrentTick(); while(CurrentTick()-t0 < p){ if(SENSOR_3 <= BLACK){ right_rotation }else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){ turn_right t0 = CurrentTick(); }else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){ go t0 = CurrentTick(); }else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){ turn_left t0 = CurrentTick(); }else if(WHITE <= SENSOR_3){ left_rotation t0 = CurrentTick(); } Wait(STEP); } Off(OUT_BC); } ライントレースは定義した白から黒までの4段階感知を使ってトレースするようにしました。灰色のときは直進で黒灰、白灰では片方のタイヤのみ回転する旋回で、黒、白を感知すると左右のタイヤを逆回転させて大幅に修正するようにしています。黒をq[ms]間感知すると交差点と判断し停止します。変数qは交差点の感知する感度を調整することができるようにするために導入しました。follow_lineは黒線の左トレースで，follow_lineRは黒線の右側をトレースするようになっています。どちらも黒が続いた時間で交差点を判断します。 void follow_line_for_intersection(long t) { SetSensorLight(S3); long t0; t0 = CurrentTick(); while(CurrentTick()-t0 < t){ if(SENSOR_3 <= BLACK){ left_rotation_s }else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){ turn_left }else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){ go }else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){ turn_right }else if(WHITE <= SENSOR_3){ right_rotation } Wait(STEP); } Off(OUT_BC); } void follow_line_for_intersectionR(long y) { SetSensorLight(S3); long t0; t0 = CurrentTick(); while(CurrentTick()-t0 < y){ if(SENSOR_3 <= BLACK){ right_rotation_s }else if((BLACK < SENSOR_3) && (SENSOR_3 < GBLACK)){ turn_right }else if((GBLACK <= SENSOR_3) && (SENSOR_3 <= GWHITE)){ go }else if((GWHITE < SENSOR_3) && (SENSOR_3 < WHITE)){ turn_left }else if(WHITE <= SENSOR_3){ left_rotation } Wait(STEP); } Off(OUT_BC); } 上記の関数はfollow_line、follow_lineRの交差点を感知せず指定時間だけひたすらライントレースをするプログラムです。時間指定は変数yになっています。以下、説明部ではintersectionと略して使用します。 void fetch_ball() { SetSensorLowspeed(S2); long t0; t0 = CurrentTick(); while(CurrentTick()-t0 < 200){ if(SensorUS(S1) <= 15){ Off(OUT_BC); RotateMotor(OUT_A,-SPEED,70); }else{ gofwd t0=CurrentTick(); } } Off(OUT_BC); } void open() { OnFwd(OUT_A,30); Wait(400); Off(OUT_A); } void close() { OnRev(OUT_A,30); Wait(700); Off(OUT_A); } void followC() { SetSensorLowspeed(S2); SetSensorLight(S3); while(true){ int s=SensorUS(S2); if(s<15){ Off(OUT_BC); Wait(300); close(); int tenkan = GetAngle(70); RotateMotorEx(OUT_BC,SPEED,tenkan,100,true,true); break; }else{ if(SENSOR_3<BLACK){ RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true); }else if(SENSOR_3<GBLACK){ OnFwd(OUT_B,70); Off(OUT_C); }else if(SENSOR_3<GWHITE){ OnFwd(OUT_BC,100); Wait(10); Off(OUT_BC); }else if(SENSOR_3<WHITE){ Off(OUT_B); OnFwd(OUT_C,70); }else if(SENSOR_3>=WHITE){ RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true); } } } } void followF() { SetSensorLowspeed(S2); SetSensorLight(S3); while(true){ int s=SensorUS(S2); if(s<15){ Off(OUT_BC); Wait(300); close(); back(120); turnR(66); break; }else{ if(SENSOR_3<BLACK){ RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true); }else if(SENSOR_3<GBLACK){ OnFwd(OUT_B,70); Off(OUT_C); }else if(SENSOR_3<GWHITE){ OnFwd(OUT_BC,100); Wait(10); Off(OUT_BC); }else if(SENSOR_3<WHITE){ Off(OUT_B); OnFwd(OUT_C,70); }else if(SENSOR_3>=WHITE){ RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true); } } } } void followE() { SetSensorLowspeed(S2); SetSensorLight(S3); while(true){ int s=SensorUS(S2); if(s<15){ Off(OUT_BC); Wait(300); close(); back(120); turnR(50); break; }else{ if(SENSOR_3<BLACK){ RotateMotorEx(OUT_BC,SPEED,5.0,70,true,true); }else if(SENSOR_3<GBLACK){ Off(OUT_B); OnFwd(OUT_C,70); }else if(SENSOR_3<GWHITE){ OnFwd(OUT_BC,100); Wait(10); Off(OUT_BC); }else if(SENSOR_3<WHITE){ OnFwd(OUT_B,70); Off(OUT_C); }else if(SENSOR_3>=WHITE){ RotateMotorEx(OUT_BC,SPEED,2.0,-50,true,true); } } } } これらはすべて缶を掴む・離すことに使用する関数です。fetch_ballやopen、closeでアームを動かします。followC、followE、followFでは缶が置いてあるそれぞれの交差点でそれぞれに数値を最適化させたものをまとめています。これらは缶と超音波センサーの距離が15cm以下になったときはclose関数を使用して掴む動作を行い、缶との距離が15cm以上であればライントレースを行うようになっています。 void catch() { SetSensorLowspeed(S2); long tO; tO = CurrentTick(); while(CurrentTick()-tO < 10000){ int O = SensorUS(S2); if(O <= 11){ break; }else if(11 < O){ Wait(STEP); } Wait(STEP); Wait(2500); } } この関数では超音波センサーによってボールを感知するプログラムです。缶を持っていき、ボールを乗せてもらったときに超音波センサーが計測する値は缶だけのときよりも小さくなります。そのためこの関数では11cm以下になったときにボールがあると捉えるようにしました。完成版には使われていなませんが、制作段階で使ったり使わなかったりしていたため、プログラム内には残してあります。 ***task main()の中身 [#o1a6a17d] task main() { RotateMotorEx(OUT_BC,SPEED_H,180,0,true,true); follow_line_for_intersection(10000); follow_line(200); follow_line_for_intersection(1000); follow_line(200); follow_line_for_intersection(1500); 最初のCにたどり着くためのプログラムです。最初の１０秒間はきついカーブの交差点の誤認識を防止するために交差点を認識しないintersectionを入れています。３行目のfollow_line(200)でHの交差点を認識し、その後のintersectionで交差点を曲がり切り、次のfollow_lineで次のG交差点を探しながらライントレースします。今後も交差点の認識と通過はこの仕組です。それぞれのintersectionで数値が異なるのはその状況に合わせて細かく調整をしているからです。 followC(); // C開始 follow_line_for_intersection(750); follow_line(150); RotateMotor(OUT_C,SPEED,150); turnR(40); RotateMotorEx(OUT_BC,SPEED_L,60,0,false,true); follow_line_for_intersection(1000); follow_line(200); follow_line_for_intersection(3000); Wait(10000); followCで缶を探しながらライントレースを行います。缶を見つけると缶を掴んだあとにUターンをします。Uターン後は位置が安定しないため、intersectionで整えます。その後にG交差点を感知してRotateMotor以下３行でG交差点を曲がるようにします。intersectionで曲がらないのは、確実に次のライントレースを始められるように位置を調整するためです。そして、最後のH交差点を曲がり、３秒間進んだところで停止し，ボールが運ばれてくるのを待ちます。待っている10秒間でボールを缶に乗せてもらいます。 RotateMotor(OUT_C,-SPEED,330); RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true); turnL(40); follow_lineR(135); RotateMotor(OUT_C,SPEED,70); follow_line_for_intersection(1000); follow_line(180); follow_line_for_intersection(1000); follow_line(180); back(120); open(); // C終了 最初の３行はボールを受け取ったあとの動きです。以下の図のように動くことでFにある缶に当たることなく逆向きになります。逆に向いた後は黒い線の右トレースすることで，H交差点を認識できるように知ました． #ref(12.jpg) H交差点を感知したあとは左のタイヤだけ回転させることで曲がった先で黒線の左にセンサーが来るようになっています。Cまでは今まで通り交差点を通過していきます。交差点Cを感知したところで後退して缶を離します。この動きをすることで缶を円の中に収めることができます。 back(160); turnR(130); RotateMotorEx(OUT_BC,SPEED_L,40,0,false,true); follow_line_for_intersection(2000); 一度下がり、アームが置いた缶に当たらないようにして右回転でUターンをするようにしました。その後、前進することで確実にCから見て交差点Gの先にセンサーが来るようになります。ライントレースを行い、Fに近づいていきます。 followF(); // F開始 follow_line_for_intersection(1000); follow_line(200); follow_line_for_intersection(3000); Wait(9000); Fで缶を掴みIまで持っていきます。これは1つ目の缶を持っていくときとほぼ変わりません。ボールを受け取るまでの時間調整で9秒間待ちます。 RotateMotor(OUT_C,-SPEED,330); RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true); turnL(40); follow_lineR(140); follow_line_for_intersectionR(1000); follow_lineR(140); RotateMotor(OUT_C,SPEED_H,90); follow_line(160); back(120); open(); //F 終了 1つ目の缶でボールを受け取ったあとと同じように方向転換をして右側ライントレースをGまで行います。そこから先程と同じように左のタイヤだけ回転させて左トレースに戻り、Fに缶を置きます。 back(150); turnL(77); RotateMotorEx(OUT_BC,SPEED_L,50,0,false,true); follow_line_for_intersectionR(1000); follow_lineR(140); follow_line_for_intersectionR(2000); Cで缶を置いたときと同じようにアームが缶に当たらないように下がったあとにG-Dの黒線の右側にセンサーが来るように方向転換をします。そしてEまで右側トレースで進みます。 followE(); // E開始 follow_line_for_intersection(1500); follow_line(160); RotateMotor(OUT_C,SPEED,75); follow_line_for_intersection(500); follow_line(160); follow_line_for_intersection(3000); Wait(15000); Eで缶を掴んだあとはそのままintersectionでDを通過し、Gを感知しても直進することで通過、Iまで行きます。15秒間待ち、最後の缶を戻しに行きます。 RotateMotor(OUT_C,-SPEED,330); RotateMotorEx(OUT_BC,SPEED_L,70,0,false,true); turnL(40); follow_lineR(150); follow_line_for_intersectionR(1500); follow_lineR(160); RotateMotorEx(OUT_BC,SPEED_L,65,0,false,true); ここまででボールを受け取ったあとの方向転換、からGまでのトレースです。 RotateMotor(OUT_B,SPEED,30); follow_line_for_intersectionR(1000); follow_lineR(150); follow_line_for_intersectionR(1750); follow_lineR(140); back(120); turnL(15); open(); // E終了 Gの交差点感知から直進後は進路が右に向きすぎているため、右タイヤのみ回転させて左に向きを調整する。これが一行目のRotateMotorです。そこからは今まで通りEまで運びます。最後にEだけは確実に円内に缶を収めるためにE交差点を感知して後退したあとに左に向きを調節しています。 back(300); } 最後に缶から離れて終了です。 *まとめ [#e0ed3342] 今回は，ボールを運ぶ側と，缶を運ぶ側の二手に分かれて，機械を制作しました．この時は交差点を曲がる，この時は交差点を渡る，というように切り替えるのが難しいなと思いました．また，ロボットを組み立てる上で，いろいろ，改良しなければいけなかった部分もあってすごく大変でした．
