[[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続いた場合、交差点と判別します。 **プログラムの流れ [#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]