;*******************************************************************
;Dbt_light_seek
;Derbot seeks light. PWM applied. Speed is dependent on 
;light difference (front to back), so Derbot comes to a 
;halt when light difference is minimal. Microswitches used
;for bump detection.
;
;TJW 19.5.05									Tested 19.5.05
;*******************************************************************
;Clock is 4MHz
;
	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
ldr_rt	 equ	22	;stores for ldr values, read thru ADC
ldr_left equ	23
ldr_rear equ	24
ldr_ave	 equ	25	;computed average of front ldrs
ldr_diff equ	26	;difference between front ldrs, left-right
ldr_fwd	 equ	27	;ave fwd speed, if used
temp	 equ	28  ;a temp location, to be used only in consecutive instructions
	
;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 4	;right microswitch
us_left		equ 5	;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
;Initialise
	bcf	status,rp1
	bsf	status,rp0 		;select memory bank 1
	movlw	B'00001011'	;set port A bits according to their function
	movwf	trisa		
	movlw	B'11001000'
	movwf	trisb		;also port B bits
	movlw	B'10000000'
	movwf	trisc		;and port C bits
	movlw	B'00000100' ;Set up ADC, left justified result,
	movwf	adcon1		  ;port A bits 0,1,3 for analog input
	movlw 	0ff			;set PWM prd
	movwf	pr2
	bcf	status,rp0 		;select bank 0
;set up PWM
	movlw	B'00000100'	;switch on Timer2, no pre or postscale
	movwf	t2con
	movlw	B'00001100' ;enable PWM
	movwf	ccp1con
	movwf	ccp2con
	movlw	B'01000001'	;Set up ADC, clock Fosc/8, switch ADC on but not running,
	movwf	adcon0		;input channel selection currently irrelevant
;
;Switch all outputs off
	movlw 00
	movwf porta
	movwf portb
	movwf portc
;diagnostic, flash leds
	bsf portc,6
	bsf portc,5
	call delay500
	bcf portc,6
	bcf portc,5
	call delay500
;Read mode switch to determine Derbot operating mode
	btfsc	portc,mode
	goto	sw_move
	bsf		porta,mot_en_left	;enable motors permanently
	bsf		porta,mot_en_rt
;***********************************
;The “main” program loop starts here
;***********************************
;check first for collisions
light_loop btfss portb,us_rt ;check right microswitch
	call	rev_rt
	btfss 	portb,us_left	;check left microswitch
	call	rev_left
;
;read and store ldrs
	movlw B'01000001' ;select channel 0 (left ldr)
	movwf	adcon0
	call 	delay11u	;acquisition time 
	bsf		adcon0,go	;start conversion
 	btfsc	adcon0,go   ;wait for conversion to complete
	goto	$-1
	movf	adresh,0
	sublw	0ff			;reverse "polarity" of result
	movwf	ldr_left	
	movlw   B'01001001' ;select channel 1 (right ldr)
	movwf	adcon0
	call 	delay11u
	bsf		adcon0,go	;start conversion
	btfsc	adcon0,go   ;wait for conversion to complete
	goto	$-1
	movf	adresh,0
	sublw	0ff			;reverse "polarity" of result
	movwf	ldr_rt	
	movlw   B'01011001' ;select channel 3 (rear ldr)
	movwf	adcon0
	call 	delay11u
	bsf		adcon0,go	;start conversion
	btfsc	adcon0,go   ;wait for conversion to complete
	goto	$-1
	movf	adresh,0
	sublw	0ff			;reverse "polarity" of result
	movwf	ldr_rear	
;divide all values by 2, to avoid over-range when adding PWM offset
	bcf		status,c
	rrf		ldr_rt,1
	bcf		status,c
	rrf		ldr_left,1	
	bcf		status,c
	rrf		ldr_rear,1
;average front two and store
	movf	ldr_rt,0	;add two front ldr values
	addwf	ldr_left,0
	movwf	ldr_ave
	rrf		ldr_ave,1	;divide by two, this is the average
;determine ave fwd velocity term, which may be used later
	movf	ldr_rear,0	;subtract rear from ave
	subwf	ldr_ave,0
	movwf	ldr_fwd
;test for brightest, and take action
	movf	ldr_rt,0	;subtract right from left
	subwf	ldr_left,0
	movwf	ldr_diff	;save the difference value
	btfss	status,c	;skip if ldr_left>ldr_rt
	goto	rt_gt_left
;here if ldr_left>ldr_rt
	movf	ldr_rear,0 ;subtract rear from left
	subwf	ldr_left,0
	btfss	status,c	;skip if ldr_left>ldr_rear
	goto	rot_left	;go here if rear is brightest
	goto	lgt_fwd_l	;light is front left, hence move fwd left
;here if ldr_rt>ldr_left
rt_gt_left comf	ldr_diff,0 ;this will be holding a negative no,
	addlw	1			;therefore correct by taking 2's comp.
	movwf	ldr_diff
	movf	ldr_rear,0 ;subtract rear from right
	subwf	ldr_rt,0
	btfss	status,c	;skip if ldr_rt>ldr_rear
	goto	rot_rt
	goto	lgt_fwd_r	;light is front rt, hence move fwd rt
;fixed speed rotation, if light is at rear
rot_left call rtmot_fwd
	call 	leftmot_rev
	goto 	loopend
