2016a/Member
はじめに†
ロボット本体の説明†
プログラムの説明†
#define THRES1 39 //閾値1(黒、黒線中央)
#define THRES2 43 //閾値2(黒に近い黒、黒線中央から少し外側)
#define THRES3 53 //閾値3(薄黒、黒線中央と境界の中間)
#define THRES4 59 //閾値4(白に近い黒、黒線境界から少し内側)
#define SPEED_H 40 //速いスピード
#define SPEED_L 17 //遅いスピード
#define TIME_CROSS 230 //交差点を渡る時の時間
#define STEP 1 //1回の判定で進む時間
#define nMAX 220 //交差点での黒の判定基準
#define move_RL(speedR,speedL) OnFwd(OUT_A,speedR);OnFwd(OUT_B,speedL); //基本動作
#define short_break Off(OUT_BC); Wait(500); //小休憩
#define SPEED 30
#define ARM 50
#define go_forward OnFwd(OUT_AB,SPEED);
#define arm(t) OnFwd(OUT_AB,SPEED);Wait(t);\
Off(OUT_AB);OnRev(OUT_C,ARM);Wait(200);Off(OUT_C)
#define CONN 0
#define Boot 11
sub turn_right_A() //右折
{
move_RL(0, 30);
}
sub turn_right_B() //右旋回
{
move_RL(-SPEED_L,25);
}
sub back() //後退
{
move_RL(-SPEED,-30);
Wait(1700);
}
sub AC()
{
SetSensorLight(S1);
int n_black=0; //黒をカウント
int m_kousa=0; //交差点をカウント
while(m_kousa < 3) { //交差点が5回目未満の間
while (n_black < nMAX) { //n_black(黒)がnMAX未満の間、ライントレースをする
if (SENSOR_1 < THRES1) { //もし黒のとき左旋回、黒の回数をカウント
move_RL(SPEED_L,-25);
n_black++;
} else { //そうでないとき
if (SENSOR_1 < THRES2) { //もし黒に近い黒⇔黒の間は左折
move_RL(SPEED_L,0);
} else if (SENSOR_1 < THRES3) { //薄黒⇔黒に近い黒の間は直進
move_RL(SPEED_H,50);
} else if (SENSOR_1 < THRES4) { //白に近い黒⇔薄黒の間は右折
move_RL(0, 30);
} else { //白のとき右旋回
move_RL(-SPEED_L,25);
}
n_black=0; //n_black(黒)をリセットして
}
Wait(STEP); //上記の動作を、STEP(1回)ずつ行う。
}
if (m_kousa < 1) { //もし交差点が3回目まで交差点を直進
short_break;
PlaySound(SOUND_DOUBLE_BEEP); //直進の合図
move_RL(SPEED_L,-25);
Wait(1100);
m_kousa++; //交差点をカウント
n_black=0; //n_blackを0に戻すことでライントレースに戻る。
} else if (m_kousa < 2) { //交差点が4回目のとき交差点を左折
short_break;
PlaySound(SOUND_DOWN); //左折の合図
move_RL(0, 27);
Wait(2800);
m_kousa++;
n_black=0;
} else { //B地点前T字を交差点5回目として直進しB地点へ入る
short_break;
move_RL(-SPEED_L,30);
Wait(600);
Off(OUT_AB);
m_kousa++; //m_kousa(交差点)=5回目となり、While(m_kousa<5)から抜け出 す。
PlaySound(SOUND_FAST_UP); //終了の合図
}
}
}
sub CP()
{
SetSensorLight(S1);
int n_black=0; //黒をカウント
int m_kousa=0; //交差点をカウント
while(m_kousa < 2) { //交差点が5回目未満の間
while (n_black < nMAX) { //n_black(黒)がnMAX未満の間、ライントレースをする
if (SENSOR_1 < THRES1) { //もし黒のとき左旋回、黒の回数をカウント
move_RL(SPEED_L,-25);
n_black++;
} else { //そうでないとき
if (SENSOR_1 < THRES2) { //もし黒に近い黒⇔黒の間は左折
move_RL(SPEED_L,0);
} else if (SENSOR_1 < THRES3) { //薄黒⇔黒に近い黒の間は直進
move_RL(SPEED_H,50);
} else if (SENSOR_1 < THRES4) { //白に近い黒⇔薄黒の間は右折
move_RL(0, 25);
} else { //白のとき右旋回
move_RL(-SPEED_L,25);
}
n_black=0; //n_black(黒)をリセットして
}
Wait(STEP); //上記の動作を、STEP(1回)ずつ行う。
}
if (m_kousa < 1) { //もし交差点が3回目まで交差点を直進
short_break;
PlaySound(SOUND_DOUBLE_BEEP); //直進の合図
move_RL(SPEED_L,-25);
Wait(900);
m_kousa++; //交差点をカウント
n_black=0; //n_blackを0に戻すことでライントレースに戻る。
} else { //交差点が4回目のとき交差点を左折
short_break;
PlaySound(SOUND_DOWN); //左折の合図
Off(OUT_AB);
m_kousa++;
}
}
}
sub PD()
{
SetSensorLight(S1);
int n_black=0; //黒をカウント
int m_kousa=0; //交差点をカウント
while(m_kousa < 2) { //交差点が5回目未満の間
while (n_black < nMAX) { //n_black(黒)がnMAX未満の間、ライントレースをする
if (SENSOR_1 < THRES1) { //もし黒のとき左旋回、黒の回数をカウント
move_RL(SPEED_L,-25);
n_black++;
} else { //そうでないとき
if (SENSOR_1 < THRES2) { //もし黒に近い黒⇔黒の間は左折
move_RL(SPEED_L,0);
} else if (SENSOR_1 < THRES3) { //薄黒⇔黒に近い黒の間は直進
move_RL(SPEED_H,50);
} else if (SENSOR_1 < THRES4) { //白に近い黒⇔薄黒の間は右折
move_RL(0, 25);
} else { //白のとき右旋回
move_RL(-SPEED_L,25);
}
n_black=0; //n_black(黒)をリセットして
}
Wait(STEP); //上記の動作を、STEP(1回)ずつ行う。
}
if (m_kousa < 1) { //もし交差点が3回目まで交差点を直進
short_break;
PlaySound(SOUND_DOUBLE_BEEP); //直進の合図
move_RL(SPEED_L,-25);
Wait(900);
m_kousa++; //交差点をカウント
n_black=0; //n_blackを0に戻すことでライントレースに戻る。
} else { //交差点が4回目のとき交差点を左折
short_break;
move_RL(-SPEED_L,30);
Wait(800);
Off(OUT_AB);
m_kousa++; //m_kousa(交差点)=5回目となり、While(m_kousa<5)から抜け出 す。
PlaySound(SOUND_FAST_UP); //終了の合図
}
}
}
sub DP()
{
SetSensorLight(S1);
int n_black=0; //黒をカウント
int m_kousa=0; //交差点をカウント
while(m_kousa < 1) { //交差点が5回目未満の間
while (n_black < nMAX) { //n_black(黒)がnMAX未満の間、ライントレースをする
if (SENSOR_1 < THRES1) { //もし黒のとき左旋回、黒の回数をカウント
move_RL(SPEED_L,-25);
n_black++;
} else { //そうでないとき
if (SENSOR_1 < THRES2) { //もし黒に近い黒⇔黒の間は左折
move_RL(SPEED_L,0);
} else if (SENSOR_1 < THRES3) { //薄黒⇔黒に近い黒の間は直進
move_RL(SPEED_H,50);
} else if (SENSOR_1 < THRES4) { //白に近い黒⇔薄黒の間は右折
move_RL(0, 25);
} else { //白のとき右旋回
move_RL(-SPEED_L,25);
}
n_black=0; //n_black(黒)をリセットして
}
Wait(STEP); //上記の動作を、STEP(1回)ずつ行う。
}
if (m_kousa < 1) { //もし交差点が3回目まで交差点を直進
short_break;
PlaySound(SOUND_DOWN); //左折の合図
Off(OUT_AB);
m_kousa++; short_break;
} else { //交差点が4回目のとき交差点を左折
short_break;
PlaySound(SOUND_DOWN); //左折の合図
Off(OUT_AB);
m_kousa++;
}
}
}
sub PC()
{
SetSensorLight(S1);
int n_black=0; //黒をカウント
int m_kousa=0; //交差点をカウント
while(m_kousa < 2) { //交差点が5回目未満の間
while (n_black < nMAX) { //n_black(黒)がnMAX未満の間、ライントレースをする
if (SENSOR_1 < THRES1) { //もし黒のとき左旋回、黒の回数をカウント
move_RL(SPEED_L,-25);
n_black++;
} else { //そうでないとき
if (SENSOR_1 < THRES2) { //もし黒に近い黒⇔黒の間は左折
move_RL(SPEED_L,0);
} else if (SENSOR_1 < THRES3) { //薄黒⇔黒に近い黒の間は直進
move_RL(SPEED_H,50);
} else if (SENSOR_1 < THRES4) { //白に近い黒⇔薄黒の間は右折
move_RL(0, 25);
} else { //白のとき右旋回
move_RL(-SPEED_L,25);
}
n_black=0; //n_black(黒)をリセットして
}
Wait(STEP); //上記の動作を、STEP(1回)ずつ行う。
}
if (m_kousa < 1) { //もし交差点が3回目まで交差点を直進
short_break;
PlaySound(SOUND_DOWN); //左折の合図
move_RL(0, 27);
Wait(2800);
m_kousa++;
n_black=0;
} else { //交差点が4回目のとき交差点を左折
short_break;
move_RL(-SPEED_L,30);
Wait(600);
Off(OUT_AB);
m_kousa++; //m_kousa(交差点)=5回目となり、While(m_kousa<5)から抜け出 す。
PlaySound(SOUND_FAST_UP);
}
}
}
sub get() //紙コップを掴む(1周目)
{
SetSensorLowspeed(S2); // 端子2には超音波センサ
go_forward;
/* 25cm以内に障害物があるとコップを掴みに行く */
if (SensorUS(S2) <= 25) { arm(590); }
Off(OUT_AB);
}
sub get_2() //紙コップを掴む(2周目)
{
SetSensorLowspeed(S2); // 端子2には超音波センサ
go_forward;
/* 25cm以内に障害物があるとコップを掴みに行く */
if (SensorUS(S2) <= 25) { arm(1150); }
Off(OUT_AB);
}
sub release() //紙コップを離す
{
OnFwd(OUT_C,ARM);
Wait(200);
Off(OUT_C);
SendRemoteNumber(CONN,MAILBOX1,Boot);
}
task main()
{
AC();
get();
back();
turn_right_B();
Wait(2200);
CP();
release();
back();
turn_right_B();
Wait(3800);
PD();
get();
back();
turn_right_B();
Wait(2200);
turn_right_A();
Wait(3000);
DP();
release();
back();
turn_right_B();
Wait(3400);
PC();
get_2();
back();
turn_right_B();
Wait(2200);
CP();
release();
back();
turn_right_B();
Wait(3800);
}
苦労した点†