.nolist
.include "C:\Program Files\Atmel\AVR Tools\AvrAssembler\Appnotes\m163def.inc"

;register definitions
.def capturel=r1
.def captureh=r2
.def bump=r3
.def leftIR=r4
.def rightIR=r5
.def rearIR=r6
.def IRleft=r7
.def IRright=r8
.def timeout=r9

.def temp=r16
.def temp1=r17
.def temp2=r18
.def temp3=r19
.def subret=r20
.def subarg0=r21
.def rwsl=r22
.def rwsh=r23
.def lwsl=r24
.def lwsh=r25

;equates
.equ lwfh=$06
.equ lwfl=$27
.equ rwfh=$02
.equ rwfl=$A3

.equ lwrh=$02
.equ lwrl=$A3
.equ rwrh=$06
.equ rwrl=$27

.equ lwph=$04
.equ lwpl=$6f
.equ rwph=$04
.equ rwpl=$6f

;macros
.listmac
.macro outi @0,@1
	ldi temp,@1
	out @0,temp
			.endmacro

.list


;interrupt vectors
.org 0
	jmp	main
.org $00c
	jmp T1OCA_ISR
.org $00e
	jmp T1OCB_ISR
.org $012
	jmp T0_OVR_ISR





;main routine
.org $24
main:                
	outi SPH,high(ramend)		; Stack Pointer Setup 			
	outi SPL,low(ramend)

;set data direction
	outi ddrc,$ff
	outi ddrb,$05
	outi DDRD,0b10110000		;pins7,5,4 as outputs
	cbi portb,2				;clear portb pin 2


;init T1
	outi TCCR1B,0b10000010		;prescaler clk/8
								;enable input noise filter
								;falling edge input capture
	outi TIMSK,0b00011000		;enable interrupt for T1OCA and T1OCB

;set Timer 0 prescaler to 1024
	outi TCCR0,5

;init ADC
	sbi admux,adlar			;left adjust result

	sbi adcsr,aden			;enable ADC

;wait for user to start by pressing any bump switch
waitbump:
	outi admux,$20
	sbi	adcsr,adsc			;start A2D conversion
wait4bump:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp wait4bump
	in temp,adch			;move adc data to temp
	andi temp,$E0
	breq waitbump			;if zero then loop

	sei

















spinInit:
;init values for wheels forward
	ldi rwsh,rwfh
	ldi rwsl,rwfl
	ldi lwsh,lwrh
	ldi lwsl,lwrl

;start wheel and enable interrupts
	outi TCCR1A,0b01010000		;set to toggle pins T1OCA and T1OCB

;set T0 OVR interrupt
	outi TIMSK,0b00011001		;set Timer 0 Overflow interrupt enable

	clr timeout

	;user feedback
	ldi temp,$1
	com temp
	out portc,temp


spin:

	call wait100ms

;get a2d for IRdetLeft
	outi admux,$24
	sbi	adcsr,adsc			;start A2D conversion
waitADC4:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp waitADC4
	in IRleft,adch				;move adc data to bump

;get a2d for IRdetRight
	outi admux,$25
	sbi	adcsr,adsc			;start A2D conversion
waitADC5:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp waitADC5
	in IRright,adch				;move adc data to bump


;if timeout goto wanderInit
	ldi temp,$F0
	cp timeout,temp
	brlo testIR

	outi TIMSK,0b00011000		;disable T0_OVR
	jmp wanderInit

testIR:
;if IRright && IRleft < $70, jmp mainloop
	ldi temp,$70
	cp IRleft,temp
	brsh greaterThanSixty

	cp IRright,temp
	brlo spin

greaterThanSixty:

;if IRleft == IRright, stop turning
	cp IRleft,IRright
	breq stop

;if IRright-6 < IRleft < IRright+6, stop turning
	ldi temp,$6				
	sub IRright,temp		;IRright - 6
	cp IRright,IRleft
	brsh alterDirection

	add IRright,temp		;IRright-6+6+6=IRright+6
	add IRright,temp
	cp IRleft,IRright
	brsh alterDirection

