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

1、1附录 1 main.c#include “wei.h“#define P_IOA_Data (volatile unsigned int *)0x7000#define P_IOA_Dir (volatile unsigned int *)0x7002#define P_IOA_Attrib (volatile unsigned int *)0x7003#define P_IOB_Data (volatile unsigned int *)0x7005#define P_IOB_Dir (volatile unsigned int *)0x7007#define P_IOB_Attrib (

2、volatile unsigned int *)0x7008#define P_TimerB_Data (volatile unsigned int *)0x700C#define P_TimerB_Ctrl (volatile unsigned int *)0x700D#define P_INT_Ctrl (volatile unsigned int *)0x7010#define P_INT_Clear (volatile unsigned int *)0x7011#define P_ADC (volatile unsigned int *)0x7014#define P_ADC_Ctrl

3、 (volatile unsigned int *)0x7015#define P_ADC_MUX_Ctrl (volatile unsigned int *)0x702B#define P_ADC_MUX_Data (volatile unsigned int *)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_

4、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 (volatile unsigned int *)0x7012unsigned int m

5、m,min,cny,Delaynumber,xp=255,flag,dd; unsigned int left,right; unsigned int pulse_x,pulse_y;unsigned int paixu7; /排序unsigned int chdata7; /火焰传感器检测通道unsigned int workstate=0; /进入检测范围unsigned int sucessfire=0; /灭火成功标记void PWM_left(unsigned int high_time,unsigned int cyclical_time);void PWM_right(unsig

6、ned int high_time,unsigned int cyclical_time);void forward(unsigned int m,unsigned int n) ;void forward_s(unsigned int m,unsigned int n);void forward_s2(unsigned int m,unsigned int n); void backward(unsigned int m,unsigned int n);void turn_left (unsigned int m);void turn_right(unsigned int m);void s

