;*******************************************************************
;dbt_speed_control
;Derbot wheel speed is measured by frequency counting. Derbot
;runs forward at fixed speed - demonstrates simple speed control.
;
;TJW 7.7.05										Tested, 24.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'10011001' ;
	movwf	trisc		;and port C bits, bit 0 ip for opto. I2C bits set to ip (tho not used)
	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 	0ff			;set PWM prd to its maximum. 
	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 input, sync with int clock
	movlw	B'01111100' ;switch on Timer2, no prescale, /16 postscale. 
	movwf	t2con		  ;Timer int prd is 4.096ms, freq is 244.14Hz
	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'125'	;load interrupt counter, will adjust speed every 512ms
	movwf	int_cntr
	bcf pir1,tmr2if	;clear pending interrupt
	bsf	intcon,peie	;enable peripheral interrupt
	bsf	intcon,gie	;enable global interrupt
;
;start running, then wait for interrupt
	movlw	D'125'	;set PWM rate mid-range (ie 0 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 4.096ms. Count 125 (0.512s), then
;measure pulse count on each wheel, and calculate new PWM setting
;*********************************************************************
Timer2_Int	bsf 	portc,5	;diagnostic
	decfsz	int_cntr
	goto	int_end
;here if checking speed
	movf	tmr0,0	;save counter values
	movwf	tmr0_temp
	movf	tmr1l,0
	movwf	tmr1_temp
	clrf	tmr0	;clear counters 
	clrf	tmr1l
	btfss	portc,0	;but increment T1 if it is zero, as first rising edge isn't seen
	incf	tmr1l
;Adjust left motor speed. Find “error” frequency by subtraction
	movf	tmr0_temp,0
	sublw	D'10'	 ;this is demand speed
	movwf	tmr0_temp
	btfss	status,c ;check for polarity of result, skip if +ve
	goto	left_err_neg
	bcf		status,c ;here if result positive, 
;"Amplify" result by shifting left. 
;Check for over-range, bit 7 is initially 0 (from numbers used)	
	btfss  tmr0_temp,6	;check for possible over-range, don't shift if bit 6 set
	rlf		tmr0_temp,f	
	bcf		status,c
	btfss	tmr0_temp,6	;shift again
	rlf		tmr0_temp,f	
	bcf		status,c
	btfss	tmr0_temp,6	;shift again
	rlf		tmr0_temp,f	
	movf	tmr0_temp,w
	addwf	ccpr2l,0	;add in current PWM value, result to W
	movwf	tmr0_temp	;test and correct for over-range
	btfsc	status,c
	goto 	set_l_max	;carry set, so set max op
	movf	tmr0_temp,0 ;output calculated result to PWM
	movwf	ccpr2l
	goto 	rt_adj		;and proceed
set_l_max movlw	D'255'	;here if output value has saturated, hence output max value
	movwf	ccpr2l
	goto 	rt_adj		;and proceed
left_err_neg comf tmr0_temp,0 ;this will be holding a negative no,
	addlw	1			;therefore correct by taking 2's comp.
	movwf	tmr0_temp
	btfss  tmr0_temp,6	;check for possible over-range, don't shift if bit 6 set
	rlf		tmr0_temp,f	
	bcf		status,c
	btfss	tmr0_temp,6	;shift again
	rlf		tmr0_temp,f	
	bcf		status,c
	btfss	tmr0_temp,6	;shift again
	rlf		tmr0_temp,f	
	movf	tmr0_temp,w
	subwf	ccpr2l,1	;and subtract from current PWM value. Over-range testing not included
;
;now adjust right motor speed
rt_adj	movf	tmr1_temp,0
	sublw	D'10'	 ;this is demand speed
	movwf	tmr1_temp
	btfss	status,c ;check for polarity of result, skip if +ve
	goto	rt_err_neg
	bcf		status,c ;here if result positive, 
;"Amplify" result by shifting left. 
;Check for over-range, bit 7 is initially 0 (from numbers used)	
	btfss  tmr1_temp,6	;check for possible over-range, don't shift if bit 6 set
	rlf		tmr1_temp,f	
	bcf		status,c
	btfss	tmr1_temp,6	;shift again
	rlf		tmr1_temp,f	
	bcf		status,c
	btfss	tmr1_temp,6	;shift again
	rlf		tmr1_temp,f	
	movf	tmr1_temp,w
	addwf	ccpr1l,0	;add in current PWM value, result to W
	movwf	tmr1_temp	;test and correct for over-range
	btfsc	status,c
	goto 	set_r_max	;carry set, so set max op
	movf	tmr1_temp,0 ;output calculated result to PWM
	movwf	ccpr1l
	goto 	loop_end		;and proceed
set_r_max movlw	D'255'	;here if output value has saturated, hence output max value
	movwf	ccpr1l
	goto 	loop_end		;and proceed
rt_err_neg comf tmr1_temp,0 ;this will be holding a negative no,
	addlw	1			;therefore correct by taking 2's comp.
	movwf	tmr1_temp
	btfss  tmr1_temp,6	;check for possible over-range, don't shift if bit 6 set
	rlf		tmr1_temp,f	
	bcf		status,c
	btfss	tmr1_temp,6	;shift again
	rlf		tmr1_temp,f	
	bcf		status,c
	btfss	tmr1_temp,6	;shift again
	rlf		tmr1_temp,f	
	movf	tmr1_temp,w
	subwf	ccpr1l,1	;and subtract from current PWM value. Over-range testing not included
;
loop_end movlw	D'125'	;reload interrupt counter
	movwf	int_cntr
int_end bcf pir1,tmr2if
	call delay1		;just to see diag led
	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