;stop wheels and wait .5 sec and jump to toDistance
stop:
	ldi rwsh,rwph
	ldi rwsl,rwpl
	ldi lwsh,lwph
	ldi lwsl,lwpl
	call wait500ms				;wait 500ms
	outi TCCR1A,0b00000000		;stop wheels
	jmp toDistance

;turn towards the higher IR reading
alterDirection:
	cp IRright,IRleft
	brsh turnRight

;turn left
	outi TCCR1A,0b01010000		;start wheels
	ldi rwsh,rwfh+$100
	ldi rwsl,rwfl+$100
	ldi lwsh,lwrh+$100
	ldi lwsl,lwrl+$100
	jmp spin

;turn right
turnRight:
	outi TCCR1A,0b01010000		;start wheels
	ldi rwsh,rwrh-$100
	ldi rwsl,rwrl-$100
	ldi lwsh,lwfh-$100
	ldi lwsl,lwfl-$100
	jmp spin



















	
toDistance:
	;user feedback
	ldi temp,$3
	com temp
	out portc,temp

	outi TIMSK,0b00011000		;disable T0_OVR
	sbi PORTB,0				;initialize sonar ping

	ldi temp,20				;wait 10us
wait10us:
	dec temp
	brpl wait10us

	ldi temp,0b00100100		;clear overflow flag 1
	out TIFR,temp			;clear IC flag
							

	in capturel,TCNT1L		;store timer1 value
	in captureh,TCNT1H

	cbi PORTB,0				;start sonar ping

;while sonar pinging, check for room behind robot
;get a2d for rearIR
	outi admux,$23
	sbi	adcsr,adsc			;start A2D conversion
waitADC3:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp waitADC3
	in rearIR,adch				;move adc data to bump

;wait IC flag
chk_ic_flg:
	in temp,TIFR
	sbrs temp,ICF1			;wait for IC flag
	rjmp chk_ic_flg

	in temp,ICR1l			;load capture registers
	in temp1,ICR1h

	cp temp,capturel		;test original time with new capture time
	cpc temp1,captureh

	brsh captSub			;if new>original branch
	
;$FFFF - original time + new time
	ldi temp2,$ff			;use temp3:temp2 for accumulator
	ldi temp3,$ff

	sub temp2,capturel		;$FFFF - original time
	sbc temp3,captureh

	add temp2,temp			;      + new time
	adc temp3,temp1

	mov capturel,temp2			;move acculmulator back to captureh/capturel
	mov captureh,temp3

	jmp wait10m

captSub:	
	sub temp,capturel		;subtract 16bit time
	sbc temp1,captureh

	mov capturel,temp			;move acculmulator back to captureh/capturel
	mov captureh,temp1


wait10m:
	ldi temp,$4F			;wait 10ms
wait10ms:
	ldi temp1,$FF
wait10msa:
	dec temp1
	brne wait10msa
	dec temp
	brne wait10ms

;test if captureh < $1D && >1B
	mov temp,captureh
	cpi temp,$1C
	brlo reverse

	cpi temp,$1D
	brsh forward

;stop wheels and shoot
	ldi rwsh,rwph
	ldi rwsl,rwpl
	ldi lwsh,lwph
	ldi lwsl,lwpl

	call wait100ms

	outi TCCR1A,0b00000000		;stop wheels

	sbi portb,2				;set portb pin 2

;user feedback release
	ldi temp,$4
	com temp
	out portc,temp


wait1000ms:
	ldi temp,$10
wait1000msa:
	ldi temp1,$FF
wait1000msb:
	ldi temp2,$FF
wait1000msc:
	dec temp2
	brne wait1000msc
	dec temp1
	brne wait1000msb
	dec temp
	brne wait1000msa

	cbi portb,2				;clear portb pin 2

	jmp detectInit

