智能灭火机器人设计第程序Word格式.docx
《智能灭火机器人设计第程序Word格式.docx》由会员分享，可在线阅读，更多相关《智能灭火机器人设计第程序Word格式.docx（28页珍藏版）》请在冰点文库上搜索。
//P_INT_Ctrl
C_FIQ_TMA
0x2000
C_FIQ_TMB
0x0800
C_IRQ4_1KHz
0x0010
C_IRQ4_2KHz
0x0020
C_IRQ4_4KHz
0x0040
C_IRQ5_2Hz
0x0004
C_IRQ5_4Hz
0x0008
C_TMB_32KHz
0x0002
//P_TimerB_Ctrl
C_TMB_PWM_OFF
0x0000
P_Watchdog_Clear
（volatileunsignedint*）0x7012
unsigned
intmm,min,cny,Delaynumber,xp=255,flag,dd;
intleft,right;
intpulse_x,pulse_y;
intpaixu[7];
//
排序
intchdata[7];
火焰传感器检测通道
intworkstate=0;
//
进入检测范围
intsucessfire=0;
//灭火成功标记
void
PWM_left（unsignedinthigh_time,unsignedintcyclical_time）;
PWM_right（unsignedinthigh_time,unsignedintcyclical_time）;
forward（unsignedintm,unsignedintn）;
forward_s（unsignedintm,unsignedintn）;
forward_s2（unsignedintm,unsignedintn）;
backward（unsignedintm,unsignedintn）;
turn_left（unsignedintm）;
turn_right（unsignedintm）;
stop（void）;
voidget_AD（void）;
//AD转换
voidsystem_init（void）;
//系统初始化
voiddelay_ms（unsignedintt）;
//ms级延时程序
voiddelay_s（unsignedintt）;
//s级延时程序
adjust（void）;
调整
adjust_s（void）;
粗调
seekfire（void）;
寻找火源
fire（void）;
灭火
hf（void）;
intabs（intm）;
voidbizhan（void）;
避障
voidbiya（void）;
避崖
Stage0（void）;
阶段0
Stage1（void）;
阶段1
Stage2（void）;
阶段2
Stage3（void）;
阶段3
revison（）;
修正函数
//++++++++++++主函数++++++++++++++++++//
intmain（void）
{
system_init（）;
delay_s（3）;
while
（1）
seekfire（）;
fire（）;
delay_s（8）;
}
//***************系统初始化*********************//
voidsystem_init（void）
__asm（"
INTOFF"
）;
*P_IOA_Dir=0xE700;
*P_IOA_Attrib=0xE7FF;
*P_IOA_Data=0x1800;
*P_IOB_Dir=0x0FFF;
*P_IOB_Attrib=0x0FFF;
*P_IOB_Data=0xF000;
*P_TimerB_Ctrl=（C_TMB_32KHz|C_TMB_PWM_OFF）;
*P_TimerB_Data=（65536-0.1*0.001*32768）;
*P_INT_Ctrl|=（C_FIQ_TMB+C_IRQ4_1KHz+C_IRQ5_2Hz+C_IRQ5_4Hz）;
//TMB
用来控制
2个电机，
1KHZ控制避崖，
INTIRQ,FIQ"
//2HZ
AD采集
4HZ
用来避
障
//***************寻找火源*********************//
voidseekfire（void）
Stage0（）;
Stage1（）;
阶段
1
按一定的路径走
有发现火源，做初步调整
Stage2（）;
Stage3（）;
2
3
接近火源，边走边调整
到达火源前面，做最后的调整
voidStage0（void）//
flag=0;
turn_left（120）;
//转360度扫描有没有火源
forward（250,250）;
if（workstate==1&
&
flag==0）break;
voidStage1（void）//
flag=1;
Set_IOB_Data（0,1）;
开启蜂鸣器报警
adjust_s（）;
switch（mm）
case1:
forward（60,60）;
break;
case2:
forward（55,55）;
break;
case3:
forward（50,50）;
case4:
forward（45,45）;
case5:
forward（35,35）;
case6:
forward（30,30）;
case7:
forward（20,20）;
case8:
forward（10,10）;
case9:
forward（5,5）;
case10:
forward（2,2）;
case11:
forward_s（1,1）;
default:
voidStage2（void）//阶段2
flag=2;
adjust（）;
forward（40,40）;
forward（25,25）;
forward（18,18）;
forward（15,15）;
forward（12,12）;
if（chdata[3]<
=0x05||chdata[4]<
=0x05）
voidStage3（void）//
灭火前最后调整
unsignedinti;
flag=3;
for（i=0;
i<
20;
i++）
=0x05&
chdata[4]<
=0x05
abs（chdata[3]-chdata[4]）<
=2
abs（chdata[2]-chdata[5]）<
=8）break;
chdata[4]>
0x05）{turn_left
（1）;
delay_ms（10）;
}if（chdata[3]>
0x05&
chdata[4]<
=0x05）{turn_right
（1）;
}
stop（）;
//***************灭火*********************//
voidfire（）
unsignedintn;
while（chdata[3]<
=0x7f||chdata[4]<
=0x7f）
if（n>
=1）revison（）;
如果
1次吹不灭则调用修正
Set_IOA_Data（15,1）;
灭火风扇开启
Set_IOA_Data（15,0）;
灭火风扇关闭
n++;
sucessfire=1;
//1
灭火成功
workstate=0;
Set_IOB_Data（0,0）;
xp=255;
mm=0;
min=255;
//***************修正子程序*********************//
voidrevison（）
forward（8,8）;
//***************voidadjust_s（void）{
调整*********************//
if（min==chdata[1]）turn_left（21）;
if（min==chdata[2]）turn_left（12）;
if（min==chdata[3]&
abs（chdata[3]-chdata[4]）>
30）turn_left（5）;
if（min==chdata[4]&
30）turn_right（5）;
if（min==chdata[5]）turn_right（15）;
if（min==chdata[6]）turn_right（30）;
voidadjust（void）
while（min<
=0xc9）
if（min==chdata[1]）
if（min==chdata[2]）
{turn_left（21）;
delay_ms（20）;
{turn_left（5）;
if（min==chdata[3]&
20
delay_ms（20）;
if（min==chdata[4]&
=20）break;
）{turn_left
（1）;
）{turn_right
（1）;
if（min==chdata[5]）{turn_right（5）;
if（min==chdata[6]）{turn_right（30）;
//***************前进*********************//
voidforward（unsignedintm,unsignedintn）
pulse_x=0;
pulse_y=0;
if（pulse_x<
=m&
pulse_y<
=n）{PWM_left（17,217）;
PWM_right（13,213）;
}if（pulse_x>
m&
=n）{PWM_right（13,213）;
}if（pulse_x<
pulse_y>
n）{PWM_left（17,217）;
if（pulse_x>
n）{pulse_x=0;
pulse_y=0;
}if（workstate==1&
flag==0）{pulse_x=0;
voidforward_s（unsignedintm,unsignedintn）
=n）{PWM_left（17,417）;
PWM_right（13,413）;
=n）{PWM_right（13,413）;
n）{PWM_left（17,417）;
voidforward_s2（unsignedintm,unsignedintn）
=n）{PWM_left（15,215）;
PWM_right（15,215）;
=n）{PWM_right（15,215）;
n）{PWM_left（15,215）;
//***************后退*********************//
voidbackward（unsignedintm,unsignedintn）
=n）{PWM_left（13,213）;
PWM_right（17,217）;
=n）{PWM_right（17,217）;
n）{PWM_left（13,213）;
//***************左转*********************//
voidturn_left（unsignedintm）
=m）{PWM_left（13,413）;
=m）{PWM_right（13,413）;
m）{PWM_left（13,413）;
m）{pulse_x=0;
//***************右转*********************//
voidturn_right（unsignedintm）
=m）{PWM_left（17,417）;
PWM_right（17,417）;
=m）{PWM_right（17,417）;
m）{PWM_left（17,417）;
}}
//***************
voidstop（void）
停止*********************//
*P_IOA_Data&
=~（bit13+bit14）;
将IOA13、IOA14清
//***************控制小车左轮*********************//
voidPWM_left（unsignedinthigh_time,unsignedintcyclical_time）
if（left
<
=high_time）
/*high_time
为脉冲的高电平时间，
cyclical_time
为脉冲周期时间
*/
Set_IOA_Data（14,1）;
将IOA14置
else
Set_IOA_Data（14,0）;
将IOA14清
if（left>
cyclical_time）
left=0;
pulse_x++;
*P_Watchdog_Clear=0x0001;
//***************控制小车右轮*********************//
voidPWM_right（unsignedinthigh_time,unsignedintcyclical_time）
if（right<
Set_IOA_Data（13,1）;
将IOA13置
Set_IOA_Data（13,0）;
将IOA13清
if（right>
right=0;
pulse_y++;
//*************
升级会员 