;***************************************************************
;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