;*******************************************************************
;dbt_speed_meas
;Derbot wheel speed is measured by frequency counting. This is a demo
;program. The frequency measurement concept can be embedded into larger 
;programs, eg for speed control.
;TJW 7.7.05					       Tested 11.7.05
;*******************************************************************
;Clock is 4MHz
;Timer 0 counts left motor, Timer 1 right.
;CCP2 drives left motor, CCP1 drives right.
;
	list 	p=16F873A
	#include p16f873A.inc
;
;Set Configuration Word: crystal oscillator HS, WDT off, 
;     power-up timer on, code protect off, LV Program off.
	__CONFIG _HS_OSC & _WDT_OFF & _PWRTE_ON & _LVP_OFF
;
;Specify RAM
delcntr1 equ	20	;used in delay5 & delayADC SRs
delcntr2 equ	21
int_cntr equ	22
tmr0_temp equ	23	;used to develop left PWM op value
tmr1_temp equ	24	;used to develop right PWM op value
flags	equ	29	;0 is L motor has completed, 1 for rt motor
	
;Specify some port bits
;For Port A
mot_en_rt 	equ 2
mot_en_left	equ 5

;For Port B
sounder		equ 1	;piezo electric sounder
us_rt		equ 6	;right microswitch
us_left		equ 7	;left microswitch

;For PortC
mot_left 	equ 1	;left motor direction bit, 1=forward
mot_rt 		equ 2	;right motor direction bit
led_rt		equ 5	;diagnostic led
led_left	equ 6	;diagnostic led
mode		equ 7	;mode switch

	org 00
	goto main

	org	04
	goto	Timer2_Int

;Initialise
;set up SFRs in Bank 1
main 	bcf	status,rp1
	bsf	status,rp0 	;select memory bank 1
	movlw	B'00011011'	;set port A bits, bit 4 ip for opto
	movwf	trisa		
	movlw	B'11001000'
	movwf	trisb		;also port B bits
	movlw	B'10000001' 
	movwf	trisc		;and port C bits, bit 0 ip for opto
	movlw	B'01000100' 
	movwf	adcon1		;select port A bits 0,1,3 for analog input, others digital
	movlw	B'11101000'  	;set up Timer 0: external input, low to high transition,
							;no prescale
	movwf	option_reg	
	movlw 	D'250'		;set PWM prd
	movwf	pr2
	bsf	pie1,tmr2ie	;enable Timer 2 interrupt
;set up SFRs in Bank 0	
	bcf	status,rp0 	;select bank 0
	movlw	B'00000011' 	;set up Timer 1: no prescale, oscillator disabled,  
	movwf	t1con					;external sync input
	movlw	B'01111100' 	;switch on Timer2, no prescale, /16 postscale
	movwf	t2con
	movlw	B'00001100' 	;enable PWM
	movwf	ccp1con
	movwf	ccp2con
;
;Switch all outputs off
	clrf 	porta
	clrf 	portb
	clrf 	portc

;diagnostic, flash leds
	bsf 	portc,6
	bsf 	portc,5
	call 	delay500
	bcf 	portc,6
	bcf 	portc,5
;
;clear timers
	clrf 	tmr0
	clrf 	tmr1l
	clrf 	tmr1h
;set interrupts
	movlw	D'250'		;load interrupt counter
	movwf	int_cntr
	bcf 	pir1,tmr2if	;clear pending interrupt
	bsf	intcon,peie	;enable peripheral interrupt
	bsf	intcon,gie	;enable global interrupt
;
;start running, and wait for Timer interrupt
	movlw	D'200'		;set PWM rate for reasonable forward speed.
	movwf	ccpr1l
	movwf	ccpr2l
	bsf	portb,2		;switch on optos
	bsf	porta,mot_en_left ;enable motors
	bsf	porta,mot_en_rt
wait	goto 	wait
;
;*********************************************************************
;ISR is here. Interrupts occur every 4ms. Count 250 (i.e.1.0s), then
;measure pulse count on each wheel.
;*********************************************************************
Timer2_Int bsf 	portc,5		;diagnostic
	decfsz	int_cntr
	goto	int_end
;here if making a measurement
	movf	tmr0,0		;save counter values
	movwf	tmr0_temp
	movf	tmr1l,0
	movwf	tmr1_temp
	clrf	tmr0		;clear counters 
	clrf	tmr1l
	movlw	D'250'		;reload interrupt counter
	movwf	int_cntr
int_end bcf 	pir1,tmr2if
	bcf 	portc,5		;diagnostic
	retfie	

;*********************************************************
;SUBROUTINES
;*********************************************************
;
;introduces delay of 1ms approx
delay1  movlw 	D'250'	;250 cycles called, 
				;each taking 4us
        movwf  	delcntr1
del1    nop	;4 inst cycles in this loop, ie 4us
        decfsz 	delcntr1,1
        goto 	del1
        return
;500ms delay (approx) - 500 calls to delay1
delay500  movlw D'250'
        movwf  	delcntr2
del5    call 	delay1
		call 	delay1
        decfsz  delcntr2,1
        goto 	del5
        return  
;
	end