;-----------------------------------------------------------------
; Name:			ServoTest.asm
; Description:	Tests external interrupt functionality.
; 				When a respective switch is pressed, generate an output compart
;				to control a servo in the -90,0,90 directions
; Author:		Max Koessick
; Class:		EEL5666C, Intelligent Machine Design Lab
; Date:			June 6, 2003
; Revision		1.a
; Changes to Date:
;				6/5/03 First Revision
			
;-----------------------------------------------------------------

.include "m128def.inc"

; Interrupt service vectors

.org $0000
	rjmp Reset						; Reset vector
.org INT0addr
	rjmp IntV0						; INT0 vector (ext. interrupt from pin D0)	
.org INT1addr	
	rjmp IntV1						; INT1 vector (ext. interrupt from pin D1)
.org INT2addr		
	rjmp IntV2						; INT2 vector (ext. interrupt from pin D2)
.org OC0addr						
	rjmp INTOC0						; OC0 Vector

	
;-----------------------------------------------------------------
; Register defines for main loop
;-----------------------------------------------------------------


.def		mpr=r16					; defines multipurpose register
.def		mpr2=r17				; multipurpose register 2
.def		ServoDir=r18				; servo direction register (values 1-3)


;-----------------------------------------------------------------
; Reset vector
;-----------------------------------------------------------------

Reset:
;-----Setting Stackpointer----------------------------------------
	ldi		MPR,low(RAMEND)			; Set stackptr to ram end
	out		SPL,MPR
	ldi 	MPR, high(RAMEND)
	out 	SPH, MPR

;-----Set Port Directions-----------------------------------------
	ser		mpr						; Set TEMP to $FF to...
	out		DDRB,mpr				; ...set data direction to "out"
	out		PORTB,mpr				; ...all lights off!

	clr 	mpr						; clear all bits
	out 	DDRD,mpr				; Set 'D' as input


;-----Enabling Interrupts-----------------------------------------

;Clear INT0 and INT1 in EIMSK register
	ldi		r30,EIMSK
	ld		mpr,Z
	cbr		mpr,(0<<INT0)+(0<<INT1)+(0<<INT2) ; Disable int 0,1,2 to set level control
	st		Z,mpr
;Set level sense control
	ldi		r30,EICRA
	ld		mpr,Z
	sbr 	mpr,(1<<ISC00)+(1<<ISC01)+(1<<ISC10)+(1<<ISC11)+(1<<ISC20)+(1<<ISC21)				
									; Set three pairs of interrupt control bits high
	st		Z,mpr
;Clear any interrupts pending in EIFR
	ldi		r30,EIFR
	ld		mpr,Z
	sbr     mpr,(1<<INTF0)+(1<<INTF1)+(1<<INTF2);Set interrupt flag in EIFR
	st		Z,mpr
;Set INT0 and INT1 in EIMSK register
	in		mpr,EIMSK
	sbr		mpr,(1<<INT0)+(1<<INT1)+(1<<INT2) ; int masks 0 and 1 set
	out		EIMSK,mpr

;-----Enable Output Compare----------------------------------------

	ldi		mpr,(1<<OCIE0)
	out		TIMSK,mpr				;Enable Output Compare 0

	ldi		r30,TCCR0
	ld		mpr,Z
	sbr		mpr,(1<<COM00)+(1<<WGM01)+(1<<CS02)+(1<<CS01)
	st		Z,mpr
;Sets Output Compare to:
;	CTC (Clear Timer on Compare) mode WGM01=1,WGM00=0
;   Prescaler = /256	CS02:0 = 0b110
;	Compare Output Mode = Toggle on Compare COM1:0 = 0b01

;	Move servo to Neutral position to start
	ldi r30,OCR0			; load address of output compare register 0 into Z
	ldi mpr,$16				; Load OCR0 (3.69MHz)/(256 Prescaler)=14.41 kHz; f=1/14.41 = 69.38us
	st	Z,mpr				; Want 0 degrees or OCO0 = 1.5ms; 1.5ms/69.38us = 14.4 or 
							; 22 clocks.  So load OCR0 with 22 or $16


;Enable global interrupts

	sei

mainloop:

	rjmp mainloop					;loop here forever.

;-----------------------------------------------------------------
;
; Int0 vector - set equate equal to desired servo direction
;				INT0 = -90 degrees or ServoDir = 1

IntV0:
	ldi		ServoDir,$01
	reti

;-----------------------------------------------------------------
;
; Int1 vector - set equate equal to desired servo direction
;				INT0 = 0 degrees or ServoDir = 2

IntV1:
	ldi		ServoDir,$02
	reti
;-----------------------------------------------------------------
;
; Int1 vector - set equate equal to desired servo direction
;				INT0 = +90 degrees or ServoDir = 3

IntV2:
	ldi		ServoDir,$03
	reti


;------------------------------------------------------------------
;Output Compare Interrupt

INTOC0:
	push mpr				; push mpr onto stack
	ldi r30,OCR0			; load address of output compare register 0 into Z


							; LEFT POSITION (-90 DEGREES)

	cpi ServoDir,$01		; check servo direction commnd register (1=-90 degrees)
	brne next				; if not, keep checking
	
	ldi mpr,$0F				; Load OCR0 (3.69MHz)/(256 Prescaler)=14.41 kHz; f=1/14.41 = 69.38us
	st	Z,mpr				; Want -90 degrees or OCO0 = 1.0ms; 1.0ms/69.38us = 14.4 or 
							; 15 clocks.  So load OCR0 with 15 or $0F
	rjmp EndINTOC0

Next: 						; NEUTRAL POSITION ( 0 DEGREES)
	cpi ServoDir,$02
	brne next2
	ldi mpr,$16				; Load OCR0 (3.69MHz)/(256 Prescaler)=14.41 kHz; f=1/14.41 = 69.38us
	st	Z,mpr				; Want 0 degrees or OCO0 = 1.5ms; 1.5ms/69.38us = 14.4 or 
							; 22 clocks.  So load OCR0 with 22 or $16
	rjmp EndINTOC0

Next2:						; RIGHT POSITION (+90 DEGREES)
	cpi ServoDir,$03
	brne EndINTOC0			; if not, BAD VALUE, do nothing and return to main program
	ldi mpr,$1C				; Load OCR0 (3.69MHz)/(256 Prescaler)=14.41 kHz; f=1/14.41 = 69.38us
	st	Z,mpr				; Want +90 degrees or OCO0 = 2.0ms; 2.0ms/69.38us = 14.4 or 
							; 28 clocks.  So load OCR0 with 28 or $1C
	rjmp EndINTOC0

EndINTOC0:
	pop mpr
	reti