reverse:
	outi TCCR1A,0b01010000		;start wheels
	ldi rwsh,rwrh
	ldi rwsl,rwrl
	ldi lwsh,lwrh
	ldi lwsl,lwrl

;if no more room in rear goto wanderInit
	mov temp,rearIR
	cpi temp,$70
	brsh wanderInit

	jmp toDistance

forward:
	outi TCCR1A,0b01010000		;start wheels
	ldi rwsh,rwfh
	ldi rwsl,rwfl
	ldi lwsh,lwfh
	ldi lwsl,lwfl
	jmp toDistance





















wanderInit:
;go forward
	ldi rwsh,rwfh
	ldi rwsl,rwfl
	ldi lwsh,lwfh
	ldi lwsl,lwfl

	outi TCCR1A,0b01010000		;set to toggle pins T1OCA and T1OCB

;user feedback
	ldi temp,$2
	com temp
	out portc,temp

wander:
;get a2d for bump
	outi admux,$20
	sbi	adcsr,adsc			;start A2D conversion
waitADC0:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp waitADC0
	in bump,adch				;move adc data to bump

;get a2d for left IR
	outi admux,$21
	sbi	adcsr,adsc			;start A2D conversion
waitADC1:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp waitADC1
	in leftIR,adch				;move adc data to leftIR

;get a2d for right IR
	outi admux,$22
	sbi	adcsr,adsc			;start A2D conversion	
waitADC2:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp waitADC2
	in rightIR,adch				;move adc data to rightIR

;get a2d for IRdetLeft
	outi admux,$24
	sbi	adcsr,adsc			;start A2D conversion
waitADC4a:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp waitADC4a
	in IRleft,adch				;move adc data to bump

;get a2d for IRdetRight
	outi admux,$25
	sbi	adcsr,adsc			;start A2D conversion
waitADC5a:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp waitADC5a
	in IRright,adch				;move adc data to bump


;if castle found, goto spinInit
	ldi temp,$70
	cp IRleft,temp
	brsh jumpSpin

	cp IRright,temp
	brlo testbump

jumpSpin:
	jmp spinInit


;test sensors to determine obstance avoidance
testbump:
;if $40 < bump > $60, then reverse and turn randomly
	mov temp,bump
	cpi temp,$40			;is bump < $40?
	brlo testLeftAndRightIR
	cpi temp,$60			;is bump > $60
	brsh testLeftAndRightIR

	call reverseAndRandomTurn
	jmp wander

testLeftAndRightIR:
;if LeftIR > $60 and RightIR > $60, then reverse and turn randomly
	mov temp,LeftIR
	cpi temp,$60			;is LeftIR < $60
	brlo testRightIR
	mov temp,RightIR
	cpi temp,$60			;is RightIR < $60
	brlo testRightIR

	call reverseAndRandomTurn
	jmp wander

testRightIR:
;if RightIR > $60, then turn left
	mov temp,RightIR
	cpi temp,$60			;is RightIR < $60
	brlo testLeftIR

	call turnLeftMethod
	jmp wander

testLeftIR:
;if LeftIR > $60, then turn right
	mov temp,LeftIR
	cpi temp,$60			;is LeftIR < $60
	brlo default

	call turnRightMethod
	jmp wander

default:
;no sensor readings, wait ~100ms
	call wait100ms
	jmp wander












DetectInit:
;set T0 OVR interrupt
	outi TIMSK,0b00011001		;set Timer 0 Overflow interrupt enable

	clr timeout

	;user feedback
	ldi temp,$1
	com temp
	out portc,temp

Detect:
	call wait100ms

;if timeout goto ram
	ldi temp,$50
	cp timeout,temp
	brlo againDetect

	outi TIMSK,0b00011000		;disable T0_OVR
	jmp ram

againDetect:
	;get a2d for IRdetLeft
	outi admux,$24
	sbi	adcsr,adsc			;start A2D conversion
