智能滅火機器人設(shè)計第程序

1、附錄1 main.c#include "wei.h"#defineP_IOA_Data(volatile unsigned int *)0x7000#defineP_IOA_Dir(volatile unsigned int *)0x7002#defineP_IOA_Attrib(volatile unsigned int *)0x7003#defineP_IOB_Data(volatile unsigned int *)0x7005#defineP_IOB_Dir(volatile unsigned int *)0x7007#defineP_IOB_Attrib(vola

2、tile unsigned int *)0x7008#defineP_TimerB_Data(volatile unsigned int *)0x700C#defineP_TimerB_Ctrl(volatile unsigned int *)0x700D#defineP_INT_Ctrl(volatile unsigned int *)0x7010#defineP_INT_Clear(volatile unsigned int *)0x7011#defineP_ADC(volatile unsigned int *)0x7014#defineP_ADC_Ctrl(volatile unsig

3、ned int *)0x7015#defineP_ADC_MUX_Ctrl(volatile unsigned int *)0x702B#defineP_ADC_MUX_Data(volatile unsigned int *)0x702C#defineC_FIQ_PWM0x8000/ P_INT_Ctrl#defineC_FIQ_TMA0x2000/ P_INT_Ctrl#defineC_FIQ_TMB0x0800/ P_INT_Ctrl#defineC_IRQ4_1KHz0x0010/ P_INT_Ctrl#defineC_IRQ4_2KHz0x0020/ P_INT_Ctrl#defin

4、eC_IRQ4_4KHz0x0040/ P_INT_Ctrl#defineC_IRQ5_2Hz0x0004/ P_INT_Ctrl#defineC_IRQ5_4Hz0x0008/ P_INT_Ctrl#defineC_TMB_32KHz0x0002/ P_TimerB_Ctrl#defineC_TMB_PWM_OFF0x0000/ P_TimerB_Ctrl#defineP_Watchdog_Clear(volatile unsigned int *)0x7012unsigned int mm,min,cny,Delaynumber,xp=255,flag,dd; unsigned int l

5、eft,right; unsigned int pulse_x,pulse_y;unsigned int paixu7; /排序unsigned int chdata7; /火焰?zhèn)鞲衅鳈z測通道unsigned int workstate=0;/進入檢測范圍unsigned int sucessfire=0;/滅火成功標(biāo)記void PWM_left(unsigned int high_time,unsigned int cyclical_time);void PWM_right(unsigned int high_time,unsigned int cyclical_time);void for

