;
; Define Switch Input variables
;
.equ	RDSWI	= PINB
.set	PORTB_INIT = PORTB_INIT + (1<<R_Bump) + (1<<L_Bump) + (1<<F_Floor) + (1<<R_Floor)

.equ	R_Bump	= PB7		; port bits our switches are 
.equ	L_Bump	= PB6		; connected to.
.equ	F_Floor	= PB5
.equ	R_Floor	= PB4

;
; Bump detect process.  Called continuously when no bumps
; detected.  Otherwize, sets motor commands and sleeps for a 
; while.  All actions are two part: back up a bit, then turn a bit.
;
	.cseg

bump:
	sbrc	B_State, B_Bump
	rjmp	Br_turn

	clr	B_State			; Clear any previous drive action

	sbic	RDSWI, R_Bump		; if (R_Bump) (low = active)
	rjmp	Br2			; {

	ldi	B_State, Bk_Fast | 1<<B_Bump | 1<<B_Left
	ldi	E_Temp, 3
	mov	B_Delay, E_Temp		;     B_Delay(3/10 second)
Br2:					; }
	sbic	RDSWI, L_Bump		; if (L_Bump)
	rjmp	Br3

	ldi	E_Temp, 3
	mov	B_Delay, E_Temp		;     B_Delay = 3/10 second;
	sbrc	B_State, B_Bump		;     If (Both bumpers)
	ldi	E_Temp, 6
	mov	B_Delay, E_Temp		;     B_Delay(6/10 second)

	ldi	B_State, Bk_Fast | 1<<B_Bump | 1<<B_Right
Br3:
	sbic	RDSWI, F_Floor		; if (F_Floor)
	rjmp	Br4			; {

	ldi	B_State, Bk_Fast | 1<<B_Bump | 1<<B_Right
	ldi	E_Temp, 4
	mov	B_Delay, E_Temp		;     B_Delay(3/10 second)
Br4:					; }
	rjmp	br_exit
Br_Turn:
	sbrc	B_State, B_Left		; All actions that get us here
	ldi	B_State, Turn_Left	; involve a turn
	sbrc	B_State, B_Right	; reset all flags so next pass is
	ldi	B_State, Turn_Right	; just scanning for switch closures
	ldi	E_Temp, 3
	mov	B_Delay, E_Temp		;     B_Delay(3/10 second)
Br_Exit:
	ret

;
; Eye historical data EEPROM DATA!!!
;
	.eseg
Max_L:		.db	0xFF
Min_L:		.db	0x00
Max_R:		.db	0xFF
Min_R:		.db	0x00
Eye_Gap:	.db	0x04	; Difference between eyes to have an effect
Eye_Mid:	.db	0x20	; Avoid/seek threshold

	.cseg
;
; No state info here: Simply scan the eyes and set drive state if delta
; greater than our minimum.
;
Eyes:
	ldi	E_Temp, Eye_Gap
	out	EEAR, E_Temp
	sbi	EECR, EERE
	in	E_Delay, EEDR		; DeadBand -> E_Delay (temp)

	mov	E_Temp, Right_Eye
	sub	E_Temp, Left_Eye
	brsh	ey1
	neg	E_Temp
ey1:
	cp	E_Temp, E_Delay		; if abs(difference) < Eye_Gap
	brsh	ey2
	clr	E_State			;    then no action needed,  
	rjmp	E_Exit			; (Sleep Eye_Gap and check again)
ey2:
	ldi	E_Temp, Eye_Mid
	out	EEAR, E_Temp
	sbi	EECR, EERE
	in	E_Temp, EEDR		; Get "mid point" of seek/avoid
				
	ldi	E_State, Arc_Left	; Pre load Arc_left

	cp	Right_Eye, E_Temp	; if either eye "Bright" e.g. less
	brlo	Eye_Avoid
	cp	Left_Eye, E_Temp	; than the midpoint, then AVOID
	brlo	Eye_Avoid
Eye_Seek:
	cp	Right_Eye, Left_Eye	; 
	rjmp	foo
Eye_Avoid:
	cp	Left_Eye, Right_Eye	; 
foo:
	brlo	E_Exit
	ldi	E_State, Arc_Right	; Arc_right
E_Exit:
	ldi	E_Temp, 3		; Scan eyes 3 times a second
	mov	E_Delay, E_Temp
	ret
;
; Simple, but long, arbitration and motor drive control.
;
; First decide if "Cruise", Bump or Light Seek sets the motor drive
; then parse the motor drive and set the PWM accordingly.  The drive levels
; should come out of the EEPROM, but that would make for very long dull code.
; perhaps a macro is in order?
;
Arbitrate:
	sbrc	Flags, Ar_Disable	; debug hack to disable arbitrate
	ret
	
	ldi	E_Temp, Fd_Slow		; Assume Cruise

	tst	E_State			; override with eyes
	breq	ar1
	mov	E_Temp, E_State
ar1:
	tst	B_State
	breq	ar2
	mov	E_Temp, B_State		; Override with Bumper
ar2:
;
; Parse motor control stuff.
;
Drive:
	cbr	E_Temp, 0xF0		; get rid of state info, isolate drive

	cpi	E_Temp, Fd_Slow
	brne	dr3
	ldi	RightM, PWM_MED
	ldi	LeftM,  PWM_MED
dr3:
	cpi	E_Temp, Fd_Fast
	brne	dr4
	ldi	RightM, PWM_FAST
	ldi	LeftM,  PWM_FAST
dr4:
	cpi	E_Temp, Bk_Slow
	brne	dr5
	ldi	RightM, -PWM_MED
	ldi	LeftM,  -PWM_MED
dr5:
	cpi	E_Temp, Bk_Fast
	brne	dr6
	ldi	RightM, -PWM_FAST
	ldi	LeftM,  -PWM_FAST
dr6:
	cpi	E_Temp, Turn_Right
	brne	dr7
	ldi	RightM, PWM_STOP
	ldi	LeftM,  PWM_MED
dr7:
	cpi	E_Temp, Turn_Left
	brne	dr8
	ldi	RightM, PWM_MED
	ldi	LeftM,  PWM_STOP
dr8:
	cpi	E_Temp, Arc_RIGHT
	brne	dr9
	ldi	RightM, PWM_MED
	ldi	LeftM,  PWM_FAST
dr9:
	cpi	E_Temp, Arc_LEFT
	brne	dr10
	ldi	RightM, PWM_FAST
	ldi	LeftM,  PWM_MED
dr10:
	cpi	E_Temp, Dr_Stop
	brne	dr11
	ldi	RightM, PWM_STOP
	ldi	LeftM,  PWM_STOP
dr11:
dr_Exit:
	ret				;    return;