	title  "WANDER.ASM - Robot wander behavior test";;;   DT101 Breadbot wander program, 2/1/99, Robert Nansel;; After initialization, Breadbot moves forward until an obstacle is detected.; A right whisker touch causes Breadbot to back, turn left, then resume forward; motion, a left whisker touch triggers a backup then a right turn, and a front; bumper touch triggers a backup then a random left or right turn.;; NOTE: Debug sections commented out in this version;	LIST P=16F84, F=INHX8M, R=DEC	;  16F84 Runs at 10 MHz	errorlevel 0,-305	INCLUDE "\pic\mpasm\p16F84.inc";  RAM Usage	CBLOCK 0x00C_w		; Interrupt Service Routine Context Saving Values_statusleft, right	; Servos speedsRTC		; Real Time clock, 20 msec resolutionMsec_Cnt	; Millisecond delay counterdelay_count	; 20 msec resolution delay counterdebug_buf	; debug byte bufferbit_count	; bit shift countertemp		; temporary storage	ENDC;  I/O bit ConstantsSCL	equ	0	; Serial Clock, output, bit 0 PortASDA	equ	1	; Serial Data, output, bit 1 PortARPulse	equ	0	; Right servo, output, bit 0 PortBLPulse	equ	1	; Left servo, output, bit 1 PortBLWhisk	equ	5	; Right whisker, input, bit 5 PortBFBump	equ	6	; Front bumper, input, bit 6 PortBRWhisk	equ	7	; Right whisker, input, bit 7 PortB	PAGE		__CONFIG _CP_OFF & _WDT_OFF & _HS_OSC & _PWRTE_ON;  Note: WatchDog Timer is OFF				;  Code for WANDER	org	0	goto   Main			; Skip Over Interrupt Service Routine;  Interrupt Service Routine - Output proper speeds to servos	org	4			; ISR at Address 4Int	movwf	_w			; Save the Context Registers	movf	STATUS, w	movwf	_status	movlw	61			; Wait 20 msec between operations	movwf	TMR0	movlw	0x020			; Reset the Interrupt Handler	movwf	INTCON		incf	RTC			; Update Real Time Clock		bsf	PORTB, RPulse		; Start both servo pulses	bsf	PORTB, LPulse	movlw	250			; Delay 1 msec before doing 					; variable	movwf	Msec_Cnt		; part of pulsesMsec	goto	$ + 1			; 10 Cycle loop delay -> 2500 cycles	goto	$ + 1	goto	$ + 1	nop	decfsz	Msec_Cnt	goto	Msec	movlw	250			; Set up to time second msec	movwf	Msec_Cnt		;  -> must be an even number <-	Intloop					; 20 cycle loop, 19 on termination					; average 10 cycles per pulse check	movf	Msec_Cnt, w		; Check right servo	subwf	right, w			;	btfsc	STATUS, C		; If right <= Count then more time left	bcf	PORTB, RPulse		; Else, end right pulse	movf	Msec_Cnt, w		; Check left servo	subwf	left, w	btfsc	STATUS, C		; If left <= Count  then more time left	bcf	PORTB, LPulse		; Else, end left pulse	decf	Msec_Cnt		; Note: won't = 0 if Msec_Cnt is even					; on loop entry	movf	Msec_Cnt, w		; Check right servo	subwf	right, w			;	btfsc	STATUS, C		; If right <= Count then more time left	bcf	PORTB, RPulse		; Else, end right pulse	movf	Msec_Cnt, w		; Check left servo	subwf	left, w	btfsc	STATUS, C		; If left <= Count  then more time left	bcf	PORTB, LPulse		; Else, end left pulse	decfsz	Msec_Cnt		; Continue millisecond countdown	goto	Intloop	bcf	PORTB, RPulse	; Make sure both servo pulses turn off	bcf	PORTB, LPulse	andwf	PORTB;	call	debug			; Shift out debug value, msb firstIntEnd					; Finished with the Interrupt	movf	_status, w	movwf	STATUS	swapf	_w	swapf	_w, w	retfie	PAGEMain	bsf	STATUS, RP0	movlw	0x0D7			; Setup Timer to clock from Internal 					; Clock	movwf	OPTION_REG & 0x07F	;  at slowest	bcf	TRISA & 0x07F, SCL	; RA0, output, Serial Clock	bcf	TRISA & 0x07F, SDA	; RA1, output, Serial Data	bcf	TRISB & 0x07F, RPulse	; RB0 is Output (Right)	bcf	TRISB & 0x07F, LPulse	; RB1 is Output (Left)	bcf	STATUS, RP0	movlw	61			;  Set up the Timer - wait 20 msec	movwf	TMR0	; Initialize Real Time Clock	clrf	RTC; Set servo pulses to neutral	movlw	125			; right pulse width = 1.5 msec	call	set_right_pulse	movlw	125			; left pulse width = 1.5 msec	call	set_left_pulse;	movlw	1;	movwf	debug_buf; Start interrupts	movlw	0x0A0			; Set up Interrupt to run from TMR0	movwf	INTCON; Press Front bumper to start motionwait_bump	btfsc	PORTB, FBump	goto wait_bump;	movlw	2;	movwf	debug_buf	movlw	12	call	delay; Set servo speeds & directions according to state bumper switches;  -> "forward" is default direction of motionwait_bump2;	movlw	9;	movwf	debug_buf	btfss	PORTB, FBump		; Check whisker & bumper switches	goto	front_bump	btfss	PORTB, RWhisk	goto	right_bump	btfss	PORTB, LWhisk	goto	left_bumpforward	movlw	0			; right pulse width = 1.00 msec	call	set_right_pulse	movlw	250			; left pulse width = 2.00 msec	call	set_left_pulse	goto	wait_bump2		front_bump;	movlw	3;	movwf	debug_buf	call	backup	btfss	RTC, 0			; Use RTC as random coin flip:	goto	goleft			; If RTC is even, do left turn	call	turnright			; else, do right turn	goto	forwardgoleft	call	turnleft	goto	forward		right_bump;	movlw	4;	movwf	debug_buf	call	backup	call	turnleft	goto	forwardleft_bump;	movlw	5;	movwf	debug_buf	call	backup	call	turnright	goto	forwardturnright;	movlw	6;	movwf	debug_buf	movlw	250			; right pulse width = 2.00 msec	call	set_right_pulse	movlw	250			; left pulse width = 2.00 msec	call	set_left_pulse	movlw	12	call	delay	return	turnleft;	movlw	7;	movwf	debug_buf	movlw	0			; right pulse width = 1.00 msec	call	set_right_pulse	movlw	0			; left pulse width = 1.00 msec	call	set_left_pulse	movlw	12	call	delay	returnbackup;	movlw	8;	movwf	debug_buf	movlw	250			; right pulse width = 2.00 msec	call	set_right_pulse	movlw	0			; left pulse width = 1.00 msec	call	set_left_pulse	movlw	12	call	delay	return	set_right_pulse	sublw	250			; Convert to Msec_Cnt countdown format	btfss	STATUS, C		; Is right pulse > 250 ?	movlw	250			; yes, limit to 250	movwf	right			; Set the pulse countdown	returnset_left_pulse	sublw	250			; Convert to Msec_Cnt countdown format	btfss	STATUS, C		; Is left pulse > 250 ?	movlw	250			; yes, limit to 250	movwf	left			; Set the pulse countdown	return;; - Delay - delays "w" 20 msec ticks;delay	addwf	RTC, w			; Delay w * 20 msec	movwf	delay_countd1	movf	RTC, w	subwf	delay_count, w	btfsc	STATUS, Z		; RTC = delay count?	return				; yes, delay is done	goto	d1			; no, continue delay;; - Debug routine - 2-wire synchronous serial output of data in debug_buf;debug	movf	debug_buf, w		; Shift debug_buf out to shift 					; register	movwf	temp	movlw	8			; # of bits to shift	movwf	bit_countShift_out	rlf	temp			; Shift msb into C	btfsc	STATUS, C		; If bit set	goto	Set_bit			; Then set SDA bitClr_bit	bcf	PORTA, SDA		; Else clear SDA data bit	goto	Clock_outSet_bit	bsf	PORTA, SDA		 Clock_out				; Toggle SCL clock bit	bcf	PORTA, SCL	bsf	PORTA, SCL	decfsz	bit_count		; All bits shifted?	goto	Shift_out			; No, go shift next bit	return end