waitADC4b:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp waitADC4b
	in IRleft,adch				;move adc data to bump

;get a2d for IRdetRight
	outi admux,$25
	sbi	adcsr,adsc			;start A2D conversion
waitADC5b:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp waitADC5b
	in IRright,adch				;move adc data to bump

;if IRright or IRleft > $58, retreat
	ldi temp,$58
	cp IRright,temp
	brlo retreat

	cp IRleft,temp
	brlo retreat

	jmp detect









retreat:
;init values for wheels reverse
	ldi rwsh,rwrh
	ldi rwsl,rwrl
	ldi lwsh,lwrh
	ldi lwsl,lwrl

;start wheel and enable interrupts
	outi TCCR1A,0b01010000		;set to toggle pins T1OCA and T1OCB

retreata:
	call wait100ms


;get a2d for rearIR
	outi admux,$23
	sbi	adcsr,adsc			;start A2D conversion
waitADC3a:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp waitADC3a
	in rearIR,adch				;move adc data to bump

;if rearIR > 60, stop
	ldi temp,$60
	cp rearIR,temp
	brlo retreata

	jmp fullspeed















ram:
;init values for wheels reverse
	ldi rwsh,rwfh
	ldi rwsl,rwfl
	ldi lwsh,lwfh
	ldi lwsl,lwfl

;start wheel and enable interrupts
	outi TCCR1A,0b01010000		;set to toggle pins T1OCA and T1OCB

rama:
	call wait100ms


;get a2d for left IR
	outi admux,$21
	sbi	adcsr,adsc			;start A2D conversion
waitADC1a:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp waitADC1a
	in leftIR,adch				;move adc data to leftIR

;get a2d for right IR
	outi admux,$22
	sbi	adcsr,adsc			;start A2D conversion	
waitADC2a:
	sbis adcsr,adif			;wait until A2D conversion complete
	jmp waitADC2a
	in rightIR,adch				;move adc data to rightIR

;if rightIR or leftIR > $60, goto fullspeed
	ldi temp,$60
	cp rightIR,temp
	brsh fullspeed

	cp leftIR,temp
	brlo rama

fullspeed:
	call wait100ms


	outi TCCR1A,0b00000000		;set to toggle pins T1OCA and T1OCB

	jmp main










reverseAndRandomTurn:
	ldi rwsh,rwrh
	ldi rwsl,rwrl
	ldi lwsh,lwrh
	ldi lwsl,lwrl

	ldi temp,$20
wait2sa:
	ldi temp1,$FF
wait2sb:
	ldi temp2,$FF
wait2sc:
	dec temp2
	brne wait2sc
	dec temp1
	brne wait2sb
	dec temp
	brne wait2sa

	in temp,TCNT1L
	andi temp,$01
	brne goRight
	call turnLeftMethod
	call turnLeftMethod
	ret
goRight:
	call turnRightMethod
	call turnRightMethod
	ret




turnLeftMethod:
	ldi rwsh,rwfh
	ldi rwsl,rwfl
	ldi lwsh,lwrh
	ldi lwsl,lwrl

	ldi temp,$19
wait1sa:
	ldi temp1,$FF
wait1sb:
	ldi temp2,$FF
wait1sc:
	dec temp2
	brne wait1sc
	dec temp1
	brne wait1sb
	dec temp
	brne wait1sa

	ldi rwsh,rwfh
	ldi rwsl,rwfl
	ldi lwsh,lwfh
	ldi lwsl,lwfl
	ret




turnRightMethod:
	ldi rwsh,rwrh
	ldi rwsl,rwrl
	ldi lwsh,lwfh
	ldi lwsl,lwfl

	ldi temp,$19
wait1sd:
	ldi temp1,$FF
wait1se:
	ldi temp2,$FF
wait1sf:
	dec temp2
	brne wait1sf
	dec temp1
	brne wait1se
	dec temp
	brne wait1sd

	ldi rwsh,rwfh
	ldi rwsl,rwfl
	ldi lwsh,lwfh
	ldi lwsl,lwfl
	ret