rot_rt	call leftmot_fwd
	call 	rtmot_rev
	goto 	loopend
;light is front left, hence move forward left
lgt_fwd_l	movf ldr_diff,0
	addwf 	ldr_fwd,0
	movwf	temp
	btfsc	status,c	;check for overflows
	movlw	7f			;will use this value if overflowed
	btfsc	temp,7
	movlw	7f
	movwf	temp
	bcf		status,c
	btfss	temp,6
	rlf		temp,1	;scale up this value
	btfss	temp,6
	rlf		temp,1	;scale up this value
	btfss	temp,6
	rlf		temp,1	;scale up this value
	movf	temp,0
	addlw	80		;offset for PWM
	movwf	CCPR1L	;set right motor, which is greater	
	movf 	ldr_diff,0
	subwf 	ldr_fwd,0
	movwf	temp
	btfss	status,c ;has subtraction caused borrow?
	clrf	temp	;if yes, clear temp to 0
	bcf		status,c
	btfss	temp,6
	rlf		temp,1	;scale up this value
	btfss	temp,6
	rlf		temp,1	;scale up this value
	btfss	temp,6
	rlf		temp,1	;scale up this value
	movf	temp,0
	addlw	80		;offset for PWM
	movwf	CCPR2L	;drive left motor weaker
	goto 	loopend	
;
;light is front rt, hence move forward rt
lgt_fwd_r	movf ldr_diff,0
	addwf 	ldr_fwd,0
	movwf	temp
	btfsc	status,c	;check for overflows
	movlw	7f
	btfsc	temp,7
	movlw	7f
	movwf	temp
	bcf		status,c
	btfss	temp,6
	rlf		temp,1	;scale up this value
	btfss	temp,6
	rlf		temp,1	;scale up this value
	btfss	temp,6
	rlf		temp,1	;scale up this value
	movf	temp,0
	addlw	80		;offset for PWM
	movwf	CCPR2L	;drive left motor harder	
	movf 	ldr_diff,0
	subwf 	ldr_fwd,0
	movwf	temp
	btfss	status,c	;has subtraction caused borrow?
	clrf	temp	;if yes, clear temp to 0
	bcf		status,c
	btfss	temp,6
	rlf		temp,1	;scale up this value
	btfss	temp,6
	rlf		temp,1	;scale up this value
	btfss	temp,6
	rlf		temp,1	;scale up this value
	movf	temp,0
	addlw	80		;offset for PWM
	movwf	CCPR1L	;drive right motor weaker
	goto 	loopend	
;
loopend	call delay200
	movlw	B'01000000'	;toggle led
	xorwf	portc,1
	goto 	light_loop
;
;*************************************
;move microswitch states to diag leds
;*************************************
sw_move bcf portc,led_left	;preclear left led
	btfss 	portb,us_left
	bsf		portc,led_left	;but set it if button pressed
;
	bcf		portc,led_rt	;preclear port C, bit 5
	btfss 	portb,us_rt
	bsf		portc,led_rt	;but set it if button pressed
	goto	sw_move
	
;**********************************************
;SUBROUTINES
;**********************************************
;Right microswitch "bump" detected. AGV reverses, and turns left.
rev_rt bsf	portc,led_rt
	bcf	porta,mot_en_left ;stop motors
	bcf	porta,mot_en_rt	
	bsf portb,sounder
	call delay200
	bcf portb,sounder
;both motors in reverse
	call leftmot_rev
	call rtmot_rev
	call delay500
	call delay500
	call leftmot_fwd	;left motor forward to turn
	call delay500
	call delay500
	bcf	portc,led_rt
	return
;
;Left microswitch "bump" detected. AGV reverses, and turns right.
rev_left bsf portc,led_left
	bcf	porta,mot_en_rt ;stop motors
	bcf	porta,mot_en_left	
	bsf portb,sounder
	call delay200
	bcf portb,sounder
;both motors in reverse
	call leftmot_rev
	call rtmot_rev
	call delay500
	call delay500
	call rtmot_fwd	;right motor forward to turn
	call delay500
	call delay500
	bcf	portc,led_left
	return

;introduces delay of 11us approx
delay11u  movlw 	D'2'	;5 cycles called, 
;			each taking 3us, plus call, return (2 ea), and 2 move insts
;			less one cycle lost when last goto is hopped
        movwf  	delcntr1
dela    decfsz 	delcntr1,1 ;3 inst cycles in this loop, ie 3us
        goto 	dela
        return
;
;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
;
;200ms delay (approx)	;200 calls to delay1
delay200  movlw D'200'
        movwf  	delcntr2
del2    call 	delay1
        decfsz  delcntr2,1
        goto 	del2
        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  
;motor drive SRs, set motors running fixed (modest) speed
leftmot_fwd		;sets left motor running forward
		bsf		porta,mot_en_left
		movlw	D'196'
		movwf	CCPR2L
		return
rtmot_fwd	bsf	porta,mot_en_rt	
		movlw	D'196'
		movwf	CCPR1L
		return
leftmot_rev bsf	porta,mot_en_left	
		movlw	D'60'
		movwf	CCPR2L
		return
rtmot_rev	bsf	porta,mot_en_rt	
		movlw	D'60'
		movwf	CCPR1L
		return
	end
