智能灭火机器人第程序.doc

该【智能灭火机器人第程序 】是由【读书百遍】上传分享，文档一共【36】页，该文档可以免费在线阅读，需要了解更多关于【智能灭火机器人第程序 】的内容，可以使用淘豆网的站内搜索功能，选择自己适合的文档，以下文字是截取该文章内的部分文字，如需要获得完整电子版，请下载此文档到您的设备，方便您编辑和打印。
#include""
#define P_IOA_Data (volatileunsignedint*)0x7000
#define P_IOA_Dir (volatileunsignedint*)0x7002
#define P_IOA_Attrib (volatileunsignedint*)0x7003
#define P_IOB_Data (volatileunsignedint*)0x7005
#define P_IOB_Dir (volatileunsignedint*)0x7007
#define P_IOB_Attrib (volatileunsignedint*)0x7008
#define P_TimerB_Data (volatileunsignedint*)0x700C
#define P_TimerB_Ctrl (volatileunsignedint*)0x700D
#define P_INT_Ctrl (volatileunsignedint*)0x7010
#define P_INT_Clear (volatileunsignedint*)0x7011
#define P_ADC (volatileunsignedint*)0x7014
#define P_ADC_Ctrl (volatileunsignedint*)0x7015
#define P_ADC_MUX_Ctrl (volatileunsignedint*)0x702B
#define P_ADC_MUX_Data (volatileunsignedint*)0x702C
#define C_FIQ_PWM 0x8000 //P_INT_Ctrl
#define C_FIQ_TMA 0x //P_INT_Ctrl
#define C_FIQ_TMB 0x0800 //P_INT_Ctrl
#define C_IRQ4_1KHz 0x0010 //P_INT_Ctrl
#define C_IRQ4_2KHz 0x0020 //P_INT_Ctrl
#define C_IRQ4_4KHz 0x0040 //P_INT_Ctrl
#define C_IRQ5_2Hz 0x0004 //P_INT_Ctrl
#define C_IRQ5_4Hz 0x0008 //P_INT_Ctrl
#define C_TMB_32KHz 0x0002 //P_TimerB_Ctrl
#define C_TMB_PWM_OFF 0x0000 //P_TimerB_Ctrl
#define P_Watchdog_Clear (volatileunsignedint*)0x7012
unsignedintmm,min,cny,Delaynumber,xp=255,flag,dd;
unsignedintleft,right;
unsignedintpulse_x,pulse_y;
unsignedintpaixu[7];//排序
unsignedintchdata[7];//火焰传感器检测通道
unsignedintworkstate=0; //进入检测范畴
unsignedintsucessfire=0; //灭火成功标记
voidPWM_left(unsignedinthigh_time,unsignedintcyclical_time);
voidPWM_right(unsignedinthigh_time,unsignedintcyclical_time);
voidforward(unsignedintm,unsignedintn);
voidforward_s(unsignedintm,unsignedintn);
voidforward_s2(unsignedintm,unsignedintn);
voidbackward(unsignedintm,unsignedintn);
voidturn_left(unsignedintm);
voidturn_right(unsignedintm);
voidstop(void);
voidget_AD(void);//AD转换
voidsystem_init(void);//系统初始化
voiddelay_ms(unsignedintt);//ms级延时程序
voiddelay_s(unsignedintt);//s级延时程序
voidadjust(void);//调节
voidadjust_s(void);//粗调
voidseekfire(void);//寻找火源
voidfire(void);//灭火
voidhf(void);
intabs(intm);
voidbizhan(void);//避障
voidbiya(void);//避崖
voidStage0(void);//阶段0
voidStage1(void);//阶段1
voidStage2(void);//阶段2
voidStage3(void);//阶段3
voidrevison();//修正函数
//++++++++++++主函数++++++++++++++++++//
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-**32768);
*P_INT_Ctrl|=(C_FIQ_TMB+C_IRQ4_1KHz+C_IRQ5_2Hz+C_IRQ5_4Hz);//TMB用来控制2个电机,1KHZ控制避崖,
__asm("INTIRQ,FIQ"); //2HZ用来控制AD采集4HZ用来避障
}
//***************寻找火源*********************//
voidseekfire(void)
{
Stage0();//阶段0按一定的途径走
Stage1();//阶段1有发现火源,做初步调节
Stage2();//阶段2接近火源,边走边调节
Stage3();//阶段3达到火源前面,做最后的调节
}
voidStage0(void)//阶段0
{
flag=0;

while(1)
{
turn_left(120);//转360度扫描有无火源
forward(250,250);
if(workstate==1&&flag==0)break;
}
}
voidStage1(void)//阶段1
{
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);break;
case4:forward(45,45);break;
case5:forward(35,35);break;
case6:forward(30,30);break;
case7:forward(20,20);break;
case8:forward(10,10);break;
case9:forward(5,5);break;
case10:forward(2,2);break;
case11:forward_s(1,1);break;
default:break;
}
}
voidStage2(void)//阶段2
{
flag=2;
while(1)
{
adjust();
switch(mm)
{
case1:forward(40,40);break;
case2:forward(30,30);break;
case3:forward(25,25);break;
case4:forward(20,20);break;
case5:forward(18,18);break;
case6:forward(15,15);break;
case7:forward(12,12);break;
case8:forward(10,10);break;
case9:forward(5,5);break;
case10:forward(2,2);break;
case11:forward_s(1,1);break;
default:break;
}
if(chdata[3]<=0x05||chdata[4]<=0x05) break;
}
}
voidStage3(void)//阶段3灭火前最后调节
{
unsignedinti;
flag=3;
for(i=0;i<20;i++)
{
if(chdata[3]<=0x05&&chdata[4]<=0x05&&abs(chdata[3]-chdata[4])<=2&&abs(chdata[2]-chdata[5])<=8)break;
if(chdata[3]<=0x05&&chdata[4]>0x05){turn_left(1);delay_ms(10);}
if(chdata[3]>0x05&&chdata[4]<=0x05){turn_right(1);delay_ms(10);}
}
stop();
}
//***************灭火*********************//
voidfire()
{
unsignedintn;
while(chdata[3]<=0x7f||chdata[4]<=0x7f)
{
if(n>=1)revison();//如果1次吹不灭则调用修正
Set_IOA_Data(15,1);//灭火电扇启动
delay_s(8);
Set_IOA_Data(15,0);//灭火电扇关闭
delay_s(3);
n++;
}
sucessfire=1; //1灭火成功
workstate=0;
flag=0;
Set_IOB_Data(0,0);//启动蜂鸣器报警
xp=255;
mm=0;
min=255;
}
//***************修正子程序*********************//
voidrevison()
{
forward(8,8);
adjust();

}
//***************调节*********************//
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]&&abs(chdata[3]-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]) {turn_left(21);delay_ms(20);}
if(min==chdata[2]) {turn_left(5);delay_ms(20);}
if(min==chdata[3]&&abs(chdata[3]-chdata[4])>20){turn_left(1);delay_ms(20);}
if(min==chdata[4]&&abs(chdata[3]-chdata[4])>20){turn_right(1);delay_ms(20);}
if(min==chdata[3]&&abs(chdata[3]-chdata[4])<=20)break;
if(min==chdata[4]&&abs(chdata[3]-chdata[4])<=20)break;

