智能灭火机器人设计第程序.docx
《智能灭火机器人设计第程序.docx》由会员分享，可在线阅读，更多相关《智能灭火机器人设计第程序.docx（28页珍藏版）》请在冰点文库上搜索。
智能灭火机器人设计第程序
附录1main.c
#include
"wei.h"
#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
0x2000
//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
unsigned
intmm,min,cny,Delaynumber,xp=255,flag,dd;
unsigned
intleft,right;
unsigned
intpulse_x,pulse_y;
unsigned
intpaixu[7];//
排序
unsigned
intchdata[7];//
火焰传感器检测通道
unsigned
intworkstate=0;
//
进入检测范围
unsigned
intsucessfire=0;
//灭火成功标记
void
PWM_left（unsignedinthigh_time,unsignedintcyclical_time）;
void
PWM_right（unsignedinthigh_time,unsignedintcyclical_time）;
void
forward（unsignedintm,unsignedintn）;
void
forward_s（unsignedintm,unsignedintn）;
void
forward_s2（unsignedintm,unsignedintn）;
void
backward（unsignedintm,unsignedintn）;
void
turn_left（unsignedintm）;
void
turn_right（unsignedintm）;
void
stop（void）;
voidget_AD（void）;//AD转换
voidsystem_init（void）;//系统初始化
voiddelay_ms（unsignedintt）;//ms级延时程序
voiddelay_s（unsignedintt）;//s级延时程序
void
adjust（void）;//
调整
void
adjust_s（void）;//
粗调
void
seekfire（void）;//
寻找火源
void
fire（void）;//
灭火
void
hf（void）;
intabs（intm）;
voidbizhan（void）;//
避障
voidbiya（void）;//
避崖
void
Stage0（void）;//
阶段0
void
Stage1（void）;//
阶段1
void
Stage2（void）;//
阶段2
void
Stage3（void）;//
阶段3
void
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控制避崖，
__asm（"INTIRQ,FIQ"）;
//2HZ
用来控制
AD采集
4HZ
用来避
障
}
//***************寻找火源*********************//
voidseekfire（void）
{
Stage0（）;//
Stage1（）;//
阶段
阶段
0
1
按一定的路径走
有发现火源，做初步调整
Stage2（）;//
Stage3（）;//
阶段
阶段
2
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]）
if（min==chdata[2]）
{turn_left（21）;delay_ms（20）;}
{turn_left（5）;delay_ms（20）;}
if（min==chdata[3]&&abs（chdata[3]-chdata[4]）>20
delay_ms（20）;}
if（min==chdata[4]&&abs（chdata[3]-chdata[4]）>20
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;
）{turn_left
（1）;
）{turn_right
（1）;
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）
{
if（pulse_x<=m&&pulse_y<=m）{PWM_left（13,413）;PWM_right（13,413）;}if（pulse_x>m&&pulse_y<=m）{PWM_right（13,413）;}if（pulse_x<=m&&pulse_y>m）{PWM_left（13,413）;}
if（pulse_x>m&&pulse_y>m）{pulse_x=0;pulse_y=0;break;}if（workstate==1&&flag==0）{pulse_x=0;pulse_y=0;break;}
}
}
//***************右转*********************//
voidturn_right（unsignedintm）
{
pulse_x=0;
pulse_y=0;
while
（1）
{
if（pulse_x<=m&&pulse_y<=m）{PWM_left（17,417）;PWM_right（17,417）;}if（pulse_x>m&&pulse_y<=m）{PWM_right（17,417）;}
if（pulse_x<=m&&pulse_y>m）{PWM_left（17,417）;}
if（pulse_x>m&&pulse_y>m）{pulse_x=0;pulse_y=0;break;}if（workstate==1&&flag==0）{pulse_x=0;pulse_y=0;break;}}
}
//***************
voidstop（void）
{
停止*********************//
*P_IOA_Data&=~（bit13+bit14）;//
将IOA13、IOA14清
0
pulse_x=0;
pulse_y=0;
}
//***************控制小车左轮*********************//
voidPWM_left（unsignedinthigh_time,unsignedintcyclical_time）
{
if（left
<=high_time）
/*high_time
为脉冲的高电平时间，
cyclical_time
为脉冲周期时间
*/
Set_IOA_Data（14,1）;//
将IOA14置
1
else
Set_IOA_Data（14,0）;//
将IOA14清
0
if（left>cyclical_time）
{
left=0;
pulse_x++;
}
*P_Watchdog_Clear=0x0001;
}
//***************控制小车右轮*********************//
voidPWM_right（unsignedinthigh_time,unsignedintcyclical_time）
{
if（right<=high_time）
Set_IOA_Data（13,1）;//
将IOA13置
1
else
Set_IOA_Data（13,0）;//
将IOA13清
0
if（right>cyclical_time）
{
right=0;
pulse_y++;
}
*P_Watchdog_Clear=0x0001;
}
//*************
升级会员 