;***************************************************************
;odometry_test
;Derbot runs forward a fixed distance, turns by 180 degrees,
;and returns - looping continuously.
;
;TJW 19.5.05								Tested 23.5.05
;***************************************************************
;Clock is 4 MHz

	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
flags	equ		29	;0 if L motor has completed, 1 for rt motor
	
;Specify some port bits
;For Port A
mot_en_rt 	equ	2	;motor enable right
mot_en_left	equ	5	;motor enable left

;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 PWM op
mot_rt 		equ 2	;right motor PWM op
led_rt		equ 5	;diagnostic led
led_left	equ 6	;diagnostic led
mode		equ	7	;mode switch

	org 00
;Initialise
;set up SFRs in Bank 1
	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' ;bit 0 ip for opto
	movwf	trisc		;and port C bits
	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
;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'00000100'	;switch on Timer2, no pre or 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
	bsf	portb,2		;switch on optos
;*************************************************
;run forward fixed distance, then turn and return
;*************************************************
opto_move clrf	tmr0 ;clear timers
	clrf	tmr1l
	clrf	tmr1h
	clrf	flags
	btfss	portc,0	;increment T1 if it is zero, as first rising edge isn't seen
	incf	tmr1l
	call	leftmot_fwd	;start motors running
	call	rtmot_fwd
opto_loop call opto_to_led	;transfer opto states to diagnostic leds
;move forward set distance (1m)
	movlw	D'91'		;test if counter has reached this value
	subwf	tmr0,0		
	btfsc	status,z
	bcf		porta,mot_en_left ;disable motor if value reached
	movlw	D'91'
	subwf	tmr1l,0
	btfsc	status,z
	bcf		porta,mot_en_rt
;if both motors stopped, proceed to turn, otherwise loop
	btfsc	porta,mot_en_left
	goto	opto_loop
	btfsc	porta,mot_en_rt
	goto	opto_loop
;now turn
	call 	delay500	;ensure AGV is at rest
	movlw 	00
	movwf	tmr0		;clear timers and flags
	movwf	tmr1l
	btfss	portc,0	;increment T1 if it is zero, as first rising edge isn't seen
	incf	tmr1l
	call	leftmot_fwd	;turn on spot, left motor forward, right back 
	call	rtmot_rev	
;execute the turn
opt_loop1 call opto_to_led	;transfer opto states to diagnostic leds
;rotate by 180 degrees
	movlw	D'23'		;test if counter has reached this value
	subwf	tmr0,0		
	btfsc	status,z
	bcf		porta,mot_en_left ;disable motor if value reached
	movlw	D'23'
	subwf	tmr1l,0
	btfsc	status,z
	bcf		porta,mot_en_rt
;if both motors stopped, proceed to straight line, otherwise loop
	btfsc	porta,mot_en_left
	goto	opt_loop1
	btfsc	porta,mot_en_rt
	goto	opt_loop1
	call 	delay500	;ensure we're at rest
	goto	opto_move
;**********************************************
;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  
;motor drive SRs, set motors running fixed (modest) speed
;Forward and reverse speeds must be equal and opposite, for good turn on spot.
leftmot_fwd		;sets left motor running forward
		bsf		porta,mot_en_left
		movlw	D'190'
		movwf	CCPR2L
		return
rtmot_fwd	bsf	porta,mot_en_rt	
		movlw	D'190'
		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
opto_to_led bcf 	portc,led_left	;preclear left led
		btfss 	porta,4
		bsf		portc,led_left	;but set it if opto on
		bcf		portc,led_rt	;preclear right led
		btfss 	portc,0
		bsf		portc,led_rt	;but set it if opto on
		return
		end