if(min==chdata[5]){turn_right(5);delay_ms(20);}
if(min==chdata[6]){turn_right(30);delay_ms(20);}
}
}
//***************迈进*********************//
voidforward(unsignedintm,unsignedintn)
{
pulse_x=0;
pulse_y=0;
while(1)
{
if(pulse_x<=m&&pulse_y<=n){PWM_left(17,217);PWM_right(13,213);}
if(pulse_x>m&&pulse_y<=n){PWM_right(13,213);}
if(pulse_x<=m&&pulse_y>n){PWM_left(17,217);}
if(pulse_x>m&&pulse_y>n){pulse_x=0;pulse_y=0;break;}
if(workstate==1&&flag==0){pulse_x=0;pulse_y=0;break;}
}
}
voidforward_s(unsignedintm,unsignedintn)
{
pulse_x=0;
pulse_y=0;
while(1)
{
if(pulse_x<=m&&pulse_y<=n){PWM_left(17,417);PWM_right(13,413);}
if(pulse_x>m&&pulse_y<=n){PWM_right(13,413);}
if(pulse_x<=m&&pulse_y>n){PWM_left(17,417);}
if(pulse_x>m&&pulse_y>n){pulse_x=0;pulse_y=0;break;}
if(workstate==1&&flag==0){pulse_x=0;pulse_y=0;break;}
}
}
voidforward_s2(unsignedintm,unsignedintn)
{
pulse_x=0;
pulse_y=0;
while(1)
{
if(pulse_x<=m&&pulse_y<=n){PWM_left(15,215);PWM_right(15,215);}
if(pulse_x>m&&pulse_y<=n){PWM_right(15,215);}
if(pulse_x<=m&&pulse_y>n){PWM_left(15,215);}
if(pulse_x>m&&pulse_y>n){pulse_x=0;pulse_y=0;break;}
if(workstate==1&&flag==0){pulse_x=0;pulse_y=0;break;}
}
}
//***************后退*********************//
voidbackward(unsignedintm,unsignedintn)
{
pulse_x=0;
pulse_y=0;
while(1)
{
if(pulse_x<=m&&pulse_y<=n){PWM_left(13,213);PWM_right(17,217);}
if(pulse_x>m&&pulse_y<=n){PWM_right(17,217);}
if(pulse_x<=m&&pulse_y>n){PWM_left(13,213);}
if(pulse_x>m&&pulse_y>n){pulse_x=0;pulse_y=0;break;}
if(workstate==1&&flag==0){pulse_x=0;pulse_y=0;break;}
}
}
//***************左转*********************//
voidturn_left(unsignedintm)
{
pulse_x=0;
pulse_y=0;
while(1)