[[2016b/Member]] #contents * 概要 [#z3b19960] 今回の[[課題:http://yakushi.shinshu-u.ac.jp/robotics/?2016b%2FMission3]]は球技ロボットである. ロボットは - ''ピッチャー側''(A地点からM地点まで移動, ボールを投げる; 以降ピッチャーと表記) - ''バッター側''(C地点からB地点まで移動, ボールを打つ; 以降バッターと表記) に分けることができる. 今回私はバッターのロボット, コードを考えた. 動作時の流れは, + バッターがC地点からB地点まで移動, ピッチャーにシグナル(1)を送信 + ピッチャーはM地点まで移動, バッターにシグナル(完了)を送信 + バッターは超音波センサーでピッチャーの方向を特定し, 向きを合わせ, ピッチャーにシグナル(2)を送信 + ピッチャーは同様にバッターの位置を特定し, 向きを合わせ, バッターにシグナル(完了)を送信 + バッターはピッチャーにシグナル(3)を送信, ピッチャーがボールを発射する + ボール発射の数秒後にバッターはボールを捕獲, 方向を変え, ボールを発射 である. * ソフトウェアについて [#edaf597c] 以下がコードである. #define moter_conect_move OUT_BC //駆動輪モーターの接続コネクタ #define moter_conect_option OUT_A //ハンドルのモーターの接続コネクタ #define moter_conect_move_L OUT_B //左駆動輪モーターの接続コネクタ #define moter_conect_move_R OUT_C //右駆動輪モーターの接続コネクタ #define SPEED 45 //通常モーター速度 #define SPEED_FAST 60 //通常モーター速度(高速) #define SPEED_SLOW 40 //通常モーター速度(低速) #define SPEED_line_trace 30 //ライントレース時の速度(機体によって調節) #define SPEED_FAST_line_trace 40 #define SPEED_SLOW_line_trace 30 #define THRESHOLD 44 //白と黒の中間の反射率 const float diameter = 5.45; //タイヤの直径(cm) const float track = 15; //タイヤのトレッド幅(cm) const float pi=3.1415; //円周率 /****************** ここから, 長谷部くんのコードを引用した. *******************/ //すべての出力モーターの停止 void stop_move() { Off(OUT_ABC); Wait(1); } //左に ang (°) 回転 void turnAng_L(float ang) { stop_move(); //左回転に必要な角度の計算 float angle = track / diameter * ang; //計算した角度動かす RotateMotorEx(moter_conect_move, SPEED_SLOW, angle, 100, true, true); } //ライントレース void line_trace(int sensor_light_lv) { if (sensor_light_lv < THRESHOLD - 8) { OnFwd(moter_conect_move_R, SPEED_FAST_line_trace); OnFwd(moter_conect_move_L, -SPEED_FAST_line_trace); } else if (sensor_light_lv < THRESHOLD - 5) { OnFwd(moter_conect_move_L, SPEED_SLOW_line_trace); } else if (sensor_light_lv < THRESHOLD + 5) { OnFwdSync(moter_conect_move, SPEED_line_trace,0); } else if (sensor_light_lv < THRESHOLD + 8) { OnFwd(moter_conect_move_R, SPEED_SLOW_line_trace); } else { OnFwd(moter_conect_move_L, SPEED_FAST_line_trace); OnFwd(moter_conect_move_R, -SPEED_FAST_line_trace); } } //Sensorの値がThreshold(任意の反射率)以上のとき, 引数に現在の時刻を代入し返す. //それ以外は変更せず引数を返す long intersection_judgment(long t0, int threshold) { if (SENSOR_4 > threshold) { t0 = CurrentTick(); } return t0; } //seconds(秒)間、ライントレースをする void line_tracing_in_seconds(float seconds) { stop_move(); long t0 =CurrentTick(); float temp = 1000 * seconds; while(CurrentTick() - t0 < temp) { line_trace(SENSOR_4); } } //交差点を判断(Thresholdの値で判断)するまで、ライントレースをする void line_trace_and_intersection_judgment(int threshold, int judgment_time) { stop_move(); long t0 = CurrentTick(); while(CurrentTick()-t0 < judgment_time) { line_trace(SENSOR_4); t0=intersection_judgment(t0,threshold); } stop_move(); Wait(1000); PlaySound(SOUND_UP); } /****************** ここまで, 長谷部くんのコードを引用した. *******************/ //ピッチャーが移動中, 位置合わせ中の時に使用する. ピッチャーが"1"というメッセー //を返すまで次の行動に移らない. sub waitpartner() { int msg = 0; while(msg == 0) { ReceiveRemoteNumber(MAILBOX1, true, msg); } } //バッター側のNXTが位置合わせを行わないほうがうまくいくようなので, あえて位置合 //わせを行わない. (コメントアウト) int searchDirection(long ang) { long tacho_min; int d_min = 300; /*long angle = (track / diameter) * ang; turnAng_L(ang / 2); ResetTachoCount(OUT_BC); OnFwdSync(OUT_BC, SPEED, -100); while(MotorTachoCount(OUT_B) <= angle) { if (SensorUS(S1) < d_min) { d_min = SensorUS(S1); tacho_min = MotorTachoCount(OUT_B); } } OnFwdSyncEx(OUT_BC, SPEED, 100, RESET_NONE); until(MotorTachoCount(OUT_B) <= tacho_min || SensorUS(S1) <= d_min); Wait(150); */ Off(OUT_BC); Wait(500); return d_min; } sub ajustRobot() { int tmp = searchDirection(40); Wait(500); } //そのままではボールを打つハンドルが下がってきてしまうので, 上方向にある程度回転 //させて, 固定する sub handlehold() { OnFwd(OUT_A, 10); } sub ball_catch() { //ボール発車前に毎回位置合わせをする場合, コメントを外す. //ajustRobot(); SendRemoteNumber(1, MAILBOX2, 2); waitpartner(); handlehold(); Wait(1000); //ボール発射の司令 SendRemoteNumber(1, MAILBOX2, 3); handlehold(); Wait(1550); RotateMotor(OUT_A, 50, -50); OnRev(OUT_A, 50); Wait(500); } sub ball_hit(int drctn) { if (drctn == 40) //4点(右) { RotateMotor(OUT_B, SPEED, 210); Wait(500); RotateMotor(OUT_A, 100, 100); Wait(1000); handlehold(); RotateMotor(OUT_B, SPEED, -190); Wait(500); } else if (drctn == 41) //4点(左) { RotateMotor(OUT_B, SPEED, -240); Wait(500); RotateMotor(OUT_A, 100, 100); Wait(1000); handlehold(); RotateMotor(OUT_B, SPEED, 250); Wait(500); } else if (drctn == 60) //6点(右) { RotateMotor(OUT_B, SPEED, 100); Wait(500); RotateMotor(OUT_A, 100, 100); Wait(1000); handlehold(); RotateMotor(OUT_B, SPEED, -100); Wait(500); } else if (drctn == 61) //6点(左) { RotateMotor(OUT_B, SPEED, -190); Wait(500); RotateMotor(OUT_A, 100, 100); Wait(1000); handlehold(); RotateMotor(OUT_B, SPEED, 190); Wait(500); } else if (drctn == 10) //10点 { RotateMotor(OUT_B, SPEED, -100); Wait(500); RotateMotor(OUT_A, 100, 100); Wait(1000); handlehold(); RotateMotor(OUT_B, SPEED, 100); Wait(500); } else if (drctn == 00) //4点(右) { RotateMotor(OUT_B, SPEED, 30); Wait(500); } } //スタート地点から規定の位置まで進む sub start1() { RotateMotor(OUT_BC, SPEED, 40); RotateMotor(OUT_C , SPEED, 355); RotateMotor(OUT_BC, SPEED, 900); //3秒間無条件でライントレース(丁字路判別を行わない) line_tracing_in_seconds(3); //ライントレース, 丁字路判別 line_trace_and_intersection_judgment(33,90); RotateMotor(OUT_BC, SPEED, 300); RotateMotor(OUT_B , SPEED, -100); RotateMotor(OUT_BC, SPEED, 100); PlaySound(SOUND_LOW_BEEP); } task main() { SetSensorLowspeed(S1); SetSensorLight(S4); handlehold(); start1(); SendRemoteNumber(1, MAILBOX2, 1); waitpartner(); ajustRobot(); SendRemoteNumber(1, MAILBOX2, 2); Wait(2000); waitpartner(); handlehold(); //4点(右)を狙う ball_catch(); ball_hit(40); //4点(左)を狙う ball_catch(); ball_hit(41); //6点(左)を狙う ball_catch(); ball_hit(60); //6点(左)を狙う ball_catch(); ball_hit(61); //6点(左)を狙う ball_catch(); ball_hit(61); } 以下に説明を述べる. ** ハンドルを固定 [#c996e9e3] ボールを捕獲・発射するためのハンドルが, 動作中に自重によって下がってきてしまう. これを防ぐために sub handlehold() { OnFwd(OUT_A, 10); } によって, ハンドルを弱いトルクで上げ続けておく. ** 一定距離前進, ライントレース, 交差点判断 [#d635d5cf] スタート直後は RotateMotor によってフィールド上のR地点付近まで移動する. その後はライントレースを行ってB地点まで移動する. ライントレースは //3秒間無条件でライントレース(丁字路判別を行わない) line_tracing_in_seconds(3); //ライントレース, 丁字路判別 line_trace_and_intersection_judgment(33,90); となっているが, これはライントレース開始直後は無条件にライントレースを行い, ライントレースを開始して3秒経過してから, 交差点(B地点手前)の判断を行うものである. 交差点は黒線の濃さと時間によって行っている(上の場合は明るさ33以下が0.09秒間続いたとき, 交差点と認識する). 交差点を確認後はおよその向きを合わせておく. ** ピッチャーに移動の司令, 移動を待つ [#x52f5b47] SendRemoteNumber(1, MAILBOX2, 1); waitpartner(); で, ''ピッチャー(COMM=1)のMAILBOX2に, "1"というメッセージが伝わる''が, ピッチャーがこれを受信するとA地点からM地点まで移動を開始する. ピッチャーの移動が完了すると''バッター(COMM=0)のMAILBOX1に, "1"というメッセージを伝える''. バッターはこのメッセージを受信するまで何もしない. ** 双方の方向を合わせる [#cff8be55] ajustRobot(); SendRemoteNumber(1, MAILBOX2, 2); Wait(2000); 最初にバッターが, 超音波センサーを使用してピッチャーの位置を特定するが, これが誤差が無視できないほど大きく, 向きを合わせないほうが却ってよいように思われたので, これを無効にした(ajustRobot自体は残してあるが, これらの中身をコメントアウトしてあり, 実質何も行わない). その後, ''ピッチャー(COMM=1)のMAILBOX2に, "2"というメッセージが伝わる''が, ピッチャーはこれを受信すると超音波センサーで向きを合わせる. 向きを合わせ次第, 先程と同様に''バッター(COMM=0)のMAILBOX1に, "1"というメッセージを伝える''.