胡泊 */
/*机器人须加装红外测距卡，探头安装在机器人左侧，并将地面灰度传感器的信号线从PE5跳到PE7，即用导线将PE5和PE7连接起来，且将PE5下面的针脚剪断。然后将红外测距卡装在下，地面灰度检测卡装在上*/
int ir_1=0; /*红外变量*/
int photo_1=0; /*左眼光敏变量*/
int photo_2=0; /*右眼光敏变量*/
int bmp_1=0; /*碰撞变量*/
float tim_1=0.0; /*时间变量，用于搜索4号房间*/
float tim_2=0.0; /*时间变量，用于回家途中，观察经过白线的时间，
如果超过0.3秒，说明已到家了*/
int rot_1=1; /*用于从2号房间回家。从2号房间出来，走到通向3号房间的巷子口，
须右转180度，再左手走，沿着4号房间的墙壁回家*/
int gi_1=0; /*搜索房间时，从3号房间出来，走到4号房间的位置，
须右转180度，再左手走，进入4号房间*/
int gi_2=0; /*用于任务切换，先搜索房间，然后趋光，其次灭火，最后回家*/
int gi_3=0; /*发现火焰前，用于记住房间号，进1号房间该变量为
1，出1号房间该变量 为2；进2号房间该变量为3，出2号房间该变量为4；依此类推。*/
float gf_1=0.0; /*浮点变量，用于记下发现白线的时刻*/
float gf_2=0.0; /*浮点变量，用于计算经过白线的时间*/
int ma_1=0; /*扩展卡模拟变量，用于红外测距卡检测距离*/
int ma_3=0; /*扩展卡模拟变量，用于地面灰度检测*/
void main()
{
start_process(task_0()); /*因为地面灰度会影响到机器人运行中的每一个环节：进房 间、灭火、回家。所以必须随时检测，单开进程。*/
start_process(task_1()); /*感觉红外测距似乎没必要单开一个进程，只是回家时候使 用，并且对时间要求不高，单开进程会浪费资源，影响程序效率*/
gi_1 = 1 ;
gi_2 = 1 ;
gi_3 = 0 ;
while(gi_2 < 5)
{
if(gi_2 == 1) /*搜索房间*/
{
if(gi_1 == 1)
{
SubRoutine_1 (); /*左手走*/
tim_1 =seconds();
if( tim_1 > 70.000000) /*已完成123号房间的搜索，并且到了4号房间的附近*/
{
gi_1=2 ; /*
此种进入4号房间的方法我也曾经用过，靠时间来判断是否到了4号房间并不可靠，
本人就曾经栽在这种方法上，不建议使用此种方法 */
}
}
else
{
if(gi_1 == 2)
{
drive( 0 , 80); /*右转180度*/
wait( 0.650000 );
stop();
gi_1 = 3 ;
}
else
{
if(gi_1 == 3)
{
SubRoutine_1 (); /*继续左手走，进入4号房间*/
}
}
}
photo_1=photo(1); /*检测左眼的亮度*/
photo_2=photo(2); /*
检测右眼的亮度*/
if((photo_1 < 100) || (photo_2 < 100)) /*如果左眼或右眼发现火焰*/
{ /*用光敏传感器我没有做过，如果用火焰传感器的话小于100时机器人必定已进入了蜡烛周围的白圈，此参数应根据自己实际试验的数据来确定*/
gi_2 = 2 ; /*切换到趋光*/
}
}
else
{
if(gi_2 == 2)
{
SubRoutine_2 (); /*趋光*/
ma_1 =analogport(7); /*检测地面灰度*/ /*趋光后才检测白线不可靠， 如3或4小房间机器人刚进入后有可能已经进入白圈了*/
if(ma_1 < 170) /*如果发现蜡烛前的白线*/
{
gi_2 = 3 ; /*切换到灭火*/
}
}
else
{
if(gi_2 == 3)
{
SubRoutine_4 (); /*灭火*/
gi_2 = 4 ; /*切换到回家*/
}
else
{
if(gi_2 == 4) /*进入回家模式*/
{
if(gi_3 == 1) /*从1号房间回家*/
{
SubRoutine_5 (); /*右手走*/
}
else
{
if(gi_3 == 3) /*从2号房间回家*/
{
if(rot_1 == 1) /*灭火后，从2号房间出来*/
{
SubRoutine_1 (); /*左手走*/
}
else
{
if(rot_1 == 2) /*到了通往3号房间的巷子*/
{
drive( 0 , 80); /*右转180度*/ /*程序中应 尽量避免这种强制的不接受外部反馈的硬性动作，应尽量采用柔性控制 */
wait( 0.650000 );
stop();
rot_1 = 3 ;
}
else
{
if(rot_1 == 3)
{
SubRoutine_1 (); /*左手走*/
}
}
}
}
else
{
if(gi_3 == 5) /*从3号房间回家*/
{SubRoutine_1 (); /*左手走*/
}
else
{
if(gi_3 == 7) /*从4号房间回家*/
{
SubRoutine_1 (); /*左手走*//*从4号房间出 来后一直采用左手走会紧贴4号房间的墙壁走，容易错过起点白圆*/
}
}
}
}
}
}
}
}
}
}
/*主程序的结构是一个大的循环用一些判断分割成数段，这样做的好处是避免了多次调用子程序引发的频繁的出栈入栈的时间损失，但程序整体的可读性较差，不便于维护*/
void task_0() /*检测地面灰度*/
{
wait( 1.000000 ); /*离开出发点*/ /*这句很关键，可以避免重复检测白线*/
while(gi_2 < 5)
{
if(gi_2 == 1) /*如果处于搜索房间的状态*/ /*起始加入一个判断，避免无谓的检 测*/
{
ma_1 =analogport(7); /*检测地面灰度*/
if(ma_1 < 170) /*如果发现白线*/ /*此参数也应该根据自己的硬件条 件和场地光照情况适当调整*/
{
while(ma_1 < 170) /*当机器人在白线上的时候*/
{
ma_1 =analogport(7); /*检测
地面灰度*/
}
gi_3 = gi_3 + 1; /*一旦机器人离开白线，则计数一次*/
}
}else
{
if(gi_2 == 4) /*如果处于回家的状态*/
{
ma_1 =analogport(7); /*检测地面灰度*/
if(ma_1 < 170) /*如果发现白色*/
{
tim_2 =seconds(); /*读取系统时间*/
gf_1 = tim_2 ; /*记下发现白色的时刻*/
while(ma_1 < 170) /*当机器人在白色地面上的时候*/
{
ma_1 =analogport(7); /*检测地面灰度*/
tim_2 =seconds(); /*读取系统时间*/
}
gf_2 = tim_2 - gf_1; /*计算在白色地面上行走的时间*/
if(gf_2 > 0.300000) /*发现出发点的白色正方形*/
{
stop(); /*机器人停止运动*/
gi_2 = 5 ; /*跳出条件循环，程序结束*/
}
}
}
}
}
}
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();
if( bmp_1 == 3)
{
drive( -80 ,0);
wait( 0.200000 );
stop();
drive( 0 , 80);
wait( 0.100000 );
stop();
}
}
}
return;
}
void SubRoutine_2( ) /*趋光*/
{
photo_1 =photo(1);
photo_2 =photo(2);
if(photo_1 < photo_2)
{
motor( 1 , 10 );
motor( 2 , 80 );
}
else
{
if(photo_2 < photo_1)
{
motor( 1 , 80 );
motor( 2 , 10 );
}
else
{
motor( 1 , 80 );
motor( 2 , 80 );
}
}
return; /*趋光的基本方法，靠两个传感器返回值的差判断机器人相对蜡烛的 方向，同样对两个传感器的位置要求较高*/
}
void SubRoutine_4( ) /*灭火*/
{
stop();
wait( 0.500000 );
motor( 1 , -100 );
motor( 2 , 100 );
motor( 3 , 100 );
wait( 5.000000 );
stop();
return; /*灭火后应有检测，避免蜡烛火焰未熄灭机器人便开始回家*/
}
void SubRoutine_5( ) /*右手走*/
{
ir_1 =ir_detector();
if( ir_1 == 4){
drive( 0 , -80);
wait( 0.100000 );
stop();
}
else
{
ir_1 =ir_detector();
if( ir_1 == 2)
{
drive( 0 , -80);
wait( 0.100000 );
stop();
}
else
{
motor( 1 , 100 );
motor( 2 , 20 );
}
}
bmp_1 = bumper();
if( bmp_1 != 0)
{
drive( -80 ,0);
wait( 0.200000 );
stop();
drive( 0 , -80);
wait( 0.100000 );
stop();
}
return;
} /*右手走用于回家，对避免撞墙的要求不高，可适当简化程序*/
/*这是一个最基本的灭火程序，但基本上实现了灭火比赛的一般要求，回家、避障、灭火等均可完成，一些细节完成的也很好。但如果要取得好成绩，这样的程序显然是不够的，而且稳定性不够好，一个环节出现失误会带来连锁反应，影响后面任务的完成，应加入一定的失误修补代码。同时如果要在比赛中取得好成绩，硬件也十分关键，不一定要非常好的配置，但位置、连接等因素也极为关键，需要
多多调试，积累丰富的经验才可以。*/下载本文
