;***************************************************************
;Dbt_blind_Nav_PWM
;Derbot moves by “blind” navigation. 
;Moves forward, and reverses and turns on bump. 
;Fixed rate PWM applied to set reasonable speeds.
; 
;TJW 5.5.05. rev. 2.6.08		 	Tested 9.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 delay SR
delcntr2 	equ 21

;Specify some port bits
;For Port A
mot_en_rt 	equ 2		;right enable input to L293D ic
mot_en_left	equ 5		;left enable input to L293D ic

;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 PWM drive, 1=forward
mot_rt 		equ 2		;right motor PWM drive, 1=forward
led_rt		equ 5		;diagnostic led
led_left	equ 6		;diagnostic led
mode		equ 7		;mode switch

	org 	00
;Initialise, settings for Derbot Build Stage 1. For other stages settings will differ
	bcf	status,rp1
	bsf	status,rp0 	;select memory bank 1
	movlw	B'00000000'	;set port A bits according to their function, 1=input, 0=output
	movwf	trisa		
	movlw	B'00110000'
	movwf	trisb		;also port B bits
	movlw	B'10000000'
	movwf	trisc		;and port C bits
	movlw	B'00000110'
	movwf	adcon1		;set port A for digital i/o function
	movlw 	D'249'      	;sets PWM period. (alternative values can be used)
	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
;
;The “main” program starts here
;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
;determine operating mode
	btfsc	portc,mode
	goto	sw_move
;start motors
start 	call 	leftmot_fwd	;sets left motor running forward
	call 	rtmot_fwd	;sets right motor running forward
;test for bumps - reverse and turn if either microswitch closes
loop 	btfss 	portb,us_rt	;test right microswitch
	goto	rev_rt
	btfss 	portb,us_left 	;test left microswitch
	goto	rev_left
	call 	delay200
	goto	loop
;
rev_rt bsf	portc,led_rt
	bcf	porta,mot_en_left ;stop motors
	bcf	porta,mot_en_rt	
	bsf 	portb,sounder	  ;small bleep from sounder
	call 	delay200
	bcf 	portb,sounder
;reverse both motors 
	call 	leftmot_rev
	call 	rtmot_rev
	call 	delay500
	call 	delay500
	call 	delay500
	call 	leftmot_fwd	;left motor forward to turn
	call 	delay500
	call 	delay500
	bcf	portc,led_rt
	goto 	start
;
rev_left bsf 	portc,led_left
	bcf	porta,mot_en_rt ;stop motors
	bcf	porta,mot_en_left	
	bsf 	portb,sounder   ;small bleep from sounder
	call 	delay200
	bcf 	portb,sounder
;reverse both motors
	call 	leftmot_rev
	call 	rtmot_rev
	call 	delay500
	call 	delay500
	call 	delay500
	call 	rtmot_fwd	;right motor forward to turn
	call 	delay500
	call 	delay500
	bcf	portc,led_left
	goto 	start

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

;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
leftmot_fwd		;sets left motor running forward
		bsf		porta,mot_en_left
		movlw	D'176'
		movwf	CCPR2L
		return

rtmot_fwd	bsf	porta,mot_en_rt	
		movlw	D'176'
		movwf	CCPR1L
		return

leftmot_rev bsf	porta,mot_en_left	
		movlw	D'80'
		movwf	CCPR2L
		return

rtmot_rev	bsf	porta,mot_en_rt	
		movlw	D'80'
		movwf	CCPR1L
		return

	end