;------------------------------------------
;Project Name:  323 16Bit PWM Test.asm
;Description:	Test Single Channel PWM 16Bit Up/Down Counter
;Author:		Max Koessick
;Date;			July 26, 2003
;Revision:		1.0  Working 16Bit PWM
;				1.a  Working Ext Interupts (2:0)
;				1.b	 Added 8 bit PWMs
;				1.c	 Added IR IRQ Error Checking Algorithm

;****NOTE****

;You must disable I-bit around OC register changes or an Interrupt may fire

;System Calculations:
;------------------------------------------
;Use 3.69MHz clock
;Use Prescaler =/64 ->57.6kHz = T=~17uS
;8bit PWM Up/Down counts to $FF->17uS*FF=4.423ms = T(PWM)/2
;@1.0ms, 4.423-1.0/2=3.923ms
;	solve(.003923=.000017x,x)->x=226=$E2 *Servo Left*
;@1.5ms, 4.423-1.5/2=3.673ms
;	solve(.003673=.000017x,x)->x=212=$D4 *Servo Neutral*
;@2.0ms, 4.423-2.0/2=3.423ms
;	solve(.003423=.000017x,x)->x=197=$C5 *Servo Right*

.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
.org INT0addr
	rjmp IntV0
.org INT1addr
	rjmp IntV1
.org INT2addr
	rjmp IntV2

;-------------------------------------------
;Register Definitions
;-------------------------------------------

.def mpr   		=r16		; Temporary Register
.def mpr2		=r17
.def errorreg	=r20
;Initialization

RESET:

	clr		errorreg
;-----Setting Stackpointer----------------------------------------
	ldi		MPR,low(RAMEND)			; Set stackptr to ram end
	out		SPL,MPR
	ldi 	MPR, high(RAMEND)
	out 	SPH, MPR

;-----Set Port Directions-----------------------------------------
		
	ldi		mpr,0b11110000
	out		DDRD,mpr			; Set PORTD to output

	ldi		mpr,(1<<PB3)
	out		DDRB,mpr			; Set PORTB to output

	ser		mpr
	out		DDRC,mpr
	out		DDRA,mpr


;-----Enable 16Bit PWM (Sonar Servo) Counter in 8Bit Mode---------
	ldi		mpr,0b11110001		; Bit7:6 -> Inverted PWM
								; Bit5:4 -> Disable OC1B 
								; Bit3;2 -> FOC =n/a
								; Bit1:0 -> 8Bit PWM mode
	out		TCCR1A,mpr

	ldi		mpr,0b00000011		; Bit7 -> Input Noise Canceler Disabled
								; Bit6 -> Input Capter Edge Select n/a
								; Bit5:4 -> Unsused
								; Bit3 -> Clear on Compare Match Disabled
								; Bit2:0 -> Prescale = /64
	out		TCCR1B,mpr

;-----Enable 8 bit PWM (Dir and Speed) ---------------------------

	ldi		mpr,0b01110011		; Bit7 -> FOC2 force Output Compare = n/a
								; Bit6 -> PWM0 Enables PWM output 
								; Bit5:4 -> Set on match upcount, clear on match downcount (11)
								; Bit3 -> CTC0 No clear on match
								; Bit2:0 -> Prescale = /64
	out		TCCR0,mpr			; Enable PWM0
	out		TCCR2,mpr			; Enable PWM2
;------Enable External Interupts-----------------------------------

	in		mpr,MCUCSR
	andi	mpr,0b10111111		; Clear the INT2 Sense Control Bit -> Falling Edge triggered
	out		MCUCSR,mpr

	in		mpr,MCUCR
	andi	mpr,$f0				; Mask Upper Bits
	ori		mpr,0b00000010		; Set ISC1:0 Sense Control bits [3:0] -> Falling Edge for Int0
								; Low level for Int1 (IR) -> ISR must fire as long as a 
								; object is detected in the rear.
	out		MCUCR,mpr

	ldi		mpr,0b11100000		; Enable Interrupts
	out		GICR,mpr

;------------------------------------------------------------------


	ldi		mpr,$ce				; Test value *Servo Neutral*(Speed)
	out		OCR0,mpr			; Load OCR0 with value for 1.0 ms pulse in a T=8.8ms

	ldi		mpr,$a4				; Test value *Servo Neutral*(Direction)
	out		OCR2,mpr			; Load OCR0 with value for 1.0 ms pulse in a T=8.8ms


	ldi		mpr,$d9				; Test value *Servo neutral*(sonar)
	out		OCR1AL,mpr			; Load OCR1AL with value for 1.5 ms pulse in a T=8.8ms
	ldi		mpr,$ff				; Test value *Servo neutral*(sonar)
	out		OCR1BL,mpr			; Load OCR1AL with value for 1.5 ms pulse in a T=8.8ms


								; Interrupts must be disabled when changing output compare registers
sei

mainloop:
	ldi		mpr,$ff
	out		portc,mpr
	out		porta,mpr


	rjmp 	mainloop

IntV0:
	reti

IntV1:								; IR Interrupt
;	ldi		errorreg,$aa
	inc		errorreg

	cpi		errorreg,5				; Check IR 5 times before acting
	brne	endIntV1

	nop								; Execute ISR intructions here
	;cli
	;issue stop
	;call obstacle
	;sei
	clr		errorreg				; reset register


endIntV1:
	;call 	delay
	reti

IntV2:

	ldi		mpr,$aa
	com		mpr
	out		portc,mpr
	call 	delay
	reti

delay:

	ldi		r24,$ff
	ldi		r25,$ff
	ldi		mpr,$06

here:
	sbiw	r25:r24,1
	brne	here
;	dec		mpr
	;brne	here

	ret