7、top(void);void get_AD(void); /AD转换2void system_init(void); /系统初始化void delay_ms(unsigned int t ); /ms级延时程序void delay_s(unsigned int t ); /s级延时程序void adjust(void); /调整void adjust_s(void); /粗调void seekfire(void); /寻找火源void fire(void); /灭火void hf(void);int abs(int m);void bizhan(void ) ; /避障void biya(vo

8、id ); /避崖void Stage0(void);/阶段 0void Stage1(void); /阶段 1void Stage2(void); /阶段 2void Stage3(void); /阶段 3void revison();/修正函数/+主函数+/int main(void) system_init();delay_s(3); while(1)seekfire();fire(); delay_s(8);/*系统初始化*/void system_init(void) _asm(“INT OFF“);*P_IOA_Dir=0xE700; /*P_IOA_Attrib=0xE7FF;*

9、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(“INT IRQ,FIQ“); /2HZ用来控制 AD采集 4HZ用来避障3/*寻

10、找火源*/void seekfire(void)Stage0(); /阶段 0 按一定的路径走Stage1(); /阶段 1 有发现火源，做初步调整Stage2(); /阶段 2 接近火源，边走边调整Stage3(); /阶段 3 到达火源前面，做最后的调整void Stage0(void) /阶段 0 flag=0;while(1)turn_left(120); /转 360度 扫描有没有火源 forward(250,250);if(workstate=1 void Stage1(void) /阶段 1 flag=1;Set_IOB_Data(0,1); / 开启蜂鸣器报警adjust_s(

11、); switch(mm) case 1: forward(60,60); break;case 2: forward(55,55); break;case 3: forward(50,50); break;case 4: forward(45,45); break;case 5: forward(35,35); break;case 6: forward(30,30); break;case 7: forward(20,20); break;case 8: forward(10,10); break; case 9: forward(5,5); break; case 10: forward

12、(2,2); break; case 11: forward_s(1,1); break; default: break; void Stage2(void) /阶段 2 flag=2;while(1)4adjust(); switch(mm)case 1: forward(40,40); break;case 2: forward(30,30); break;case 3: forward(25,25); break;case 4: forward(20,20); break;case 5: forward(18,18); break;case 6: forward(15,15); brea

13、k;case 7: forward(12,12); break;case 8: forward(10,10); break; case 9: forward(5,5); break; case 10: forward(2,2); break; case 11: forward_s(1,1); break; default: break; if(chdata30x05 ) turn_left(1) ; delay_ms(10); if(chdata30x05 /如果 1次吹不灭则调用修正Set_IOA_Data(15,1); / 灭火风扇开启delay_s(8);Set_IOA_Data(15,

14、0); / 灭火风扇关闭delay_s(3);n+;5 sucessfire=1; / 1 灭火成功workstate=0; flag=0;Set_IOB_Data(0,0); / 开启蜂鸣器报警xp=255;mm=0;min=255;/*修正子程序*/void revison()forward(8,8); adjust(); /*调整*/void adjust_s(void)if(min=chdata1) turn_left(21);if(min=chdata2) turn_left(12);if(min=chdata3 if(min=chdata4 if(min=chdata5) turn

15、_right(15);if(min=chdata6) turn_right(30);void adjust(void) while(min20 )turn_left(1); delay_ms(20); if( min=chdata4 delay_ms(20); if( min=chdata3 if(pulse_xm pulse_y=0 ; break;if(workstate=1 pulse_y=0 ; break; void forward_s(unsigned int m,unsigned int n) pulse_x=0; pulse_y=0;while(1)if(pulse_xm if

16、(pulse_xm pulse_y=0 ; break;if(workstate=1 pulse_y=0 ; break; void forward_s2(unsigned int m,unsigned int n) pulse_x=0; pulse_y=0;while(1)if(pulse_xm if(pulse_xm pulse_y=0 ; break; if(workstate=1 pulse_y=0 ; break; /*后退*/void backward(unsigned int m,unsigned int n) 7pulse_x=0; pulse_y=0;while(1)if(p

17、ulse_xm if(pulse_xm pulse_y=0 ; break; if(workstate=1 pulse_y=0 ; break; /*左转*/void turn_left(unsigned int m) pulse_x=0; pulse_y=0;while(1)if(pulse_xm if(pulse_xm pulse_y=0 ; break; if(workstate=1 pulse_y=0 ; break;/*右转*/void turn_right(unsigned int m) pulse_x=0; pulse_y=0;while(1)if(pulse_xm if(pul

18、se_xm pulse_y=0 ; break; if(workstate=1 pulse_y=0 ; break;/*停止*/void stop(void)*P_IOA_Data /将 IOA13、IOA14 清 08pulse_x=0;pulse_y=0;/*控制小车左轮*/void PWM_left(unsigned int high_time,unsigned int cyclical_time) if(left cyclical_time)left=0;pulse_x +;*P_Watchdog_Clear=0x0001;/*控制小车右轮*/void PWM_right(unsign

19、ed int high_time,unsigned int cyclical_time)if(right cyclical_time)right=0;pulse_y +;*P_Watchdog_Clear=0x0001;/*IRQ4中断*/void IRQ4(void) _attribute_ (ISR);void IRQ4(void)if(*P_INT_Ctrl dd+;if(dd=100) /100ms检测一次dd=0;9biya(); if(*P_INT_Ctrl else *P_INT_Clear |=C_IRQ4_4KHz; /*IRQ5中断*/void IRQ5(void) _at

20、tribute_ (ISR);void IRQ5(void)if(*P_INT_Ctrl bizhan(); /避障 else /IRQ5_2HZ*P_INT_Clear |=C_IRQ5_2Hz; hf(); /比较检测到 AD的最小值if(xp8 ; /右移 8位，只取 8高位数据 aki=AD_Data; for(i=0;iakj)temp=aki;aki=akj;akj=temp;sum=0;for(k=5;kpaixuj)temp=paixui;paixui=paixuj;paixuj=temp; min=paixu1; /min 为单前这次检测到的最低值xp =xpmin ? xp:min ; /xp 为检测到的历史最低值if(0xC9xp if(0xA0xp if(0x90xp if(0x7Fxp if(0x67xp if(0x50xp if(0x3Fxp if(0x0Fxp if(0x0Cxp if(0x08xp if(0x00xp /*避崖程序*/