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

附录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(volatile 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 unsigned 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#definec_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 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(unsigned 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 stop(void);void get_ad(void); /ad转换void 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(void ); /避崖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; *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用来避障/*寻找火源*/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 & flag=0) break; void stage1(void) /阶段1 flag=1;set_iob_data(0,1); / 开启蜂鸣器报警adjust_s(); 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(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); 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 灭火前最后调整 unsigned int i; flag=3;for(i=0;i20;i+) if(chdata3=0x05 & chdata4=0x05 & abs(chdata3-chdata4)=2 &abs(chdata2-chdata5)=8 ) break;if(chdata30x05 ) turn_left(1) ; delay_ms(10); if(chdata30x05 & chdata4 =0x05) turn_right(1); delay_ms(10); stop(); /*灭火*/void fire() unsigned int n; while(chdata3=0x7f | chdata4=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;/*修正子程序*/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 & 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=chdata6) turn_right(30); void adjust(void) while(min20 )turn_left(1); delay_ms(20); if( min=chdata4 & abs(chdata3-chdata4)20 )turn_right(1); 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; pulse_y=0; while(1) if(pulse_x=m & pulse_ym & pulse_y=n) pwm_right(13,213); if(pulse_xn) pwm_left(17,217); if(pulse_xm & pulse_yn) pulse_x=0;pulse_y=0 ; break; if(workstate=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_ym & pulse_y=n) pwm_right(13,413); if(pulse_xn) pwm_left(17,417); if(pulse_xm & pulse_yn) 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_ym & pulse_y=n) pwm_right(15,215); if(pulse_xn) pwm_left(15,215); if(pulse_xm & pulse_yn) pulse_x=0;pulse_y=0 ; break; if(workstate=1 & flag=0) pulse_x=0;pulse_y=0 ; break; /*后退*/void backward(unsigned int m,unsigned int n) pulse_x=0; pulse_y=0; while(1) if(pulse_x=m & pulse_ym & pulse_y=n) pwm_right(17,217); if(pulse_xn) pwm_left(13,213); if(pulse_xm & pulse_yn) pulse_x=0;pulse_y=0 ; break; if(workstate=1 & flag=0) pulse_x=0;pulse_y=0 ; break; /*左转*/void turn_left(unsigned int m) pulse_x=0; pulse_y=0; while(1) if(pulse_x=m & pulse_ym & pulse_y=m) pwm_right(13,413); if(pulse_xm) pwm_left(13,413) ; if(pulse_xm & pulse_ym) pulse_x=0;pulse_y=0 ; break; if(workstate=1 & flag=0) pulse_x=0;pulse_y=0 ; break; /*右转*/void turn_right(unsigned int m) pulse_x=0; pulse_y=0;while(1) if(pulse_x=m & pulse_ym & pulse_y=m) pwm_right(17,417); if(pulse_xm) pwm_left(17,417) ; if(pulse_xm & pulse_ym) pulse_x=0;pulse_y=0 ; break; if(workstate=1 & flag=0) pulse_x=0;pulse_y=0 ; break; /*停止*/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 cyclical_time)left=0;pulse_x +; *p_watchdog_clear=0x0001;/*控制小车右轮*/void pwm_right(unsigned 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 & 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) _attribute_ (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以内就认为发现火源。workstate=1进入检测范围 /*a/d转换*/void get_ad(void) unsigned int ak15; unsigned int i,j,temp,k,sum; unsigned int line,ad_data; *p_adc_ctrl=0x01; /允许转换 for(line=1;line7;line+) *p_adc_mux_ctrl=line;/选择通道 ad_data=*p_adc_mux_data;/启动转换 for(i=0;i8 ; /右移8位，只取8高位数据 aki=ad_data; for(i=0;i15;i+) /从小到大排序 for(j=i+1;jakj)temp=aki;aki=akj;akj=temp; sum=0; for(k=5;k10;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;jpaixuj)temp=paixui;paixui=paixuj;paixuj=temp; min=paixu1; /min 为单前这次检测到的最低值xp =xpmin ? xp:min ; /xp 为检测到的历史最低值if(0xc9xp & xp=0xff) mm=1; if(0xa0xp & xp=0xc9) mm=2; if(0x90xp & xp=0xa0) mm=3;if(0x7fxp & xp=0x90) mm=4; if(0x67xp & xp=0x7f) mm=5;if(0x50xp & xp=0x67) mm=6; if(0x3fxp & xp=0x50) mm=7;if(0x0fxp & xp=0x3f) mm=8; if(0x0cxp & xp=0x0f) mm=9; if(0x08xp & xp=0x0c) mm=10; if(0x00xp & xp=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 & 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(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_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;it;i+) delay_ms(1000); void delay_ms(unsigned int t) / ms级延时程序 delaynumber=0; while(delaynumber=10*t)*p_watchdog_clear=0x01; /没有延时完则继续等待 附录2 fiq.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,_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 +=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_#define_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 0x2000#define bit14 0x4000#define bit15 0x8000/*单独设置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|=0x0040;if(y=0)*p_ioa_data&=0xffbf;break;case 7:if(y=1)*p_ioa_data|=0x0080;if(y=0)*p_ioa_data&=0xff7f;break;case 8:if(y=1)