
;------------------------------------------
;Project Name:  Smoothing Function.asm
;Description:	Test Smoothing function
;Author:		Max Koessick
;Date;			July 26, 2003
;Revision:		1.0  ---

.nolist
.include "m323def.inc" 		; Default Include file for ATMega128
.list						; Do not include the "m323def.inc" in the .lst file

;Interrupt Service Vector Addresses

.org $0000
	rjmp RESET				; Reset Vector

;-------------------------------------------
;Register Definitions
;-------------------------------------------

.def mpr   		=r16		; Temporary Register
.def mpr2		=r17		; Old Speed Register

.def tmpspdL	=r5			; Temp Speed Register
.def tmpspdH	=r6

.equ k			=1			; smoothing number

.equ Nspeed		=$500
.equ Ospeed		=$501
.equ Cspeed	 	=$502

;Initialization

RESET:

;-----Setting Stackpointer----------------------------------------
	ldi		MPR,low(RAMEND)			; Set stackptr to ram end
	out		SPL,MPR
	ldi 	MPR, high(RAMEND)
	out 	SPH, MPR

;
	ldi		mpr,$e3					; Fictitous speeds desired
	sts		Nspeed,mpr
	ldi		mpr,$d4					; 	"		   "   current
	sts		Cspeed,mpr
; *Smoothing Function*  ->new = (k*old + new)/k+1

Start:
	lds		mpr,Cspeed				; Load current speed into mpr
	sts		Ospeed,mpr				; Transfers current speed to oldspeed
	lds		mpr,NSpeed
	mov		tmpspdL,mpr				; Move Desired speed to temp speed register

	ldi		mpr,k
	lds		mpr2,Ospeed
	mul		mpr2,mpr				; r0=low byte -> k*old
	add		tmpspdL,r0				; Add New Speed to k*old
	in		mpr,SREG
	andi	mpr,$01					; mask the carry bit
	add		mpr,r1
	mov		tmpspdH,mpr



;***** Divide Two Unsigned 16-Bit Numbers (tmpspd/k+1)
	mov		dd16uL,tmpspdL
	mov		dd16uH,tmpspdH
	ldi		dv16uL,low(k+1)
	ldi		dv16uH,high(k+1)
	rcall	div16u		;result:	$0000 (0)
				;remainder:	$c350 (50,000)

mainloop:
	lds		mpr,Cspeed
	lds		mpr2,Nspeed
	cp		mpr2,mpr
	brne 	Start
	rjmp 	Mainloop
;***************************************************************************
;* From Atmel AVR200 Application Note
;* "div16u" - 16/16 Bit Unsigned Division
;*
;* This subroutine divides the two 16-bit numbers 
;* "dd8uH:dd8uL" (dividend) and "dv16uH:dv16uL" (divisor). 
;* The result is placed in "dres16uH:dres16uL" and the remainder in
;* "drem16uH:drem16uL".
;*  
;* Number of words	:19
;* Number of cycles	:235/251 (Min/Max)
;* Low registers used	:2 (drem16uL,drem16uH)
;* High registers used  :5 (dres16uL/dd16uL,dres16uH/dd16uH,dv16uL,dv16uH,
;*			    dcnt16u)
;*
;***************************************************************************

;***** Subroutine Register Variables
div16u:
	push 	r14
	push 	r15
	push 	r16
	push 	r17
	push 	r18
	push 	r19
	push 	r20	

;****Subroutine Equates****
.def drem16uL	=r14
.def drem16uH	=r15
.def dres16uL	=r16
.def dres16uH	=r17
.def dd16uL		=r16
.def dd16uH		=r17
.def dv16uL		=r18
.def dv16uH		=r19
.def dcnt16u	=r20


	clr		drem16uL			;clear remainder Low byte
	sub		drem16uH,drem16uH	;clear remainder High byte and carry
	ldi		dcnt16u,17			;init loop counter
d16u_1:	
	rol		dd16uL				;shift left dividend
	rol		dd16uH
	dec		dcnt16u				;decrement counter
	brne	d16u_2			;if done
	
	sts		Cspeed,dres16uL				; Transfer result to current speed register
	
	pop 	r20
	pop 	r19
	pop 	r18
	pop 	r17
	pop 	r16
	pop 	r15
	pop 	r14
	
	ret			;    return
d16u_2:	
	rol		drem16uL	;shift dividend into remainder
	rol		drem16uH
	sub		drem16uL,dv16uL	;remainder = remainder - divisor
	sbc		drem16uH,dv16uH	;
	brcc	d16u_3		;if result negative
	add		drem16uL,dv16uL	;    restore remainder
	adc		drem16uH,dv16uH
	clc			;    clear carry to be shifted into result
	rjmp	d16u_1		;else
d16u_3:	
	sec			;    set carry to be shifted into result
	rjmp	d16u_1


