;-----------------------------------------------------------------
; Name:			Staight and Reverse.asm
; Description:	Test Output Compare PWM Mode 8 Functionality.
;				Functional ramp up of forward and reverse speeds
;				Contains unused external interrupt data
;				Contains LED chase

; Author:		Max Koessick
; Class:		EEL5666C, Intelligent Machine Design Lab
; Date:			June 15, 2003
; Revision		1.a
; Changes to Date:
;				6/15/03 First Revision
;				
; 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 3.69 MHz/64(prescaler), the 
;	new clock is equal to 17us.  A TOP value must be defined in order 
;	to create the proper PWM.  The value for TOP is 20ms/2.  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)/17us=588=0x24C=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/17us=44 clocks.  TOP - 44 = 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)--Front Left	
.org INT1addr	
	rjmp IntV1						; INT1 vector (ext. interrupt from pin D1)--Front Right
.org INT2addr		
	rjmp IntV2						; INT2 vector (ext. interrupt from pin D2)--Left Rear
.org INT3addr			
	rjmp IntV3						; INT3 vector (ext. interrupt from pin D3)--Right Rear

; Bump Switch Map
; PD0->Front Left
; PD1->Front Right
; PD2->Left Rear
; PD3->Right Rear

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

.def mpr		=r16				; defines multipurpose register
.def mpr2		=r17				; multipurpose register 2
.def mpr3		=r18				; multipurpose register 3
.def delay 		=r19				; Delay Variable Register 1
.def delay2		=r20				; Delay Variable Register 2
.def eight		=r21				; 8 Counter for 8 discrete LEDs
.def delay3		=r22				; forward counter for ramp up
.def LEDs		=r23				; reverse counter for ramp down
.def dir		=r24				; interrupt direction register

;-----------------------------------------------------------------
; 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

	clr		LEDs
	out		PORTC,LEDs				; ...all lights off!

	clr 	mpr						; clear all bits
	ldi		mpr2,(1<<PD0)+(1<<PD1)+(1<<PD2)+(1<<PD3)
									; Define pull ups on the input pins of PORTD
	out 	DDRD,mpr				; Set 'D' as input
	out		PORTD,mpr2

;-----Enabling Interrupts-----------------------------------------

;Clear INT0 and INT1 in EIMSK register
	in		mpr,EIMSK
	cbr		mpr,(0<<INT0)+(0<<INT1)+(0<<INT2)+(0<<INT3) ; 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)+(1<<ISC30)+(1<<ISC31)				
									; 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)+(1<<INTF3);Set interrupt flag in EIFR
	out		EIFR,mpr
;Set INT[3..0] in EIMSK register
	in		mpr,EIMSK
	sbr		mpr,(1<<INT0)+(1<<INT1)+(1<<INT2)+(1<<INT3) ; 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,0b00010011			; Prescaler = 64
	out		TCCR1B,mpr

	ldi		mpr,$02
	ldi		mpr2,$4c
	out		ICR1H,mpr
	out		ICR1L,mpr2				; Set Top Value to 1177 = $499
									; Period = 20ms

	ldi		mpr,$00					; Clear the Timer/Counter 1 Register
	ldi		mpr2,$00
	out		TCNT1H,mpr
	out		TCNT1L,mpr2	

	sei								; Enable global interrupts



;****Main Routine LED Chase****-------------------------
	ldi 	LEDs,$7f				; Turn on left most LED (Active Low)
	out		PORTC,LEDs				; Output to LEDs on ShopBot
		

LOOP:

	ldi		Eight,$08

RightLoop:
		
	ror		LEDs					; Rotate LEDs one right
	out		PORTC,LEDs				; Output to LEDs on ShopBot
	
;Wait to make LEDs visible 

DLY:
	dec		delay
	brne	dly
	dec		delay2
	brne	dly

	dec		eight					; Shift the light eight times
	brne	rightloop

	ldi		eight,$08

LeftLoop:
		
	rol		LEDs
	out		PORTC,LEDs				; Output to LEDs on ShopBot


dly2:

	dec		delay
	brne	dly2
	dec		delay2
	brne	dly2



	dec 	eight
	brne 	leftloop

;**** Set Default Speed and Direction****--------------------------
	ldi		mpr,$02
	ldi		mpr2,$22
	out		OCR1AH,mpr
	out		OCR1AL,mpr2				; Set DIRservo = Straight

	ldi		mpr,$02
	ldi		mpr2,$1a
	out		OCR1BH,mpr
	out		OCR1BL,mpr2				; Set SPEEDservo = Forward


	rjmp	loop					; Repeat forver				
;-----------------------------------------------------------------
;
; External Interrupt Service Routines

; PORT D0 Interrrupt signifies an impact on front left
; Back up and turn right

