}
}
void task_1() /*红外测距*/
{
while(gi_2 < 5)
{
if((gi_2 == 4) && (gi_3 == 3)) /*如果处于回家的状态，且从2号房间回家*/
{
if(rot_1 == 1) /*如果从2号房间回家，尚未达到通往3号房间的巷子*/
{
wait( 3.000000 );
while(rot_1 == 1)
{
ma_3 =analogport(5); /*红外测距，机器人检测与左侧墙壁的距离 */
if(ma_3 < 25) /*如果机器人与左侧墙壁的距离超过70厘 米，说明发现巷子了*/
/*根据我使用广茂达红外测距传感器的经验，该传感器对角度要求较大，如果机器人在走廊中角度偏差较大，也可能检测离墙距离大于70cm，因此个人认为此种方法并不可行 */
{
rot_1 = 2 ; /*给出右转180度的命令，并且不再进行红外测 距*/
}
}
}
}
}
}
void SubRoutine_1( ) /*左手走*/
{
ir_1=ir_detector();
if(ir_1 == 4)
{
drive( 0 , 80);
wait( 0.100000 );
stop();
}
else
{
if(ir_1 == 1)
{
motor( 1 , 40 );
motor( 2 , 0 );
}
else
{
motor( 1 , 15 );
motor( 2 , 100 );
}
} /*基本的左手走的程序，但传感器位置需慢慢调试，找到最佳位置*/
bmp_1 = bumper(); /*碰撞检测，可避免机器人擦墙*/
if( bmp_1 == 1)
{
drive( -80 ,0);
wait( 0.200000 );
stop();
drive( 0 , 80);
wait( 0.100000 );
stop();
}
else
{
bmp_1 = bumper();
if( bmp_1 == 2)
{
drive( -80 ,0);
wait( 0.200000 );
stop();
drive( 0 , -80);
wait( 0.100000 );
stop();
}
else
{
bmp_1 = bumper();
【