![[PukiWiki]](image/robotics-logo-small.jpg "[PukiWiki]"){kind=logo}
B地点からA地点へ行くロボットを制作する
#define Black 36
#define White 56
#define Get_Time 1 //検知の間隔
int SPD = 0;
int SPD_Rev = 0;
int LineTrace = 0;
int CNT = 0;
int Limit_R = 0;
int Limit_L = 0;
int Tracetime = 0;
///////////////////////////// サブルーチン //////////////////////////////////
//アームを動かす
sub Arm(int SWD){ //SWDの数値ででアームの上下運動を決定 する
RotateMotor(OUT_A,SWD,170);
Wait(800);
Off(OUT_A);
}
//前進する
sub Foward(int Fdist){
RotateMotor(OUT_BC,100,Fdist);
}
//超信地旋回
sub Turn_Point(int Tdist){
RotateMotorEx(OUT_BC,-70,Tdist,100,true,true);
}
//超信地旋回（逆）
sub Turn_Point2(int Tdist2){
RotateMotorEx(OUT_BC,-70,Tdist2,-100,true,true);
}
//ボールを放つ
void Shot(){
Arm(-50);
Foward(300);
}
//ライントレース（時間制御）
sub Line_trace_0(int Tracetime){
CNT=0;
SetSensorLight(S3);
if(LineTrace==0){SPD=80;SPD_Rev=-80;}
if(LineTrace==1){SPD=80;SPD_Rev=-80;}
if(LineTrace==2){SPD=-80;SPD_Rev=80;}
if(LineTrace==3){SPD=80;SPD_Rev=-80;}
while(Tracetime > CNT){
if( SENSOR_3 < Black ) {
OnFwd(OUT_B,SPD);OnFwd(OUT_C,SPD_Rev);
} else if( SENSOR_3 > White ) {
OnFwd(OUT_C,SPD);OnFwd(OUT_B,SPD_Rev);
} else {
OnFwd(OUT_BC,70);
}
Float(OUT_BC);
CNT++;
}
LineTrace++;
NumOut(80,LCD_LINE1,LineTrace);
}
//ライントレース（カウント制御）
void Line_trace_1(){
CNT=0;
SetSensorLight(S3);
if (LineTrace==1){Limit_R = 200;Limit_L = -1000;SPD = 80;SPD_Rev = -70;}
if (LineTrace==3){Limit_R = 100;Limit_L = -2000;SPD = -80;SPD_Rev = 70;}
while(Limit_L < CNT && CNT < Limit_R){
if( SENSOR_3 < Black ) {
if(CNT < 0){CNT = 0;}
OnFwd(OUT_B,SPD);OnFwd(OUT_C,SPD_Rev);
CNT++;
} else if( SENSOR_3 > White ) {
OnFwd(OUT_C,SPD);OnFwd(OUT_B,SPD_Rev);
if(CNT > 0){CNT = 0;}
CNT--;
} else {
OnFwd(OUT_BC,70);
CNT = 0;
}
Wait(Get_Time);
Float(OUT_BC);
NumOut(80,LCD_LINE1,LineTrace);
}
}
//////////////////////////////// プログラム開始 ///////////////////////////////////
task main() {
Line_trace_0(6000);
Line_trace_1();
PlayTone(880,30); //分岐点に到達
Wait(2000);
Foward(250);
Line_trace_0(7000);
PlayTone(1320,30); //ボールを回収
Arm(50);
Turn_Point(80);
Line_trace_0(9000);
Line_trace_1();
PlayTone(880,30); //分岐点に到達
Wait(2000);
Turn_Point2(100);
Line_trace_0(12000);
PlayTone(1320,40); //ゴール前に到達
Shot();
}
今回の課題は本当に難しく、プログラムの書き換えが何度も行われたり、電池の減りが早く、入れ替えによって何度も設定した値を直さないといけない事態が発生した。これでOnFwdの不正確さにあきあきし、RotateMotorの有用性を改めて実感した。カウンタ系も電池に大きく影響を受けるので注意が必要だった。ラインの右側、左側を選ぶかでライントレースが成功するか大きく分かれることを知り、ライントレースは見た目は簡単そうなのに一筋縄ではいかないことを思い知った。
IMG_3166.JPG
unnamed.jpg
