[[2015b/Member]] 目次 #contents *ロボコンの説明 [#p90c333d] #ref(2015b/Member/rmsun/Mission3/2015b-mission31.png, 480*332) ロボットAはSTART A地点から出発し、P地点におかれた赤いボールを拾って350mlのアルミ缶に当てる。その後、ロボットBがボールを回収する。引き続き青いボールについても同様の動作を繰り返す。ロボットBはボールを回収した後、最終的にGOAL Bまでボールを運ぶ。 詳しい説明はhttp://yakushi.shinshu-u.ac.jp/robotics/?2015b%2FMission3を参照 *ロボットの説明 [#p06dabf5] **「ボールを缶に当てるロボット」 [#k9cf5b32] **「ボールを回収するロボット」 [#l707841a] *「ボールを缶に当てるロボット」のプログラム [#k14bab2c] *「ボールを回収するロボット」のプログラム [#z16e8d55] #define SPEED 50 //Dist, #define SPEED_SLOW 30 //関数hand,turnAngなどで使用 #define THRESHOLD 45 //閾値 #define STEP 1 //Waitの引数として使う #define STOP 3 //「ボールがセンサの前を横切った」かどうかを調べる値 const float diameter=5.45; //タイヤの直径(cm) const float pi=3.1415; //円周率 const float track = 10.35; //タイヤのトレッド幅 sub hand(int ang, int plmi) { //ボールをつかむ動作 RotateMotor(OUT_A, SPEED_SLOW * plmi, ang); } sub Dist(float d, int fwd_or_rev){ //dcm前転(後転) /*fwd_or_rev = 1 → 前転 fwd_or_rev = -1 → 後転 */ long angle; angle = d / (diameter * pi) * 360.0; RotateMotorEx(OUT_BC, SPEED * fwd_or_rev, angle, 0, true, true); Wait(100); } sub turnAng(int ang, int R_or_L) { /* R_or_L = 1 → 時計回り R_or_L = -1 → 半時計回り */ long angle; angle = track / diameter * ang; RotateMotorEx(OUT_BC, SPEED_SLOW , angle, 100* R_or_L, true, true); Wait(100); } int searchDirection(int ang) //ボールを探す関数 { SetSensorLowspeed(S1); long angle, tacho_min = 0, tacho_corr; int d_min; d_min= 300; //仮の最小値 angle = (track / diameter) * ang; turnAng(10, 1); //10度右旋回(缶の誤認を防ぐため、10度しか旋回しない) ResetTachoCount(OUT_B); OnFwdSync(OUT_BC, SPEED, -100); while(MotorTachoCount(OUT_B) <= angle) { //angleだけ左旋回 if (SensorUS(S1) < d_min) { d_min = SensorUS(S1); tacho_min = MotorTachoCount(OUT_B); } } if (d_min < 30) { //「超音波センサの示した最小値」が30以下のとき OnFwdSyncEx(OUT_BC,SPEED, 100, RESET_NONE); //最小値だった方向を向く until(MotorTachoCount(OUT_B) <= tacho_min || SensorUS(S1) <= d_min); } else { //30以上のとき turnAng(ang - 10, 1); //ang-10だけ右旋回(関数開始前と同じ向きに戻る) } Off(OUT_BC); Wait(500); //0.5秒間停止 return d_min; //「超音波センサの示した最小値」を返す } sub turn_left(int speedA, int speedB) { //左に曲がる OnFwd(OUT_B,speedA); //引数speedAのパワーで、モータBを前転 OnRev(OUT_C,speedB); //引数speedBのパワーで、モータCを後転 } sub turn_right(int speedA, int speedB) { //右に曲がる OnFwd(OUT_C,speedB); //引数speedBのパワーで、モータCを前転 OnRev(OUT_B,speedA); //引数speedAのパワーで、モータBを後転 } sub go_forward(int speed3) { //まっすぐ進む OnFwd(OUT_BC,speed3); //speed3のパワーで、モータBCを前転 } sub trace(int time, long target, int speed3) { SetSensorLight(S2); int speed1 = 25; //モータのパワー用 int speed2 = 20; //モータのパワー用(speed1より少し弱い) long tacho = 0; //「その場で左旋回する」動作をし続けたときの、回転角の合計 long t; //タイマーを使うときに使用 t = CurrentTick(); //ここで現在時刻をtに代入(CurrentTick() - t と使うことで、経過時間がわかる) while (CurrentTick() - t < time && tacho < target) { if (SENSOR_2 < THRESHOLD -15){ //光センサの数値が、THRESHOLD-15より低いとき turn_left(speed1, speed1); //左右のモータをspeed1で左旋回(その場で左回転) } else if (SENSOR_2 < THRESHOLD -7) { //光センサの数値が、THRESHOLD-7より低いとき tacho = 0; //tacho(turn_left1し続けた時の、タイヤの回転角の合計)を0にする。 turn_left(speed1, speed2); //右モータをspeed1、左モータをspeed2(speed1より少し弱い)にして、左旋回 } else if (SENSOR_2 < THRESHOLD +7){ //光センサの数値が、THRESHOLD+7より低いとき tacho = 0; //tachoを0にする go_forward(speed3); //左右のモータをspeed3にして、直進 } else if (SENSOR_2 < THRESHOLD +15){ //光センサの数値が、THRESHOLD+15より低いとき tacho = 0; /*tachoを0にする*/ turn_right(speed1, speed2); /*右モータをspeed2、左モータをspeed1にして、右旋回*/ } else { /*光センサの数値が、THRESHOLD+15より高いとき*/ tacho = 0; /*tachoを0にする*/ turn_right(speed1, speed1); /*左右のモータをspeed1で右旋回(その場で右回転)*/ } Wait(STEP); //STEP(0.001)秒だけ動く if (SENSOR_2 >= THRESHOLD-15) { /*光センサの数値が、THRESHOLD-15より高いとき (=その場で左回転していないとき) */ ResetTachoCount(OUT_B); /*モータの回転数をリセット(その場で左回転したとき以外はリセット)*/ } tacho += MotorTachoCount(OUT_B); /*tachoに、モータの回転角を代入(「その場で左回転した時」の回転角しか代入されない)*/ } Off(OUT_BC); Wait(STEP); PlaySound(SOUND_CLICK); } task main() { long t; Wait(30000); //30秒待機 Dist(48, 1); //48cm前進 turnAng(91, -1); //90度左旋回 Dist(5, 1); //5cm前進 SetSensorLowspeed(S1); //超音波センサ起動 int ball = 0; //変数「ball」...超音波センサの値 < 30 を示した回数を示す while (1) { //ボールが通過する様子を調べる if (SensorUS(S1) < 30) { /*センサ値 < 30(ボールがセンサの前を通り過ぎたとき、センサ値が20程度を示すため)*/ ball++; } if (ball == STOP) { //通過した回数がSTOP(=3)になったら break; //ループを抜け出す } Wait(STEP); //STEP(0.001)秒間 } Wait(800); //0.8秒間待機 PlaySound(SOUND_CLICK); //次の動作に入ることを知らせる t = CurrentTick(); while (CurrentTick() - t < 2000) { //2秒の間繰り返す if (SensorUS(S1) < 30) { //センサ値 < 30のとき ball++; //ball + 1 ..つまりball = STOP + 1 break; //ループを抜け出す } Wait(STEP); } if (ball == STOP + 1) { //超音波センサ < 30を示すまで前進 OnFwd(OUT_BC, SPEED_SLOW); //SPEED_SLOW(=30)のパワーで前進 while (ball < STOP + 2) { if (SensorUS(S1) < 30) { ball++; } } Off(OUT_BC); //1秒停止 Wait(1); Dist(35, 1); //35cm前進 hand(70, -1); //ボールをつかむ turnAng(30, -1); //30度右旋回(ライントレースの黒線に入るため) Dist(8, 1); //8cm前進(ライントレースの黒線に入るため) trace(20000, 15, 40); /*「20秒以内 && TachoCount < 15」の間ライントレース*/ trace(7000, 1000, 50); /*「7秒以内 && TachoCount < 1000」の間ライントレース*/ trace(15000, 10, 40); /*「15秒以内 && TachoCount < 10」の間ライントレース*/ turnAng(75, 1); //75度右旋回 Dist(15,1); //15cm前進 turnAng(20, 1); //20度右旋回 SetSensorLight(S2); //光センサー起動 OnFwd(OUT_BC, SPEED); //黒線までSPEED(=50)のパワーで前進 while (SENSOR_2 > THRESHOLD - 15) {} Off(OUT_BC); //停止 hand(70, 1); //ボールをつかむ } else { turnAng(90, -1); //90度左旋回 Dist(40, 1); //40cm進む turnAng(90, 1); //90度右旋回 Off(OUT_BC); Wait(STEP); //0.001秒間停止 while (1) { int d = searchDirection(60); //60度の範囲でボールを探す if (d < 30) { /*searchDirection(60)の返り値 < 30のとき(探した範囲にボールがあったとき)*/ Dist(20, 1); //20cm進む hand(70, -1); //ボールをつかむ break; //ループから抜け出す } else { OnFwd(OUT_BC, SPEED); //0.5秒進む Wait(500); searchDirection(0); /*searchDirectionを2連続で使うとバグるためseachDirection(0)をはさむ*/ } } trace(10000, 10000, 45); //どこかのラインにのる,ロボットの向きをゴール方向に向ける Dist(30, 1); //30cm進む、 SetSensorLight(S2); //光センサー起動 OnFwd(OUT_BC, SPEED); //SPEED(=50)のパワーで、黒線に到達するまで直進 while (SENSOR_2 > THRESHOLD - 15) {} trace(3000, 10000, 40); //「3秒以内 && TachoCount < 10000」の間ライントレース hand(70, 1); //ボールを離す } }