;-----------------------------------------------------------------
; Name:			sweep.asm
; Description:	Sweeps the servo back and forth for range finder
;				

; Author:		Max Koessick
; Class:		EEL5666C, Intelligent Machine Design Lab
; Date:			June 6, 2003
; Revision		1.a
; Changes to Date:
;				6/6/03 First Revision
;				8/11/03 Successful integration of External Interrupts 
;						servo control.

; General Info:
; Servo period is 20ms; i.e. the servo is sent a 1.0-2.0ms pulse 
; 	every 20 ms.  Using a clock speed of 16 MHz/8(prescaler), the 
;	new clock is equal to 500ns.  A TOP value must be defined in order 
;	to create the proper PWM.  The value for TOP is (20ms/2)/500ns.  The TCNT1 
;	register counts to this value and then back down again.  A compare 
;	match is made on both the upcount and downcount.  These 2 compare
;	matches generate the PWM.

;	Formulas:
;		(20ms/2)/500ns=0x4E20=TOP
;			*TCNT counts up to TOP and back down again, so desired 
;			 period=TOP*2
;			*NEUTRAL (1.5ms)--Compare match made twice while upcounting and
;			 downcounting.  Needed period = .75ms.
;			 .75ms/500ns=0x5DC clocks.  TOP - 0x5DC = OCR1A
;			 Experiment with this number to get the servo in *exact* neutral
;			 postion
;			*+90 (2.0ms) Similiar to above
;			*-90 (1.0ms) Ditto
			
;-----------------------------------------------------------------

.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 $0500
.dseg								; Data Segment

OCRAT:	.BYTE 2						; Reserve memory space for Output Compare
									;   Register A (Target)
; In order to use the 2 register commands, SBIW and ADIW, we will use the XL and
; XH registers to temporarily hold the value of the Output Compare Register A (Target).
; With a few extra commands we can store this value to memory freeing up the X pointer.


.cseg								; Code Segment

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

.def		mpr		=r16			; defines multipurpose register
.def		mpr2	=r17			; multipurpose register 2
.def		delay	=r18			; servo direction register (values 1-3)
.def		delay2	=r19			; General Purpose Delay Register
.def		delay3	=r20			; General Purpose Delay Register

; Equate statements

.equ		L90H 	= 	$4d		; Left 90 degrees upper byte
.equ		L90L	=	$00
.equ		NH		= 	$48			; Neutral Position High Byte
.equ		NL		=	$44
.equ		R90H 	= 	$45			; Right 90 deg. High Byte
.equ		R90L	=	$aa
.equ		TOPH	=	$4e			; Top High Byte
.equ		TOPL	=	$20
.equ		ZERO	= 	$00
.equ		ONEs	=	$FF


;-----------------------------------------------------------------
; 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 	DDRC,mpr
	out		PORTC,mpr	

	clr 	mpr						; clear all bits
	out 	DDRD,mpr				; Set 'D' as input


;-----Enabling Interrupts-----------------------------------------

;Clear INT0 and INT1 in EIMSK register
	in		mpr,EIMSK
	cbr		mpr,(0<<INT0)+(0<<INT1)+(0<<INT2) ; Disable int 0,1,2 to set level control
	out		EIMSK,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
	in		mpr,EIFR
	sbr     mpr, (1<<INTF0)+(1<<INTF1)+(1<<INTF2);Set interrupt flag in EIFR
	out		EIFR,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 1A-C----------------------------------------

	ldi		mpr,0b11111100			; Set Timer1 to PWM mode 8
	out		TCCR1A,mpr

	ldi		mpr,0b00010010			; Prescaler = 8
	out		TCCR1B,mpr

	ldi		mpr,TOPH
	ldi		mpr2,TOPL
	out		ICR1H,mpr
	out		ICR1L,mpr2				; Set Top Value 
									; Period = 20ms
;Enable global interrupts

	sei

mainloop:

	rjmp mainloop					;loop here forever.

;-----------------------------------------------------------------
;
; Int0 vector - 
;				

IntV0:

	push	XL
	push	XH
	push	delay
	push 	delay2
	ldi		mpr,$f0					; Turn on LEDs for random test pattern
	out		PORTC,mpr				;  at begin in interrupt

	ldi		XH,R90H					; Left 90 degrees High Byte
	ldi		XL,R90L					; Left 90 Degrees Low Byte

ADDCycle:

	sts		low(OCRAT),XL			; store OCRA to SRAM to free X pointer
	sts		high(OCRAT),XH

	out		OCR1AH,XH				; Store value to Output Compare Register 1A
	out		OCR1AL,XL				; Set SPEEDservo = Full Left

	ldi		delay2,$10				; $40 good value for slow ramp (i.e. speed control)
DELAYLOAD:							; $10 good value for fast sweep (i.e. sonar?)
	ldi		delay,$FF				

ADDCYCLEDELAY:
	dec		DELAY
	brne	ADDCYCLEDELAY
	dec 	DELAY2
	brne	DELAYLOAD

	adiw	XH:XL,1					; Add 1 from the register pair that
									;  represents OCRA (Target)
	cpi 	XL,L90L					; Compare to the low byte, R90L
	brne 	Addcycle				; Branch if not equal
	cpi 	XH,L90H					; Compare to the High byte, R90H
	brne 	Addcycle				; Branch if not equal
	
SUBCycle:

	out		OCR1AH,XH				; Store value to Output Compare Register 1A
	out		OCR1AL,XL				; Set SPEEDservo = Full Left

	ldi		delay2,$10				; $40 good value for slow ramp (i.e. speed control)
DELAYLOAD2:							; $10 good value for fast sweep (i.e. sonar?)
	ldi		delay,$FF				

SUBCYCLEDELAY:
	dec		DELAY
	brne	SUBCYCLEDELAY
	dec 	DELAY2
	brne	DELAYLOAD2

	sbiw	XH:XL,1					; Subtract 1 from the register pair that
									;  represents OCRA (Target)
	cpi 	XL,R90L					; Compare to the low byte, R90L
	brne 	SUBcycle				; Branch if not equal
	cpi 	XH,R90H					; Compare to the High byte, R90H
	brne 	SUBcycle				; Branch if not equal

	ldi		mpr,$0f					; Load test pattern to LEDs
	out		portc,mpr				;  to signify end of interrupt
	pop		delay2
	pop 	delay
	pop 	XH
	pop		XL
	reti
;-----------------------------------------------------------------
;
; Int1 vector - set equate equal to desired servo direction
;				INT0 = 0 degrees or ServoDir = 2

IntV1:

	ldi		mpr,$00					; Clear the TCNT register
	ldi		mpr2,$00
	out		TCNT1H,mpr
	out		TCNT1L,mpr2	

	ldi		mpr,NL
	ldi		mpr2,NH
	out		OCR1AH,mpr2
	out		OCR1AL,mpr

	reti
;-----------------------------------------------------------------
;
; Int1 vector - set equate equal to desired servo direction
;				INT0 = +90 degrees or ServoDir = 3

IntV2:

	ldi		mpr,R90H
	ldi		mpr2,R90L
	out		OCR1AH,mpr
	out		OCR1AL,mpr2
	reti




