; Line following program for the Blue Robot ; ; ; Written by:Steven Easley ; Email: seasley@robotdungeon.com ; Webpage: www.robotdungeon.com list p=16f876a ; list directive to define processor #include ; processor specific variable definitions __CONFIG _CP_OFF & _WDT_OFF & _BODEN_OFF & _PWRTE_ON & _HS_OSC & _LVP_OFF & _CPD_OFF ;Define control outputs for left motor #DEFINE mot1en PORTB,0 #DEFINE motdir1a PORTB,1 #DEFINE motdir2a PORTB,2 ;Define control outputs for right motor #DEFINE mot2en PORTB,3 #DEFINE motdir1b PORTB,4 #DEFINE motdir2b PORTB,5 ;Define some values for sampling different A2D channels chan0 equ b'01000101' ;select channel AN0 on PA0 chan1 equ b'01001101' ;select channel AN1 on PA1 chan2 equ b'01010101' ;select channel AN2 on PA2 chan3 equ b'01011101' ;select channel AN3 on PA3 chan4 equ b'01100101' ;select channel AN4 on PA5 ;set the minimum for a black line minvolt equ .128 cblock 0x20 temp counter d1 d2 lmot rmot sensors sen_left sen_lc sen_mid sen_rc sen_right endc org 0x0000 goto init org 0x0004 goto ISR ;---------------------------------------------------------- ;This is the interupt service routine ;---------------------------------------------------------- ISR bcf INTCON, GIE ; disable interrupts bcf INTCON, INTF bsf INTCON, GIE ; enable interrupts retfie ;---------------------------------------------------------- ;This is a 5us delay delay5us movlw 0x08 movwf d1 delay_5us decfsz d1, f goto delay_5us return ;---------------------------------------------------------- ;This is a 3500us delay delay3500us movlw 0xAB movwf d1 movlw 0x0E movwf d2 delay_3500us decfsz d1, f goto $+2 decfsz d2, f goto delay_3500us return ;---------------------------------------------------------- ;Routine that will scan the snesors and set the approprite ;bits in the sensors register check_sens ;turn the A2D on and let is settle bsf ADCON0, 0 ;turn A2D on call delay5us call delay5us call delay5us call delay5us ;check the left sensor movlw chan0 movwf ADCON0 call delay5us call delay5us call delay5us call delay5us call delay5us call delay5us bsf ADCON0,GO pause1 btfsc ADCON0,GO ;Wait for conversion to complete goto pause1 movf ADRESH,W movwf sen_left ;check the left-center sensor movlw chan1 movwf ADCON0 call delay5us call delay5us call delay5us call delay5us call delay5us call delay5us bsf ADCON0,GO pause2 btfsc ADCON0,GO ;Wait for conversion to complete goto pause2 movf ADRESH,W movwf sen_lc ;check the center sensor movlw chan2 movwf ADCON0 call delay5us call delay5us call delay5us call delay5us call delay5us call delay5us bsf ADCON0,GO pause3 btfsc ADCON0,GO ;Wait for conversion to complete goto pause3 movf ADRESH,W movwf sen_mid ;check the right-center sensor movlw chan3 movwf ADCON0 call delay5us call delay5us call delay5us call delay5us call delay5us call delay5us bsf ADCON0,GO pause4 btfsc ADCON0,GO ;Wait for conversion to complete goto pause4 movf ADRESH,W movwf sen_rc ;check the right sensor movlw chan4 movwf ADCON0 call delay5us call delay5us call delay5us call delay5us call delay5us call delay5us bsf ADCON0,GO pause5 btfsc ADCON0,GO ;Wait for conversion to complete goto pause5 movf ADRESH,W movwf sen_right bcf ADCON0, 0 ;turn A2D off return ;---------------------------------------------------------- ;Subroutine used to check the sensor values. fix_readings clrf sensors movlw minvolt subwf sen_left,w skpc goto fix_lc bsf sensors,5 fix_lc movlw minvolt subwf sen_lc,w skpc goto fix_mid bsf sensors,3 fix_mid movlw minvolt subwf sen_mid,w skpc goto fix_rc bsf sensors,2 fix_rc movlw minvolt subwf sen_rc,w skpc goto fix_right bsf sensors,1 fix_right movlw minvolt subwf sen_right,w skpc goto end_fix bsf sensors,0 end_fix return ; Initialize all the ports and stuff init ; bank 0 bcf STATUS, RP0 ;Goto the right page movlw b'00000000' movwf PORTA movlw b'00000000' movwf PORTB movlw b'00000000' movwf PORTC movlw b'10000000' ; Setup A2D on all portA pins and set osc. bits movwf ADCON0 movlw b'00000000' ; Turn the UART off movwf RCSTA ; bank 1 bsf STATUS, RP0 ; Goto the right page movlw b'00111111' ; Set portA to all inputs movwf TRISA movlw b'00000000' ; Set PortB output movwf TRISB movlw b'00000000' ; Set PortC output movwf TRISC movlw b'00000000' ; Setup A2D left justified movwf ADCON1 bcf STATUS, RP0 ;Goto the right page ;Set the motors to go forward bcf mot1en bsf motdir1a bcf motdir2a bcf mot2en bsf motdir1b bcf motdir2b movlw minvolt+50 movwf sen_mid clrf sen_left clrf sen_lc clrf sen_rc clrf sen_right main call check_sens call fix_readings middle btfss sensors,2 goto lc movlw .70 movwf rmot movwf lmot goto motor lc btfss sensors,3 goto rc movlw .85 movwf rmot movlw .60 movwf lmot goto motor rc btfss sensors,1 goto left movlw .60 movwf rmot movlw .85 movwf lmot goto motor left btfss sensors,5 goto right movlw .100 movwf rmot movlw .50 movwf lmot goto motor right btfss sensors,0 goto find movlw .50 movwf rmot movlw .100 movwf lmot goto motor find ; movlw .110 ; movwf rmot ; movlw .95 ; movwf lmot motor movlw 0x00 movwf counter bsf mot1en bsf mot2en poll_motors movf counter,w subwf lmot,w skpc bcf mot1en movf counter,w subwf rmot,w skpc bcf mot2en call delay5us call delay5us incfsz counter,f goto poll_motors call delay3500us goto main end