6、ward(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 stop(void);void get_AD(void); /AD轉(zhuǎn)換void system_init(voi

7、d); /系統(tǒng)初始化void delay_ms(unsigned int t ); /ms級延時程序void delay_s(unsigned int t ); /s級延時程序void adjust(void); /調(diào)整void adjust_s(void); /粗調(diào)void seekfire(void); /尋找火源void fire(void); /滅火void hf(void);int abs(int m);void bizhan(void ) ; /避障void biya(void ); /避崖void Stage0(void);/階段0void Stage1(void); /階段1v

8、oid Stage2(void); /階段2void Stage3(void); /階段3void revison();/修正函數(shù)/+主函數(shù)+/int main(void) system_init(); delay_s(3); while(1) seekfire(); fire(); delay_s(8); /*系統(tǒng)初始化*/void system_init(void) _asm("INT OFF"); *P_IOA_Dir=0xE700; / *P_IOA_Attrib=0xE7FF; *P_IOA_Data=0x1800; *P_IOB_Dir = 0x0FFF; *P

9、_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用來避障/*尋找火源*/void seekfire(void)Stag

10、e0(); /階段0 按一定的路徑走Stage1(); /階段1 有發(fā)現(xiàn)火源，做初步調(diào)整Stage2(); /階段2 接近火源，邊走邊調(diào)整Stage3(); /階段3 到達火源前面，做最后的調(diào)整 void Stage0(void) /階段0 flag=0;while(1) turn_left(120); /轉(zhuǎn)360度 掃描有沒有火源 forward(250,250); if(workstate=1 && flag=0) break; void Stage1(void) /階段1 flag=1;Set_IOB_Data(0,1); / 開啟蜂鳴器報警adjust_s(); swi

11、tch(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: forwar

12、d(2,2); break; case 11: forward_s(1,1); break; default: break; void Stage2(void) /階段2 flag=2; while(1) adjust(); 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

13、); break; 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(chdata3<=0x05 | chdata4<= 0x05)break; void Stage3(void) /階段3 滅火前最后調(diào)整 unsigned int i; flag=3;for(i=0;i<20;i+)

14、if(chdata3<=0x05 && chdata4<=0x05 && abs(chdata3-chdata4)<=2 &&abs(chdata2-chdata5)<=8 ) break;if(chdata3<=0x05 && chdata4 >0x05 ) turn_left(1) ; delay_ms(10); if(chdata3>0x05 && chdata4 <=0x05) turn_right(1); delay_ms(10); stop(); /*滅火

15、*/void fire() unsigned int n; while(chdata3<=0x7f | chdata4<=0x7f) if(n>=1)revison();/如果1次吹不滅則調(diào)用修正 Set_IOA_Data(15,1); / 滅火風(fēng)扇開啟 delay_s(8); Set_IOA_Data(15,0); / 滅火風(fēng)扇關(guān)閉 delay_s(3); n+; sucessfire=1;/ 1 滅火成功 workstate=0; flag=0; Set_IOB_Data(0,0); / 開啟蜂鳴器報警 xp=255; mm=0; min=255;/*修正子程序*/voi

16、d revison() forward(8,8); adjust(); /*調(diào)整*/void adjust_s(void) if(min=chdata1)turn_left(21); if(min=chdata2)turn_left(12); if(min=chdata3 && abs(chdata3-chdata4)>30 )turn_left(5) ; if(min=chdata4 && abs(chdata3-chdata4)>30 )turn_right(5); if(min=chdata5) turn_right(15); if(min=c

17、hdata6) turn_right(30); void adjust(void) while(min<=0xc9) if(min=chdata1)turn_left(21); delay_ms(20); if(min=chdata2)turn_left(5); delay_ms(20); if( min=chdata3 && abs(chdata3-chdata4)>20 )turn_left(1); delay_ms(20); if( min=chdata4 && abs(chdata3-chdata4)>20 )turn_right(1)

18、; delay_ms(20); if( min=chdata3 && abs(chdata3-chdata4)<=20 ) break; if( min=chdata4 && abs(chdata3-chdata4)<=20 ) break; if(min=chdata5) turn_right(5); delay_ms(20); if(min=chdata6) turn_right(30); delay_ms(20); /*前進*/void forward(unsigned int m,unsigned int n) pulse_x=0; puls

19、e_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=

20、1 && flag=0) pulse_x=0;pulse_y=0 ; break; void forward_s(unsigned int m,unsigned int n) 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 && puls

21、e_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; void forward_s2(unsigned int m,unsigned int n) pulse_x=0; pulse_y=0; while(1) if(pulse_x<=m && pulse_y<=n) PWM_left(15,215);

22、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; /*后退*/void backward(unsigne

23、d int m,unsigned int n) 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) pul

24、se_x=0;pulse_y=0 ; break; if(workstate=1 && flag=0) pulse_x=0;pulse_y=0 ; break; /*左轉(zhuǎn)*/void turn_left(unsigned int m) 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

