;***************************************************************
;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				       					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
	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 	D'249'      ;sets PWM period. (alternative values can be used)
	movwf	pr2
;
;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	;8 inst cycles in this loop, ie 4us
        nop
        nop
        nop
        nop
        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
