[[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"というメッセージを伝える''.



トップ   新規 一覧 検索 最終更新   ヘルプ   最終更新のRSS