- 追加された行はこの色です。
- 削除された行はこの色です。
[[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]