#define Rotate_R 55 //四つの閾値
#define Adjust_R 50
#define Adjust_L 40
#define Rotate_L 35
#define t_border 190 //直角・交差点 判断用の時間
#define POW_TURN 40
#define POWER 30
#define Gap 38 //機体旋回軸とセンサーの見ているところの距離
#define Pause() Off(OUT_BC); PlaySound(SOUND_DOWN); Wait(2000) //マクロ群
#define Snd_up() PlaySound(SOUND_UP) //サウンド関係
float ANGLE_CALC=178/90; //1度あたりの旋回時間
float Calc_long=2.0463; //角度計算用定数(360/Cir_fere)
sub go_f(int l){ //指定距離前進
float Ang=l*Calc_long;
RotateMotor(OUT_BC,POWER,Ang);
Off(OUT_BC);
}
sub turn_R(float angle){ //指定角度左旋回
RotateMotorEx(OUT_BC,POW_TURN,-ANGLE_CALC*angle,100,true,true);
}
sub rotate_L(){
Off(OUT_BC);
OnFwd(OUT_B,POWER);
OnRev(OUT_C,POWER);
until( SENSOR_4>Rotate_L ); //ちょうどいい所まで旋回
}
sub rotate_R(){
Off(OUT_BC);
OnFwd(OUT_C,POWER);
OnRev(OUT_B,POWER);
until( (SENSOR_4>=Adjust_L) && (SENSOR_4<=Adjust_R) ); //ちょうどいい所まで旋回
}
sub adjust_L(){
Off(OUT_BC);
OnFwd(OUT_B,POWER);
until( (SENSOR_4>=Adjust_L) && (SENSOR_4<=Adjust_R) || (SENSOR_4<Rotate_R)); //ちょうどいい所まで右輪だけ前進
}
sub adjust_R(){
Off(OUT_BC);
OnFwd(OUT_C,POWER);
until( (SENSOR_4>=Adjust_L) && (SENSOR_4<=Adjust_R) || (SENSOR_4>Rotate_L)); //ちょうどいい所まで左輪だけ前進
}
sub Step_Over(){ //ラインをまたぐ
OnFwd(OUT_B,POW_TURN);
OnRev(OUT_C,POW_TURN);
until(SENSOR_4<Adjust_R); //線が見えるまで旋回
//この時点では線の右側を見ているため
OnFwd(OUT_B,POW_TURN);
OnRev(OUT_C,POW_TURN);
until(SENSOR_4>Adjust_R); //左側を見るように合わせる
}
sub Turn_Right_angle(){ //直角に曲がる
Off(OUT_BC);
go_f(Gap); //直角と思われる地点に機体の旋回軸を合わせる
Step_Over(); //線の左側を見るように合わせる
}
sub cross(){ //交差点直進
Off(OUT_BC);
turn_R(30);
go_f(Gap); //少し前進
OnRev(OUT_B,POW_TURN);
OnFwd(OUT_C,POW_TURN);
until(SENSOR_4<Adjust_L); //線上に行くまで右旋回
}
task main(){
SetSensorLight(S4); //センサ定義
int N=1;
long t0=0,tmax=0,talt=0; //各変数初期化
OnFwd(OUT_BC,POWER); //前進開始
t0=CurrentTick(); //タイマーリセット
while(N<=13){
if(CurrentTick()-t0<t_border){
t0=CurrentTick(); //タイマーリセット
if(SENSOR_4<=Rotate_L) //黒の時（右に行き過ぎ＿左旋回）
rotate_L();
else if(SENSOR_4<=Adjust_L) //少し黒めの時（ちょっと右寄り＿左へ方向をいじる）
adjust_L();
else if((SENSOR_4>Adjust_L) && (SENSOR_4<=Adjust_R)) //ちょうどいい色の時（維持）
OnFwd(OUT_BC,POWER);
else if(SENSOR_4>=Rotate_R) //少し白めの時（ちょっと左寄り＿右へ方向をいじる）
rotate_R();
else if(SENSOR_4>Adjust_R) //白の時（左に行き過ぎ＿右旋回）
adjust_R();
}
else if((CurrentTick()-t0>=t_border) ){ //サイクルの時間で直角・交差点か判断
switch(N){ //N回目は直角,交差点などの判断をして関数に放り込む
case 1: //F地点
Off(OUT_BC);
Snd_up();
//Turn_Right_angle();
N++;
break;
case 2: //Q地点
Off(OUT_BC);
Pause();
//Turn_Right_angle();
N++;
break;
case 3: //R地点
Off(OUT_BC);
Snd_up();
cross();
N++;
break;
case 4: //S地点
Off(OUT_BC);
Snd_up();
//Turn_Right_angle();
N++;
break;
case 5: //G地点
Off(OUT_BC);
Snd_up();
//Turn_Right_angle();
N++;
break;
case 6: //H地点
Off(OUT_BC);
if(CurrentTick()-t0<t_border+220)
break;
Snd_up();
//Turn_Right_angle();
N++;
break;
case 7: //T_1地点
Off(OUT_BC);
Pause();
cross();
N++;
break;
case 8: //T_2地点
Off(OUT_BC);
Pause();
cross();
N++;
break;
case 9: //R地点
Off(OUT_BC);
Pause();
//Turn_Right_angle();
N++;
break;
case 10: //S地点
Off(OUT_BC);
Snd_up();
cross();
N++;
break;
case 11: //P地点
Off(OUT_BC);
Snd_up();
//Turn_Right_angle();
N++;
break;
case 12: //E地点
Off(OUT_BC);
Pause();
//Turn_Right_angle();
N++;
break;
case 13: //A地点
Off(OUT_BC);
N++;
break;
}
t0=CurrentTick() //タイマーリセット
}
}
turn_R(60);
go_f(Gap*5);
Off(OUT_BC);
PlaySound(SOUND_DOWN);
}
![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
