2006a/B4/ロボコン
#define turn_right OnFwd(OUT_A);OnRev(OUT_C);
#define turn_left OnRev(OUT_A);OnFwd(OUT_C);
#define go OnFwd(OUT_A+OUT_C);
#define back_ OnRev(OUT_A+OUT_C);
#define stop_turn Off(OUT_A+OUT_C);
#define arm_down OnRev(OUT_B);
#define arm_up OnFwd(OUT_B);
#define tuuka 100//ラインを横切る時間1/100秒
#define turn 95//曲がる時間1/100秒
#define usetumae 186//右折前の前進時間1/100秒
#define sasetumae 186//ahidari折前の前進時間1/100秒
sub touch (){
until(SENSOR_2==1);
OnRev(OUT_B);
Wait(17);
}
sub trace (){
while((SENSOR_1 >40) || (SENSOR_3 >40))
{
OnFwd(OUT_A+OUT_C);
until((SENSOR_1 <40) || (SENSOR_3 <40));
if(SENSOR_3 <40)
{
OnFwd(OUT_A);
Off(OUT_C);
Wait(10);
}
if(SENSOR_1 <40)
{
OnFwd(OUT_C);
Off(OUT_A);
Wait(10);
}
}
}
sub back () {
while((SENSOR_1 >40) || (SENSOR_3 >40))
{
OnRev(OUT_A+OUT_C);
until((SENSOR_1 <40) || (SENSOR_3 <40));
if(SENSOR_3 <40)
{
OnRev(OUT_A);
Off(OUT_C);
Wait(10);
}
if(SENSOR_1 <40)
{
OnRev(OUT_C);
Off(OUT_A);
Wait(10);
}
}
}
sub left (){
go;
Wait(30);
OnFwd(OUT_A);
OnRev(OUT_C);
Wait(turn);
until(SENSOR_1 <40);
Off(OUT_A+OUT_C);
}
sub right (){
go;
Wait(30);
OnFwd(OUT_C);
OnRev(OUT_A);
Wait(turn);
until(SENSOR_3 <40);
Off(OUT_A+OUT_C);
}
sub turn180 (){
OnFwd(OUT_A);
OnRev(OUT_C);
Wait(194);
until(SENSOR_3 <40);
}
task main ()
{
SetSensor(SENSOR_3, SENSOR_LIGHT);
SetSensor(SENSOR_1, SENSOR_LIGHT);
SetSensor(SENSOR_2, SENSOR_TOUCH);
repeat(3){
touch ();
go;
Wait(150);
trace ();
left ();
trace ();
go;
Wait(30);
trace ();
right ();
arm_up;
Wait(17);
back_;
Wait(400);
go;
Wait(50);
arm_down;
Wait(17);
trace ();
right ();
trace ();
go;
Wait(30);
trace ();
left ();
back_;
Wait(200);
arm_up;
Wait(17);
stop_turn;
}
}