;------------------------------------------
;Project Name:  323 Arm and Magnet.asm
;Description:	Test H-Bridge control of arm and Main motor plus
;				Power FET/Magnet ops
;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  Fixed Intermittent IRQ firing
;				1.d  Final Version 
;					  	Arm working correctly
;						1) Turn On Magnet
;						2) Raises Arm until feedback switch is pressed
;						3) Delay
;						4) Turn Off Magnet
;						6) Lower Arm Until Fedback switch is pressed				
;------------------------------------------
;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 oldsd		=r17		; Old Speed Register
.def newspd		=r18		; New Speed Register
.def mpr2		=r19




.equ brake		= 1
.equ ArmDir		= 0
.equ MagOn		= 6
;Initialization




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-----------------------------------------
		
	ldi		mpr,0b11110011		; Set PD2 and PD3 to input
	out		DDRD,mpr			; Set PORTD to output

	ldi		mpr,0b11111000
	out		DDRB,mpr			; Set PORTB to output
	ldi		mpr,(1<<PB0)|(1<<PB1)
	out		PORTB,mpr			; Enable Internal pull up for PB0,PB1

	ser		mpr
	out		DDRC,mpr
	out		DDRA,mpr
	out   	PORTC,mpr
	out		PORTA,mpr			; LEDs off

;-----Enable 16Bit PWM (Sonar Servo -A) and Arm Motor (OCR1B) 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

	sbi		PORTD,Brake			; Set Brake bit to low PD0=0

;-----Enable 8 bit PWM (Dir and Speed) ---------------------------



;	ldi		mpr,$d4				; Test value *Servo Neutral*
;	out		OCR0,mpr			; Load OCR0 with value for 1.0 ms pulse in a T=8.8ms
;	out		OCR2,mpr			; Sets servos to neutral at program startup

	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


;------------------------------------------------------------------

mainloop:

;******* when this code is a subroutine, clear the I-bit here ******

;	cli

; Magnet on here
; Start moving arm up
	sbi		PORTD,MagOn
	call	delay5s

	sbi		PORTD,ArmDir		; Set PD0 to '1'-> Arm Direction
	call	delay1us
	cbi		PORTD,Brake			; Set Brake bit to low PD0=0 DISENGAGE
	call	delay1us
	ldi		mpr,$aa 			; Test value *Servo neutral*(sonar)
	out		OCR1BL,mpr			; Load OCR1AL with value for 1.5 ms pulse in a T=8.8ms
	
WaitForUp:
	sbis	PINB,1				; PB1= Rear stop switch
	rjmp	WaitForUP

;	call 	delay5s
	sbi		PORTD,Brake			; Engage Brake
	call	delay5s				; Delay to smooth arm operation

	cbi		PORTD,MagON			; Magnet off here
;		
	cbi		PORTD,ArmDir		; Change Directions
	call	delay1us
;
	cbi		PORTD,Brake			; Set Brake bit to low PD0=0 DISENGAGE
	call	delay1us
	
;	ldi		mpr,$AA				; Start Arm Motor
;	out		OCR1BL,mpr	

WaitForDown:

	sbic	PINB,0				; PB0=Front Arm Switch
	rjmp	WaitForDown

	sbi		PORTD,Brake			; Engage Brake
	call	delay1us
	ldi		mpr,$FF				; Stop Arm Brake + PWM = 0-> Output transistor are off
	out		OCR1BL,mpr

	sei							; Reenable I-Bit

mloop:
;Exit subroutine here
	rjmp 	mloop

IntV0:

	reti

IntV1:	
	reti

IntV2:
	reti

;-----------------------------
delay1us:
	ldi		mpr,$ff
loopdelay1us:
	dec		mpr
	brne	loopdelay1us

	ret
;-----------------------------
delay5s:

	ldi		r24,$ff
	ldi		r25,$00
;	ldi		mpr,$3

delay5sLoop:
	sbiw	r25:r24,1
	brne	delay5sLoop
;	dec		mpr
;	brne	delay5sLoop
	ret
;-----------------------------DISENGAGE
Test:

	LDI		MPR,$aA
	OUT		PORTa,MPR
	
	rjmp	end
end:
	ret
