; ;sumo-robot control program for ULTAKA-ZERO ; include "P16F84.INC" ; ;***** VARIABLE DEFINITIONS w_temp EQU 0x0C ; variable used for context saving status_temp EQU 0x0D ; variable used for context saving CNT1 EQU 0x0e CNT2 EQU 0x0f CNT3 EQU 0x10 CNT0 equ 0x11 C4021B1 equ 0x12 C4021B2 equ 0x13 HC151 equ 0x14 WORK equ 0x15 FR equ 0x16 FL equ 0x17 BR equ 0x18 BL equ 0x19 ; ORG 0x00 goto main ORG 0x004 ; interrupt vector location movwf w_temp ; save off current W register contents movf STATUS,w ; move status register into W register movwf status_temp ; save off contents of STATUS register ; isr code can go here or be located as a call subroutine elsewhere movf status_temp,w ; retrieve copy of STATUS register movwf STATUS ; restore pre-isr STATUS register contents swapf w_temp,f swapf w_temp,w ; restore pre-isr W register contents retfie ; return from interrupt ; main banksel trisa ;SELECT bank 1 MOVLW 0xF0 ;SET INPUT/OUTPUT MOVWF trisa ;SET PORT a MOVLW 0x30 ;SET INPUT/OUTPUT MOVWF trisb ;SET PORT b bcf option_reg,not_rbpu ;enable port b pull up banksel porta ;SELECT bank 0 clrf porta clrf portb call in_C4021B ;dammy call in_C4021B ;test mode check btfss C4021B1,7 ;start sw on ? goto test ;on test ;----------------------------------------------------- call push_wait ;start sw on ? call wait5sec swapf C4021B1,W ;case mode0-mode7 andlw 0x07 addwf PCL,f goto test goto mode6 goto mode5 goto mode4 goto mode3 goto mode2 goto mode1 goto mode0 ;----------------------------- test call in_C4021B swapf C4021B1,W ;case test0-test7 andlw 0x07 addwf PCL,f goto test7 goto test6 goto test5 goto test4 goto test3 goto test2 goto test1 goto test0 test0 movf C4021B1,W ;white line sensor movwf portb goto test test1 swapf C4021B2,W ; movwf portb goto test test2 movf C4021B2,W ; movwf portb goto test test3 call push_wait call in_hc151 swapf HC151,W ; movwf portb goto test test4 call push_wait call in_hc151 movf HC151,W ; movwf portb goto test test5 movlw d'5' ;0.5sec call TIMW incf portb,F goto test test6 swapf C4021B1,W ; movwf portb goto test test7 call wait5sec goto test ;----------------------------------------------- ;sw push wait push_wait call in_C4021B btfsc C4021B1,7 ;start sw push ? goto push_wait ;off wait return ;on return ;----------------------------------------------- ;fight routine ;----------------------------- mode0 call front10w goto mode0 ;----------------------------- mode1 call front50w ; call front50w call front50w call front50w call right30w call right30w goto mode1 ;----------------------------- mode2 call front50w ; call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call left30w call left30w goto mode2 ;---------------------------- mode3 call right30w call right30w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w goto mode3 ;---------------------------- mode4 call left30w call left30w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w goto mode4 ;---------------------------- mode5 call front50w ; call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call left30w call left30w goto mode5 ;--------------------------- mode6 call front50w ; call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call left30w call left30w goto mode6 ;-------------------------- mode7 call front50w ; call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call front50w call left30w call left30w goto mode7 ;---------------------------------------------------- ;enemy follow follow return ;---------------------------------------------------- ;white line sensor white call in_C4021B btfsc C4021B1,0 ;front right sensor on ? goto fr_on btfsc C4021B1,1 ;front left sensor on ? goto fl_on btfsc C4021B1,2 ;back right sensor on ? goto br_on btfsc C4021B1,3 ;back left sensor on ? goto bl_on return fr_on bsf portb,0 call back10 call back10 call back10 call back10 call back10 call left10 call left10 call left10 call left10 call left10 call left10 call front10 call front10 call front10 call front10 call front10 bcf portb,0 return fl_on bsf portb,1 call back10 call back10 call back10 call back10 call back10 call right10 call right10 call right10 call right10 call right10 call right10 call front10 call front10 call front10 call front10 call front10 bcf portb,1 return br_on bsf portb,0 bsf portb,2 call front10 call front10 call front10 call front10 call front10 call left10 call left10 call left10 call front10 call front10 call front10 call front10 call front10 bcf portb,0 bcf portb,2 return bl_on bsf portb,1 bsf portb,2 call front10 call front10 call front10 call front10 call front10 call right10 call right10 call right10 call front10 call front10 call front10 call front10 call front10 bcf portb,1 bcf portb,2 return ;----------------------------------------------------- ;4021B data input in_C4021B clrf C4021B1 clrf C4021B2 movlw 8 movwf CNT0 bsf portb,6 ;P/S=1 call delay bcf portb,6 ;P/S=0 lop4 rlf C4021B1,f rlf C4021B2,f call delay btfsc portb,4 ;input 4021B1 Q8 bsf C4021B1,0 btfsc portb,5 ;input 4021B2 Q8 bsf C4021B2,0 bcf portb,7 ;CLK=0 call delay bsf portb,7 ;CLK=1 sampling decfsz CNT0 goto lop4 return ;----------------------------------------------------- ;HC151 data input in_hc151 clrf HC151 movlw 8 movwf CNT0 movwf WORK hc151a decf WORK,F rlf HC151,F movlw 0xF8 andwf portb,W iorwf WORK,W movwf portb nop nop btfsc porta,4 ;input 74HC151 Y bsf HC151,0 decfsz CNT0 goto hc151a return ;------------------------------------------------------ ;motion control front10 call go_ahead MOVLW d'4' ;0.4sec 1cm CALL TIMW return back10 call go_back MOVLW d'4' ;0.4sec 1cm CALL TIMW return right10 call right_turn MOVLW d'6' ;0.6sec 10deg CALL TIMW return left10 call left_turn MOVLW d'6' ;0.6sec 10deg CALL TIMW return ;------------------------------------------------ front10w call white call go_ahead MOVLW d'4' ;0.4sec 1cm CALL TIMW return back10w call white call go_back MOVLW d'4' ;0.4sec 1cm CALL TIMW return right10w call white call right_turn MOVLW d'6' ;0.6sec 10deg CALL TIMW return left10w call white call left_turn MOVLW d'6' ;0.6sec 10deg CALL TIMW return ;------------------------------------------------ front50w call front10w call front10w call front10w call front10w call front10w return back50w call back10w call back10w call back10w call back10w call back10w return right30w call right10w call right10w call right10w return left30w call left10w call left10w call left10w return ;------------------------------------------------------ ;motor control go_ahead BSF PORTA,0 ;OUTPUT BIT0=H go ahead BCF PORTA,1 ;OUTPUT BIT1=L BSF PORTA,2 ;OUTPUT BIT2=H BCF PORTA,3 ;OUTPUT BIT3=L return go_back BCF PORTA,0 ;OUTPUT BIT0=L go back BSF PORTA,1 ;OUTPUT BIT1=H BCF PORTA,2 ;OUTPUT BIT2=L BSF PORTA,3 ;OUTPUT BIT3=H return right_turn BSF PORTA,0 ;OUTPUT BIT0=H right turn BCF PORTA,1 ;OUTPUT BIT1=L BCF PORTA,2 ;OUTPUT BIT2=L BSF PORTA,3 ;OUTPUT BIT3=H return left_turn BCF PORTA,0 ;OUTPUT BIT0=L left turn BSF PORTA,1 ;OUTPUT BIT1=H BSF PORTA,2 ;OUTPUT BIT2=H BCF PORTA,3 ;OUTPUT BIT3=L return ;------------------------------------------ ; delay subroutine ;---------------------------- ;delay 5 step delay nop nop nop nop nop return ;---------------------------- ;delay 0.4msec TIM04 MOVLW 0F9H ;249 MOVWF CNT1 TIMLP1 NOP DECFSZ CNT1,F GOTO TIMLP1 ;2+4*249-1=997 RETURN ;998*0.4usec=0.4msec ;-------------------------- ;delay 100msec TIM100 MOVLW 0F9H ;249 MOVWF CNT2 TIMLP2 CALL TIM04 ;2+1003*249-1 DECFSZ CNT2,F GOTO TIMLP2 RETURN ;249749*0.4usec=100msec ;---------------------------- ;delay W x 100msec TIMW MOVWF CNT3 TIMLP3 CALL TIM100 ;1+249755*W-1 DECFSZ CNT3,F GOTO TIMLP3 RETURN ;--------------------------------------------- wait5sec bsf portb,0 bcf portb,1 bsf portb,2 MOVLW d'10' ;1sec CALL TIMW bcf portb,0 MOVLW d'10' ;1sec CALL TIMW bsf portb,0 bsf portb,1 bcf portb,2 MOVLW d'10' ;1sec CALL TIMW bcf portb,0 MOVLW d'10' ;1sec CALL TIMW bsf portb,0 bcf portb,1 MOVLW d'10' ;1sec CALL TIMW bcf portb,0 return ;--------------------------------------------- ; END