25、(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; /*右轉(zhuǎn)*/void turn_right(unsigned int m) pulse_x=0; pulse_y=0;while(1) if(pulse_x<=m && pulse_y<=

26、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; /*停止*

27、/void stop(void)*P_IOA_Data &=(bit13 + bit14); /將IOA13、IOA14清0 pulse_x=0; pulse_y=0;/*控制小車左輪*/void PWM_left(unsigned int high_time,unsigned int cyclical_time) if(left <= high_time) /*high_time為脈沖的高電平時間，cyclical_time為脈沖周期時間 */Set_IOA_Data(14,1); / 將IOA14置1else Set_IOA_Data(14,0); / 將IOA14清0if(

28、left > cyclical_time)left=0;pulse_x +; *P_Watchdog_Clear=0x0001;/*控制小車右輪*/void PWM_right(unsigned int high_time,unsigned int cyclical_time)if(right <= high_time) Set_IOA_Data(13,1); / 將IOA13置1else Set_IOA_Data(13,0); / 將IOA13清0if(right > cyclical_time)right=0;pulse_y +; *P_Watchdog_Clear=0x

29、0001;/*IRQ4中斷*/void IRQ4(void) _attribute_ (ISR);void IRQ4(void)if(*P_INT_Ctrl & C_IRQ4_1KHz) *P_INT_Clear |=C_IRQ4_1KHz ; dd+; if(dd>=100) /100ms檢測一次 dd=0; biya(); if(*P_INT_Ctrl & C_IRQ4_2KHz) *P_INT_Clear |=C_IRQ4_2KHz ;else *P_INT_Clear |=C_IRQ4_4KHz; /*IRQ5中斷*/void IRQ5(void) _attrib

30、ute_ (ISR);void IRQ5(void)if(*P_INT_Ctrl & C_IRQ5_4Hz) /IRQ5_4HZ *P_INT_Clear |=C_IRQ5_4Hz ; bizhan(); /避障 else /IRQ5_2HZ *P_INT_Clear |=C_IRQ5_2Hz; hf(); /比較檢測到AD的最小值 if(xp<=0xc8)workstate=1;/ AD值達到200以內(nèi)就認(rèn)為發(fā)現(xiàn)火源。workstate=1進入檢測范圍 /*A/D轉(zhuǎn)換*/void get_AD(void) unsigned int ak15; unsigned int i,j,

31、temp,k,sum; unsigned int Line,AD_Data; *P_ADC_Ctrl=0x01; /允許轉(zhuǎn)換 for(Line=1;Line<7;Line+) *P_ADC_MUX_Ctrl=Line;/選擇通道 AD_Data=*P_ADC_MUX_Data;/啟動轉(zhuǎn)換 for(i=0;i<15;i+) while(!(*P_ADC_MUX_Ctrl&0x8000); /沒有轉(zhuǎn)換完則繼續(xù)等待 AD_Data=*P_ADC_MUX_Data; /讀轉(zhuǎn)換后的數(shù)據(jù) AD_Data =AD_Data>>8 ; /右移8位，只取8高位數(shù)據(jù) aki=AD_

32、Data; for(i=0;i<15;i+) /從小到大排序 for(j=i+1;j<15;j+) if(aki>akj)temp=aki;aki=akj;akj=temp; sum=0; for(k=5;k<10;k+) sum=sum+akk; chdataLine =sum/5; *P_Watchdog_Clear=0x01; /*/ void hf(void) unsigned int i,j,temp; get_AD();for(i=1;i<=6;i+)paixui=chdatai;for(i=1;i<=6;i+) for(j=i+1;j<=

33、6;j+) if(paixui>paixuj)temp=paixui;paixui=paixuj;paixuj=temp; min=paixu1; /min 為單前這次檢測到的最低值xp =xp<min ? xp:min ; /xp 為檢測到的歷史最低值if(0xC9<xp && xp<=0xFF) mm=1; if(0xA0<xp && xp<=0xC9) mm=2; if(0x90<xp && xp<=0xA0) mm=3;if(0x7F<xp && xp<=0x90

34、) mm=4; if(0x67<xp && xp<=0x7F) mm=5;if(0x50<xp && xp<=0x67) mm=6; if(0x3F<xp && xp<=0x50) mm=7;if(0x0F<xp && xp<=0x3F) mm=8; if(0x0C<xp && xp<=0x0F) mm=9; if(0x08<xp && xp<=0x0C) mm=10; if(0x00<xp && xp&l

35、t;=0x08) mm=11;/*避崖程序*/ void biya(void ) if(!(*P_IOB_Data & 0x8000) /q1 1000 stop(); backward(20,20); turn_left(21); if(!(*P_IOB_Data & 0x4000) /q2 0100 stop(); backward(20,20); turn_left(21); if(!(*P_IOB_Data & 0xc000) /q12 1100 stop(); backward(20,20); turn_left(21); if(!(*P_IOB_Data &

36、amp; 0x2000) /h1 0010 stop(); forward(15,15); turn_left(10); if(!(*P_IOB_Data & 0x1000) /h2 0001 stop(); forward(15,15); turn_left(10); if(!(*P_IOB_Data & 0x3000) /h1 2 0011 stop(); forward(15,15); turn_left(10); else; /*避障程序*/ void bizhan(void ) if(Get_IOA_Data(12)=0 && Get_IOA_Data

37、(11) =1 ) /zuobingbackward(15,15);while(Get_IOA_Data(12)=0) turn_right(30); forward(20,20); turn_left(21); if(Get_IOA_Data(11)=0 && Get_IOA_Data(12) =1 ) /youbingbackward(15,15);while(Get_IOA_Data(11)=0) turn_left(21); forward(20,20); turn_right(30); if(Get_IOA_Data(11)=0 && Get_IOA_

38、Data(12) =0) backward(15,15); turn_left(21); forward(20,20); turn_right(30); /*延時子程序*/ void delay_s(unsigned int t ) / s級延時程序 unsigned int i; for(i=0;i<t;i+) delay_ms(1000); void delay_ms(unsigned int t) / ms級延時程序 Delaynumber=0; while(Delaynumber<=10*t)*P_Watchdog_Clear=0x01; /沒有延時完則繼續(xù)等待 附錄2 F

39、IQ.asm.DEFINE P_INT_Ctrl 0x7010 / Control port for interrupt source.DEFINE P_INT_Clear 0x7011 / Clear interrupt source.DEFINE C_FIQ_TMB 0x0800 / Timer B FIQ.DEFINE C_FIQ_TMA 0x2000 / Timer A FIQ.DEFINE C_FIQ_PWM 0x8000 / PWM FIQ.external _left,_right,_Delaynumber.PUBLIC _IRQ0,_IRQ1,_IRQ2,_IRQ3,_IRQ6

40、,_IRQ7,_BREAK.TEXT.PUBLIC _FIQ_FIQ:push r1,r5 to spr1 = C_FIQ_TMAtest r1,P_INT_Ctrljnz L_FIQ_TimerAr1 = C_FIQ_TMBtest r1,P_INT_Ctrljnz L_FIQ_TimerBL_FIQ_PWM:r1 = C_FIQ_PWMP_INT_Clear = r1pop r1,r5 from spretiL_FIQ_TimerA:P_INT_Clear = r1pop r1,r5 from spretiL_FIQ_TimerB:P_INT_Clear = r1 r2=_left r2

41、+=1 _left=r2 r3=_right r3 +=1 _right=r3 r4=_Delaynumber r4 +=1 _Delaynumber=r4 pop r1,r5 from spreti_IRQ0:reti_IRQ1: reti_IRQ2: reti_IRQ3: reti_IRQ6: reti_IRQ7: reti_BREAK: reti附錄3 wei.h#defineP_IOA_Data(volatile unsigned int *)0x7000#defineP_IOB_Data(volatile unsigned int *)0x7005#ifndef_WEI_h_#def

42、ine_WEI_h_/write your header here#define bit0 0x0001#define bit1 0x0002#define bit2 0x0004#define bit3 0x0008#define bit4 0x0010#define bit5 0x0020#define bit6 0x0040#define bit7 0x0080#define bit8 0x0100#define bit9 0x0200#define bit10 0x0400#define bit11 0x0800#define bit12 0x1000#define bit13 0x2

43、000#define bit14 0x4000#define bit15 0x8000/*單獨設(shè)置I/O的Data信息*voidSet_IOA_Data(unsigned int x,unsigned int y)switch(x)case 0:if(y=1)*P_IOA_Data|=0x0001;if(y=0)*P_IOA_Data&=0xfffe;break;case 1:if(y=1)*P_IOA_Data|=0x0002;if(y=0)*P_IOA_Data&=0xfffd;break;case 2: if(y=1)*P_IOA_Data|=0x0004; if(y=0)*P_IOA_Data&=0xfffb; break;case 3: if(y=1)*P_IOA_Data|=0x0008; if(y=0)*P_IOA_Data&=0xfff7; break;case 4:if(y=1)*P_IOA_Data|=0x0010;if(y=0)*P_IOA_Data&=0xffef;break;case 5:if(y=1)*P_IOA_Data|=0x0020;if(y=0)*P_IOA_Data&=0xffdf;break;case 6:if(y=1)*P_IOA_Data|=0