IntV0:								

	push	mpr
	push	mpr2
	push	delay
	push 	delay2

;	clr 	delay
;	clr		delay2

	ldi		delay3,$0a

	ldi		mpr,$02
	ldi		mpr2,$28
	out		OCR1AH,mpr
	out		OCR1AL,mpr2				; Set DIRservo = Full Left

	ldi		mpr,$02
	ldi		mpr2,$24

INT0load:
	out		OCR1BH,mpr
	out		OCR1BL,mpr2				; Set SPEEDservo = Ramp Up to Full Reverse

	cpi		mpr2,$30
	breq	INT0dly3
	inc 	mpr2

INT0dly1:							; Delay while ramping up
	dec 	delay
	brne	INT0dly1
	dec 	delay2
	brne	INT0dly1

	rjmp	INT0load

INT0dly3:							; Delay while turning
	dec 	delay
	brne	INT0dly3
	dec 	delay2
	brne	INT0dly3
	dec 	delay3
	brne	INT0dly3

	pop		delay2					; Restore registers
	pop		delay
	pop		mpr2
	pop		mpr

	reti

;**************************************************************
; PORT D1 Interrrupt signifies an impact on front right
; Back up and turn left

IntV1:								

	push	mpr
	push	mpr2
	push	delay
	push 	delay2

;	clr 	delay
;	clr		delay2

	ldi		delay3,$0a

	ldi		mpr,$02
	ldi		mpr2,$1b
	out		OCR1AH,mpr
	out		OCR1AL,mpr2				; Set DIRservo = Full right

	ldi		mpr,$02
	ldi		mpr2,$24
INT1load:
	out		OCR1BH,mpr
	out		OCR1BL,mpr2				; Set SPEEDservo = Ramp Up to Full Reverse

	cpi		mpr2,$30
	breq	INT1dly3
	inc 	mpr2

INT1dly1:							; Delay while ramping up
	dec 	delay
	brne	INT1dly1
	dec 	delay2
	brne	INT1dly1

	rjmp	INT1load

INT1dly3:							; Delay while turning
	dec 	delay
	brne	INT1dly3
	dec 	delay2
	brne	INT1dly3
	dec 	delay3
	brne	INT1dly3

	pop		delay2					; Restore registers
	pop		delay
	pop		mpr2
	pop		mpr

	reti
;**************************************************************
;**************************************************************
; PORT D2 Interrrupt signifies an impact on Left Rear
; Turn right and go forward

IntV2:								

	push	mpr
	push	mpr2
	push	delay
	push 	delay2

;	clr 	delay
;	clr		delay2

	ldi		delay3,$0a

	ldi		mpr,$02
	ldi		mpr2,$1b
	out		OCR1AH,mpr
	out		OCR1AL,mpr2				; Set DIRservo = Full right

	ldi		mpr,$02
	ldi		mpr2,$24
INT2load:
	out		OCR1BH,mpr
	out		OCR1BL,mpr2				; Set SPEEDservo = Ramp Up to Full Forward

	cpi		mpr2,$17
	breq	INT2dly3
	dec 	mpr2

INT2dly1:							; Delay while ramping up
	dec 	delay
	brne	INT2dly1
	dec 	delay2
	brne	INT2dly1

	rjmp	INT2load

INT2dly3:							; Delay while turning
	dec 	delay
	brne	INT2dly3
	dec 	delay2
	brne	INT2dly3
	dec 	delay3
	brne	INT2dly3

	pop		delay2					; Restore registers
	pop		delay
	pop		mpr2
	pop		mpr

	reti
;**************************************************************
; PORT D3 Interrrupt signifies an impact on Right REar
; Turn right and go forward

IntV3:								

	push	mpr
	push	mpr2
	push	delay
	push 	delay2

;	clr 	delay
;	clr		delay2

	ldi		delay3,$0a

	ldi		mpr,$02
	ldi		mpr2,$28
	out		OCR1AH,mpr
	out		OCR1AL,mpr2				; Set DIRservo = Full Left

	ldi		mpr,$02
	ldi		mpr2,$24

INT3load:
	out		OCR1BH,mpr
	out		OCR1BL,mpr2				; Set SPEEDservo = Ramp Up to Full Reverse

	cpi		mpr2,$17
	breq	INT3dly3
	dec 	mpr2

INT3dly1:							; Delay while ramping up
	dec 	delay
	brne	INT3dly1
	dec 	delay2
	brne	INT3dly1

	rjmp	INT3load

INT3dly3:							; Delay while turning
	dec 	delay
	brne	INT3dly3
	dec 	delay2
	brne	INT3dly3
	dec 	delay3
	brne	INT3dly3

	pop		delay2					; Restore registers
	pop		delay
	pop		mpr2
	pop		mpr

	reti

;**************************************************************