;method
;wait 100ms
wait100ms:
	ldi temp,$03
wait100msa:
	ldi temp1,$FF
wait100msb:
	ldi temp2,$FF
wait100msc:
	dec temp2
	brne wait100msc
	dec temp1
	brne wait100msb
	dec temp
	brne wait100msa

	ret

;method
;wait 500ms
wait500ms:
	ldi temp,$38
wait500msa:
	ldi temp1,$FF
wait500msb:
	ldi temp2,$FF
wait500msc:
	dec temp2
	brne wait500msc
	dec temp1
	brne wait500msb
	dec temp
	brne wait500msa

	ret



;Interrupt Service Routine to place a correct frequency on the port D pin5
;the period is %3A98 cycles at clk/8
;The duty cycle is determined by rwsh and rwsl
;high time is currentTime+rwsh:rwsl
;low time is currectTime-$3a98+rwsh:rwsl
T1OCA_ISR:
	push temp				;push temp to save contents
	in temp,SREG			;push SREG to save contents
	push temp
	push temp1
	push temp2

	in temp,PIND			;check value of toggle pin
	andi temp,0b00100000
	breq pd5low				;branch if port D pin 5 is low

	in temp,OCR1AL			;OCR1A = OCR1A + right wheel speed
	in temp1,OCR1AH
	add temp,rwsl
	adc temp1,rwsh
	out OCR1AH,temp1			;must write high byte first
	out OCR1AL,temp
	jmp END_T1OCA_ISR

pd5low:						;pin 5 is low
							;OCR1A = OCR1A + 15000 - right wheel speed
	in temp,OCR1AL
	in temp1,OCR1AH

	ldi temp2,$98
	add temp,temp2
	ldi temp2,$3A
	adc temp1,temp2

	sub temp,rwsl
	sbc temp1,rwsh

	out OCR1AH,temp1			;must write high byte first
	out OCR1AL,temp


END_T1OCA_ISR:
	pop temp2
	pop temp1
	pop temp				;restore register values
	out SREG,temp
	pop temp

	reti					;return from interrupt




;Interrupt Service Routine to place a correct frequency on the port D pin4
;the period is %3A98 cycles at clk/8
;The duty cycle is determined by lwsh and lwsl
;high time is currentTime+lwsh:lwsl
;low time is currectTime-$3a98+lwsh:lwsl
T1OCB_ISR:
	push temp				;push temp to save contents
	in temp,SREG			;push SREG to save contents
	push temp
	push temp1
	push temp2

	in temp,PIND			;check value of toggle pin
	andi temp,0b00010000
	breq pd4low				;branch if port D pin 5 is low

	in temp,OCR1BL			;OCR1A = OCR1A + right wheel speed
	in temp1,OCR1BH
	add temp,lwsl
	adc temp1,lwsh
	out OCR1BH,temp1			;must write high byte first
	out OCR1BL,temp
	jmp END_T1OCB_ISR

pd4low:						;pin 5 is low
							;OCR1A = OCR1A + 15000 - right wheel speed
	in temp,OCR1BL
	in temp1,OCR1BH

	ldi temp2,$98
	add temp,temp2
	ldi temp2,$3A
	adc temp1,temp2

	sub temp,lwsl
	sbc temp1,lwsh

	out OCR1BH,temp1			;must write high byte first
	out OCR1BL,temp


END_T1OCB_ISR:			
	pop temp2				;restore register values
	pop temp1
	pop temp				
	out SREG,temp
	pop temp

	reti					;return from interrupt



;interrupt service routine to overflow every so often
T0_OVR_ISR:
	push temp				;push temp to save contents
	in temp,SREG			;push SREG to save contents
	push temp

	inc timeout

	pop temp				;restore register values
	out SREG,temp
	pop temp

	reti					;return from interrupt

