; ; TITLE "Line Follower Robot Control" ;;; Written by Dale A. Heatherington August 1999 RADIX dec processor p16C73 ; ERRORLEVEL 1,-202 __CONFIG h'3ffa' ;see sec. 14-2 HS osc + disable watchdog include "p16c73a.inc" FOSC equ 4000000 ;4 mhz xtal TMR0DIV equ 256 - (FOSC / (8000*16)) ;timer 0 preset for 8000 hz interrupts ;Assumes divide by 4 prescaler ;------------------------------------------------------------ MACROS BANK0 macro bcf STATUS,RP0 endm BANK1 macro bsf STATUS,RP0 endm ;-------------------------------------------------------------------- ;Analog to digital converter control word for each cannnel. ;These vectors are loaded into ADCON0 before each conversion. ;After a delay of "acqusition" time the go/~done bit must be set to ;start conversion. ; AD_LMFB equ b'10011001' AD_RMFB equ b'0010001' AD_SPEED equ b'10000001' AD_TRACK equ b'10001001' ;--------------------------------------------------------------------- ;Note: w_temp is mirrored at 0xA0 w_temp equ 0x20 ;save W here during interrupt status_temp equ 0x21 ;save STATUS here during interrupt pclath_temp equ 0x22 ;save PCLATH here fsr_temp equ 0x23 ;save FSR here ; D_SPEED equ 0x24 ;Raw analog values stored here D_TRACK equ 0x25 D_RMFB equ 0x26 D_LMFB equ 0x27 ; pulseleft equ 0x28 ;speed pulse timing registers pulseright equ 0x29 counter equ 0x2a ; work equ 0x2b ;used to pass value to "rectify" subroutine SPEED_LEFT equ 0x2c ;Actual speed value (0..127) with bit 7 true = REVERSE SPEED_RIGHT equ 0x2d work2 equ 0x2e R0 equ 0x2f ;used by A-D converter subroutine dir equ 0x30 ;high 3 bits store direction flags ;bits 1,2,3 store the "stopped" bits index equ 0x31 ;used to look up values in speed table workB equ 0x32 ;hold image of PORTB pulsewidth equ 0x33 ;holds - pulse width value R_integrate equ 0x34 ; L_integrate equ 0x35 R_SPEED equ 0x36 L_SPEED equ 0x37 _temp16H equ 0x38 _temp16L equ 0x39 reverse0 equ 7 ;Reversing control bits in dir reverse1 equ 6 ;reverse2 equ 5 stop0 equ 3 ;"Motor is stopped" control bits in dir stop1 equ 2 ;Port B pulse bits FwdLeft equ 7 ;forward speed control pulse Left RevLeft equ 6 ;reverse speed control pulse Left FwdRight equ 5 ;forward speed control pulse Right RevRight equ 4 ;reverse speed control pulse Right ; ;----------------- ;------------------------------------- ;BANK1 w_temp1 equ 0xa0 ;mirror of w_temp ;----------------------------------------------------------------------- ;Start of code ; org 0 ;RESET vector goto start org 4 ;Interrupt vector ;------------------------------------------------------------------------ ;This is a macro spdControl MACRO speed,pulse,dirbit,Fwd,Rev,stop local p0,p0rev,p0b,p0xz,p0exit p0 movf pulsewidth,W addwf pulse,w ;add pulse timing register value to it btfsc STATUS,C ;test carry. carry means pulse > 10, skip if not Carry goto p0b ;goto p0b if pulse > 10 bcf workB,Fwd bcf workB,Rev btfsc dir,dirbit ;see if this is reverse mode goto p0rev bsf workB,Fwd ;set forward bit goto p0b p0rev bsf workB,Rev ;else set Reverse bit p0b decfsz pulse,f ;decrement pulse timing reg. goto p0exit ;exit if not zero yet ; ;At this point pulse timer is zero! bcf workB,Fwd bcf workB,Rev movf speed,W ;get new speed value movwf index call speedlookup ;table value translation movwf pulse ;set new pulse value movlw 10 addwf pulse,f ;bias the pulse value bcf dir,dirbit ;assume forward btfsc speed,7 ;test the sign bit bsf dir,dirbit ;if set then set the reverse flag goto p0 ;go back and start up a new pulse cycle p0exit ENDM ;----------------------------------------------------------- ;Integrator macro: ;SPEED is current speed setting ;MFB is motor feedback voltage value ;accum is the integrator output value integrate MACRO SPEED,MFB,accum local pos,iexit movf SPEED,W ;SPEED to W addlw 0x80 ;convert to unsigned value movwf work ;save it in work reg movf MFB,w ;motor feed back to w addlw 0x80 ;convert to unsigned subwf work,f ;work = SPEED - MFB btfsc STATUS,C ;test result goto pos ;If positive goto "pos" decf accum,f ;result is negative, lower integrator movlw 0x7f ;Clip it at 0x80 xorwf accum,w btfsc STATUS,Z incf accum,f ;if it underflowed to 7f bump it to 0x80 again goto iexit pos incf accum,f ;result is positive, increase integrator movlw 0x80 ;Clip it at 0x7f xorwf accum,w btfsc STATUS,Z decf accum,f ;if it overflowed to 0x80 bump it down to 0x7f iexit ENDM ;End of macro ;-------------------------------------------- ;Saturating addition macro. ;Add argA to W with result to W. ;If result is greater than 0x7f or lower than 0x80 ;clip the result at 0x7f or 0x80 ;This prevents overflows and emulates the behavior of OP-AMPS. addsat MACRO argA local A_minus,B_minus,A_B_minus,A_B_diff,m_exit movwf work ;save w value in "work" btfsc argA,7 ;Test argA for sign goto A_minus btfsc work,7 ;test work (W) for sign goto A_B_diff movf argA,w bcf STATUS,C ;clear carry addwf work,f ; argB = argA + argB movf work,w ;unclipped result in w btfsc work,7 ;test sign of result movlw 0x7f ;Return clipped result goto m_exit A_minus btfsc work,7 ;test sign of argB goto A_B_minus A_B_diff movf argA,w bcf STATUS,C addwf work,W ;result in W goto m_exit A_B_minus movf argA,W bcf STATUS,C addwf work,f movf work,w btfss work,7 ;test for sign movlw 0x80 ;return clipped result in W m_exit ENDM ;--------------------------------------------------------------------------------- ;Interrupt handler _IRQ movwf w_temp ;save W in ram swapf STATUS,W ;get flags into W (nibble swapped) clrf STATUS ;force bank 0 movwf status_temp ;save status reg in bank 0 movf PCLATH,W ;get PC latch movwf pclath_temp ;save it clrf PCLATH ;set pclath to page 0 movf FSR,W ;get FSR movwf fsr_temp ;save it isr_poll btfss INTCON,T0IF ;check for timer 0 interrupt goto isr_ret bcf INTCON,T0IF ;clear int flag if it was set movlw TMR0DIV movwf TMR0 ;restart timer 0 movf workB,W ;get port B image movwf PORTB ;write it to the real PORTB bsf PORTB,1 ;set bit 0 for timing tests BANK0 isr_02 movlw -11 movwf pulsewidth spdControl SPEED_LEFT,pulseleft,reverse0,FwdLeft,RevLeft,stop0 ;Leftchan movlw -11 movwf pulsewidth spdControl SPEED_RIGHT,pulseright,reverse1,FwdRight,RevRight,stop1 ;Right chan bcf PORTB,1 isr_ret movf fsr_temp,W ;restore FSR movwf FSR movf pclath_temp,W ;restore PCLATH movwf PCLATH swapf status_temp,W ;restore STATUS movwf STATUS swapf w_temp,F swapf w_temp,W ;restore W retfie ;return from interrupt ;Note: see 14.5 for important note about doing global interrupt disable! ;--------------------------------------------------------------------------- org 100h start clrf INTCON ;disable all interrupts clrf STATUS clrf PORTA clrf PORTB clrf PORTC ; clrf T1CON BANK1 clrf TRISB ;Port B all outputs clrf TRISC ;Port C all outputs movlw 0ffh ;all port A bits are inputs (analog) movwf TRISA movlw b'10000001' ;prescaler assigned to Timer 0 and divide by 4 movwf OPTION_REG clrf ADCON1 ;set all port A to analog with vref=VCC BANK0 ;Bank 0 movlw 0 ;clear some variables movwf SPEED_LEFT movwf SPEED_RIGHT movwf dir movwf workB movlw 255 movwf pulseleft movwf pulseright clrf TMR0 ;Clear TMR0 bcf INTCON,T0IF ;Clear TMR0 interrupt flag movlw TMR0DIV movwf TMR0 ;start timer 0 bsf INTCON,T0IE ;Enable interrupts from Timer 0 bsf INTCON,GIE ;GLOBAL INTERRUPT ENABLE nop clrf R0 Pause decfsz R0,f goto Pause ;------------------------------------------------------------------ Main_Loop bsf PORTC,0 movlw AD_SPEED ; speed channel AD0 call ad_convert movwf L_SPEED ;Speed pot value in *_SPEED movwf R_SPEED movwf D_SPEED movlw AD_TRACK ; line track channel AD1 call ad_convert movwf D_TRACK ;plus values in D_TRACK mean line is ;.. under right photocell ;so we need to speed up left motor ;and slow down right motor movlw 3 movwf counter ;Do the following 3 times L3 movf D_SPEED,w addsat D_TRACK ;Saturated ADD D_TRACK to D_SPEED movwf R_SPEED ;Result to R_SPEED comf D_TRACK,w ;complemented D_TRACK to W movwf work2 incf work2,f ; negative D_TRACK to "work" movf D_SPEED,w ;D_SPEED to W addsat work2 ;Saturated ADD -D_TRACK and D_SPEED movwf L_SPEED ;Result to L_SPEED movlw AD_RMFB call ad_convert movwf D_RMFB ;Save right motor feedback movlw AD_LMFB call ad_convert movwf D_LMFB ;save left motor feedback integrate L_SPEED,D_LMFB,L_integrate integrate R_SPEED,D_RMFB,R_integrate movf L_integrate,W movwf SPEED_LEFT movf R_integrate,W movwf SPEED_RIGHT decfsz counter goto L3 ;loop 3 times bcf PORTC,0 goto Main_Loop ;Loop forever ;--------------------------------------------------------------------------- rectify btfss work,7 ;test for value less than 128 goto _plus movlw 255 ;work in [127..0] xorwf work,W addlw 128 ;reverse speed value [127 = 0 , 0 = 127] iorlw 80h ;set the reverse bit (7) return ;work in [128..255] _plus movlw 128 ;forward speed value addwf work,W return ;returns [128=0 , 255=127] (bit 7 always clear) ;--------------------------------------------------------------------------- ;A to D conversion routine ;Enter with channel in W (ADCON0 setup byte) ;Exits with 8 bit signed value in W ad_convert bcf INTCON,GIE ;disable interrupts btfsc INTCON,GIE ;make sure they are really disabled goto ad_convert movwf ADCON0 movlw 7 ;1000us per instruction cycle at 4 mhz movwf R0 ADC_0 decfsz R0,f ;loop takes 3us per interation goto ADC_0 ;wait for acquisition time 21us bsf ADCON0,GO_DONE ;start conversion bsf INTCON,GIE ;enable interrupts ADC_1 nop btfsc ADCON0,NOT_DONE ;wait for conversion to complete goto ADC_1 movlw 128 addwf ADRES,W ;convert to signed value and return return ;---------------------------------------------------------------------------- ;Convert a signed value to an absolute unsigned value between 127 and 0 ; 0 -> 127 ; 127 -> 0 ; 0x80 -> 0 speedlookup movf index,W ;index to W btfsc index,7 ;test sign bit goto _slminus movlw 0x7f xorwf index,W return _slminus movlw 0x7f andwf index,W return END