[[2016b/Member]]
#define THRESHOLD 45 //白と黒の中間の反射率
#define SPEED_Fast 60 // 速いスピード
#define SPEED_Slow 50 // 遅いスピード
#define OnLR(speedR,speedL) OnRev(OUT_A,speedR);OnRev(OUT_B,speedL); //ABの動作同時指定
/*
task pitagora() //ボールを取ったときのメドレー
{
//while(true)
//{
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(300);
PlayTone(1175,140);
Wait(160);
PlayTone(1318,130);
Wait(300);
PlayTone(1568,140);
Wait(160);
PlayTone(1480,130);
Wait(300);
PlayTone(1175,130);
Wait(160);
PlayTone(1318,130);
Wait(300);
PlayTone(988,130);
Wait(150);
PlayTone(880,140);
Wait(310);
PlayTone(988,130);
Wait(150);
PlayTone(1046,140);
Wait(310);
PlayTone(1109,130);
Wait(150);
PlayTone(1175,140);
Wait(460);
PlayTone(1175,130);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(1568,130);
Wait(150);
PlayTone(1480,130);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(300);
PlayTone(1175,140);
Wait(160);
PlayTone(1318,130);
Wait(300);
PlayTone(2093,140);
Wait(160);
PlayTone(1975,130);
Wait(300);
PlayTone(1568,140);
Wait(160);
PlayTone(1760,130);
Wait(300);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,140);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,140);
Wait(310);
PlayTone(1568,130);
Wait(150);
PlayTone(1480,140);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(988,130);
Wait(150);
PlayTone(880,440);
Wait(460);
PlayTone(1318,130);
Wait(310);
PlayTone(1318,130);
Wait(150);
PlayTone(1175,440);
Wait(770);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(300);
PlayTone(1175,140);
Wait(160);
PlayTone(1318,130);
Wait(300);
PlayTone(1568,140);
Wait(160);
PlayTone(1480,130);
Wait(300);
PlayTone(1175,140);
Wait(160);
PlayTone(1318,130);
Wait(300);
PlayTone(1175,130);
Wait(160);
PlayTone(1318,130);
Wait(300);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,140);
Wait(310);
PlayTone(2093,130);
Wait(150);
PlayTone(1975,140);
Wait(310);
PlayTone(1568,130);
Wait(150);
PlayTone(1760,140);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(1568,130);
Wait(150);
PlayTone(1480,130);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(1046,130);
Wait(150);
PlayTone(1175,130);
Wait(300);
PlayTone(1046,140);
Wait(160);
PlayTone(1175,130);
Wait(300);
PlayTone(1397,140);
Wait(160);
PlayTone(1318,130);
Wait(300);
PlayTone(1046,140);
Wait(160);
PlayTone(1175,130);
Wait(300);
PlayTone(932,130);
Wait(160);
PlayTone(1046,130);
Wait(300);
PlayTone(932,130);
Wait(150);
PlayTone(1046,140);
Wait(310);
PlayTone(1244,130);
Wait(150);
PlayTone(1175,140);
Wait(310);
PlayTone(1046,130);
Wait(150);
PlayTone(988,140);
Wait(310);
PlayTone(880,130);
Wait(150);
PlayTone(784,1050);
Wait(1380);
PlayTone(1397,440);
Wait(460);
PlayTone(1318,130);
Wait(300);
PlayTone(1318,140);
Wait(160);
PlayTone(1480,130);
Wait(150);
PlayTone(1318,130);
Wait(150);
PlayTone(1480,140);
Wait(160);
PlayTone(1568,440);
Wait(460);
PlayTone(1175,130);
Wait(300);
PlayTone(988,130);
Wait(160);
PlayTone(1046,130);
Wait(300);
PlayTone(1046,130);
Wait(150);
PlayTone(1175,140);
Wait(160);
PlayTone(1109,130);
Wait(150);
PlayTone(1175,130);
Wait(150);
PlayTone(988,440);
Wait(770);
PlayTone(1397,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(1318,130);
Wait(150);
PlayTone(1480,130);
Wait(150);
PlayTone(1318,140);
Wait(160);
PlayTone(1480,130);
Wait(150);
PlayTone(1568,440);
Wait(460);
PlayTone(1175,130);
Wait(310);
PlayTone(988,130);
Wait(150);
PlayTone(1046,130);
Wait(310);
PlayTone(1046,130);
Wait(150);
PlayTone(1175,130);
Wait(150);
PlayTone(1109,130);
Wait(150);
PlayTone(1175,140);
Wait(160);
PlayTone(988,440);
Wait(760);
PlayTone(1175,140);
Wait(160);
PlayTone(1318,130);
Wait(300);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,140);
Wait(310);
PlayTone(1568,130);
Wait(150);
PlayTone(1480,140);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,140);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(1568,130);
Wait(150);
PlayTone(1480,130);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(300);
PlayTone(1568,140);
Wait(160);
PlayTone(1480,130);
Wait(300);
PlayTone(1175,140);
Wait(160);
PlayTone(1318,130);
Wait(300);
PlayTone(1175,140);
Wait(160);
PlayTone(1318,130);
Wait(300);
PlayTone(1175,130);
Wait(160);
PlayTone(1318,130);
Wait(300);
PlayTone(2093,130);
Wait(150);
PlayTone(1975,140);
Wait(460);
PlayTone(1568,140);
Wait(1230);
// }
//}
//task pitagora_owari()
//{
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(1175,130);
Wait(150);
PlayTone(1318,130);
Wait(310);
PlayTone(2093,130);
Wait(150);
PlayTone(1975,130);
Wait(460);
PlayTone(1568,280);
Wait(300);
//}
}
*/
sub line_trace(int sensor_light_lv) //ライントレースのプロセス
{
if (sensor_light_lv < THRESHOLD-8)
{
OnRev(OUT_B,SPEED_Fast);
OnRev(OUT_A,-SPEED_Fast);
}
else if (sensor_light_lv < THRESHOLD-5)
{
OnRev(OUT_B,SPEED_Slow);
}
else if (sensor_light_lv < THRESHOLD +5)
{
OnRev(OUT_AB,50);
}
else if (sensor_light_lv < THRESHOLD+8)
{
OnRev(OUT_A,SPEED_Slow);
}
else
{
OnRev(OUT_A,SPEED_Fast);
OnRev(OUT_B,-SPEED_Fast);
}
}
sub go_forward_intersection() //交差点を通過する�△料犧�
{
OnRev(OUT_AB,SPEED_Slow);
Wait(410);
Off(OUT_AB);
}
sub turn_r_intersection() //交差点の右に車体を向かせる
{
OnLR(60,-60);
Wait(450);
}
sub turn_l_intersection() // 交差点の左に車体を向かせる
{
OnLR(-60,60);
Wait(450);
} //ボールを取る動作
sub arm_lock()
{
Off(OUT_ABC);
OnRev(OUT_AB,25);
OnFwd(OUT_C,-12);
Wait(3200);
Off(OUT_ABC);
}
sub arm_unlock() //アームを上げ、ボールを自由にする
{
OnFwd(OUT_C,30);
}
long intersection_judgment(long t0) //Sensorの値が34(任意の反射率)以上のとき、引数に現在の時刻を代入し返す。それ以外は変更せず引数を返す。
{
if (SENSOR_1 > 34)
{
t0 = CurrentTick();
}
return t0 ;
}
task main()
{
SetSensorLight(S1); // ライトセンサーの定義
SetSensorLowspeed(S2); //超音波センサーの定義
int intersection_count = 0; //交差点のカウント変数の定義
int count_look=0; //ボールを掴んでいるかの変数の定義
long t0 = CurrentTick(); //時間を記憶する変数の定義
RotateMotor(OUT_AB,SPEED_Slow,-1000); //スタート地点から前進する
while(CurrentTick()-t0 < 6000) //前進から6秒ほど（無条件）ライントレースをすることで、確実に線上に乗る
{
line_trace(SENSOR_1); //ライントレースを呼び出す
}
t0 = CurrentTick(); // 交差点の判定を始めるためにタイマーをリセット
while(true)
{
//NumOut(0,0,SensorUS(S2) , true);
while(CurrentTick()-t0 < 410) //指定した反射率（intersection_judgmentによる処理）が、410(任意の時間)連続でなければ、繰り返す
{
line_trace(SENSOR_1);
t0=intersection_judgment(t0);
}
PlaySound(SOUND_UP); //ループを抜けた（交差点の処理が始まる）とき、音を鳴らす
Off(OUT_AB); //車体の停止
Wait(1000);
intersection_count +=1; //交差点をカウントする
if (intersection_count <=5 ) //交差点が5回以内のときの処理
{
turn_r_intersection(); //�△僚萢�
go_forward_intersection(); //��の処理
t0 = CurrentTick(); //タイマーをリセット
while(CurrentTick()-t0 < 1000) //�い僚萢�（1秒間ライントレースをする）
{
line_trace(SENSOR_1);
}
if (intersection_count ==2) //2回めの交差点のとき
{
RotateMotor(OUT_AB,SPEED_Slow,500); //後ろに（正確に）下がる
while(SensorUS(S2)<=20) //超音波センサーが20cm以内のときループ
{
OnRevSync(OUT_AB,40,0); //正確に前進
}
arm_lock(); //ボールの取得
//start pitagora ; //メロディを流す
}
}
else if (intersection_count ==6 ) //上のプログラムのgo_forward_intersectionに条件式「if (intersection_count !=6 )」をつければ、コンパクトにはなるが、シュートの処理は調節が必要なので分離
{
turn_l_intersection();
t0 = CurrentTick();
while(CurrentTick()-t0 < 1500) //ライントレースでラインに確実に乗る
{
line_trace(SENSOR_1);
}
Off(OUT_AB); //前進しながら、ボールを離し、慣性によってシュートする
OnRevSync(OUT_AB,40,0);
arm_unlock();
Wait(1500);
Off(OUT_ABC);
break; //プログラムの終了
}
t0 = CurrentTick();
}
}
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
