[[2016b/Member]]&br;
目次&br;
#contents
*課題について [#o71d3bfc]
&ref(2016b/Mission2/2016b-mission2.png,50%,マップ);
&br;

*機体について [#ia2fc844]

**機体の全体像 [#y6e3d023]
&ref(2016b/Member/pen/Mission2/全体像.jpeg,50%,全体像);
&br;
***ボールの検出と取得の機構 [#le9444f0]
&ref(2016b/Member/pen/Mission2/アーム.jpeg,50%);
&br;

***ライントレース部分(下半身) [#na6d454f]
&ref(2016b/Member/pen/Mission2/下半身1.jpeg,50%);
&ref(2016b/Member/pen/Mission2/下半身2.jpeg,50%);
&ref(2016b/Member/pen/Mission2/前輪.jpeg,50%);
&br;

*プログラムについて [#m5dc9ef0]
**プログラムの解説 [#r8449133]
***ライントレースの処理 [#sdfd9232]
[[前回と同じ:http://yakushi.shinshu-u.ac.jp/robotics/?2016b%2FMember%2Fpen%2FMission2#g8518f00]]

***交差点の処理 [#id38ca84]
[[前回の判定処理はそのまま使い:http://yakushi.shinshu-u.ac.jp/robotics/?2016b%2FMember%2Fpen%2FMission2#xe8b6c3f]]判定に使うパラメーターを調節し、交差点突入後100msで判定が可能になりました。
 line_trace_and_intersection_judgment(31,100);
この関数では、センサーの値が31以下であり、それが100ms続いた場合、交差点と判別します。

***交差点の検出 [#bcc9a264]

***ボールの検出 [#kd6477a1]

***交差点の処理 [#id38ca84]

***ボール取得のメロディ [#z596bbf6]
**プログラムの流れ [#jd9c49fa]

**本文 [#y7358e18]
 #define moter_conect_move OUT_AB             //駆動輪モーターの接続コネクタ(ABC順)
 #define moter_conect_option OUT_C            //駆動輪以外のモーターの接続コネクタ(ABC順)
 #define moter_conect_move_L OUT_A            //左駆動輪モーターの接続コネクタ
 #define moter_conect_move_R OUT_B            //右駆動輪モーターの接続コネクタ
 #define SPEED 55                             //通常モーター速度
 #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 40                         //白と黒の中間の反射率

 
 
 const float diameter = 5.45;                 //タイヤの直径(cm)
 const float track = 11.38;                   //タイヤのトレッド幅(cm)
 const float pi=3.1415;                       //円周率 
 
 

 //関数エリア
 void stop_move()                             //すべての出力モーターの停止(すべてのサブルーチン内に同じこと書くと、見づらいため)
 {
    Off(OUT_ABC);
    Wait(1);
 }
  
 void fwdDist(float d)                        //距離 d cm 前進
 {
    stop_move();                              //モーターのリセット
    float angle = d/(pi*diameter)*360.0;			//前進に必要な角度を計算する
    RotateMotorEx(moter_conect_move, SPEED_SLOW, angle, 0, true, true);    //計算した角度動かす
 }
 
 void turnAng_L(float ang)                    //左に ang (°) 回転 
 {
    stop_move();                              //モーターのリセット
    float angle = track/diameter * ang;       //左回転に必要な角度の計算
    RotateMotorEx(moter_conect_move, SPEED_SLOW, angle, 100, true, true);  //計算した角度動かす
 }
 
 void turnAng_R(float ang)                     //右に ang (°) 回転 
 {
    stop_move();                              //モーターのリセット
    float angle = track/diameter * ang;       //右回転に必要な角度の計算
    RotateMotorEx(moter_conect_move, SPEED_SLOW, angle, -100, true, true); //計算した角度動かす
 }
 
 int searchDirection(long ang)                //一番近い距離を探して、距離(cm)を返す(アルミ缶の検出プログラムと殆ど同じ)
 {
    stop_move();                              //モーターのリセット
    long tacho_min ;
    int d_min = 300 ;
    
    long angle = (track/diameter)*ang;
    turnAng_L(ang/2);
    ResetTachoCount(moter_conect_move);
    
    OnFwdSync(moter_conect_move,SPEED_SLOW,-100);
    while(MotorTachoCount(moter_conect_move_L)<=angle)
    {
        int US_average = SensorUS(S4)       ; //3回ほど超音波センサーの値を取得し、平均値を出すことで、突発的なセンサーの値のズレの影響を少なくする
        US_average = US_average+SensorUS(S4); //
        US_average = US_average+SensorUS(S4); //
        US_average = US_average / 3 ;         //
        if (US_average<d_min)
        {
            d_min = SensorUS(S4);
            tacho_min = MotorTachoCount(moter_conect_move_L);
        }
    }
    OnFwdSyncEx(moter_conect_move,SPEED_SLOW,100,RESET_NONE);
    until(MotorTachoCount(moter_conect_move_L)<=tacho_min || SensorUS(S4)<=d_min);
    
    Wait(360);
    Off(moter_conect_move);Wait(500);
    return d_min;
     
 }
    
 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);
    }
 }
 
 long intersection_judgment(long t0,int threshold)//Sensorの値がThreshold(任意の反射率)以上のとき、引数に現在の時刻を代入し返す。それ以外は変更せず引数を返す。
 {
    if (SENSOR_1 > threshold)                     //現在のセンサーの値が、Threshold以上のとき
    {
        t0 = CurrentTick();                       //現在の値をt0に代入
    }
    return t0 ;                                   //t0の値を返す
 }
 

 

 //関数(task_mainで呼び出す)エリア
 void set_sensor_all()
 {
    SetSensorLight(S1);                           // ライトセンサーの定義
    SetSensorLowspeed(S4);                        //超音波センサーの定義
 }
 
 void firing_ball()                               //ボールの発射関数
 {
    stop_move();                                  //モーターのリセット(関数が連続する際、Offを挟まないと5行目以降でバグが起きる)
    RotateMotor(moter_conect_option,85,537);      //optionモーター(ボール発射装置)のモーターの回転
 }
 
 void line_tracing_in_seconds(float seconds)      //seconds(秒)間、ライントレースをする
 {
    stop_move();
    long t0 =CurrentTick();
    float temp=1000*seconds;                      //whileループが成立する間、1000*secondsを毎回計算するのが、演算的に無駄であるため、tempに入れる
    while(CurrentTick()-t0 < temp)                //temp秒間ループ
    {
        line_trace(SENSOR_1);                     //ライントレースをする
    }
 }
 
 void line_trace_and_intersection_judgment(int threshold,int judgment_time)     //交差点を判断(Thresholdの値で判断)するまで、ライントレースをする
 {
    stop_move();                                  //モーターリセット
    long t0 = CurrentTick();                      //現在の値をt0に取得
    while(CurrentTick()-t0 < judgment_time)       //judgment_timeより短い時間の黒の検出は無視
    {
        line_trace(SENSOR_1);                     //ライントレースをする
        t0=intersection_judgment(t0,threshold);   //現在がthresholdの値より大きいとき、t0に現在の値を代入
    }
    stop_move();                                  //モーターの停止
    Wait(1000);                                   //交差点で一時停止
    PlaySound(SOUND_UP);                          //交差点検出で音を鳴らす
 }
 
 void move_A_to_inside_of_M()                    //スタート地点から発射地点までの移動
 {
    fwdDist(5.0);                                 //5cm前進
    line_tracing_in_seconds(1.2);                 //1.2秒間ライントレース
    turnAng_R(70.0);                              //右に70°転
    fwdDist(30.0);                                //30cm前進
    line_tracing_in_seconds(6.0);                 //6秒間ライントレース
    line_trace_and_intersection_judgment(31,100); //交差点を検出するまでライントレース
    line_tracing_in_seconds(7.2);                 //7.2秒間ライントレース
    turnAng_R(90.0);                              //右に90°回転
    fwdDist(13.0);                                //13cm前進
 }
 
  
 void mukiawase()                                 //ロボットの向きを合わせる
 {
    turnAng_L(10.0);                              //弁当のゴールを検出しないようにする為の振り幅の調節
    int a =searchDirection(60);                   //一番近い物体に向きを合わせる
    turnAng_R(3.0);                               //バッターの打ち返す機構に向きを合わせる微調整
 }
 
 void send_action_finish()                        //バッターに動作の完了を送る
 {
    SendRemoteNumber(0,MAILBOX1,1);
 }
 
 int receive_action_signal()                      //バッターから命令を受け取る
 {
    int temp = 0;                                 //メッセージの受取変数
    while(temp==0)                                //メッセージが初期値から変わるまでループ
    {
        ReceiveRemoteNumber(MAILBOX2,true,temp);  //メッセージの取得
    }
    PlaySound(SOUND_UP);                          //メッセージを受け取ったら音を鳴らす
    return temp;                                  //受け取った値を返す
 }
 

 //taskエリア
 task main()
 {
    set_sensor_all();                             //センサーの初期化
    int  action_number=0;                         //命令コードの記憶変数
    while(true)
    {
        action_number  =  receive_action_signal();//命令の受信をし、変数に代入(受信するまでこの行で停止)
        if (action_number == 1)                   //命令コードが1のとき
        {
            move_A_to_inside_of_M();              //ピッチャーの位置まで移動
            action_number = 0;                    //命令コードのリセット
        }
        else if (action_number == 2)              //命令コードが2のとき
        {
             mukiawase();                         //ロボットの向きを合わせる
             action_number = 0;                   //命令コードのリセット
        }
        else if (action_number == 3)              //命令コードが3のとき
        {
             firing_ball();                       //ボールの発射
             action_number = 0;                   //命令コードのリセット
        }
        send_action_finish();                     //動作の完了を通知
        PlaySound(SOUND_UP);                      //動作完了の音
    }
 }


*感想 [#ued603b3]


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