- 追加された行はこの色です。
- 削除された行はこの色です。
[[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"というメッセージを伝える''.