
;-----------------------------------------------------------------
; Name:			Main Robot.asm
; Description:	ATMega1323 Two Wire Interface (IC2) Test Program
;				Interfaces Daventech SRF08 Range Finder to I2C Bus

; Author:		Max Koessick
; Class:		EEL5666C, Intelligent Machine Design Lab
; Date:			July 27, 2003
; Revision		1.a
; Changes to Date:
;				7/27/03 First Revision

;-----------------------------------------------------------------

.nolist								; Do not include in .lst file
.include "m323def.inc"				; Standard ATMega128 Include File
.include "TWI.inc"					; Two Wire Interface Error code definitions
.list
; Interrupt service vectors

.org $0000
	rjmp Reset						; Reset vector
.org INT0addr
	rjmp IntV0
.org INT1addr
	rjmp IntV1
.org INT2addr
	rjmp IntV2
	
; ****************************************** ---------------------
; ***** Register defines for main loop ***** ---------------------
; ****************************************** ---------------------


.def		mpr			=r16		; defines multipurpose register
.def		MPR2		=r17		; multipurpose register 2
.def		mpr3		=r18
.def		ECHO1L		=r19
.def		LEDreg		=r20
.def		ErrorReg	=r21
.def		Obsreg		=r22		; Contains the object detected flag

; ******************* --------------------------------------------
; ***** Equates ***** --------------------------------------------
; ******************* --------------------------------------------

; Equate statements for SRF08 Sonar
.equ		W			= 0			; Write Bit
.equ		R			= 1			; Read Bit
.equ		SLA			= $FE		; Slave Address of SRF08
.equ		CommandReg	= $00		; Random address
.equ		Inches		= $50		; Ranging Mode returns results in inches
.equ		EchoReg2	= $02
.equ		EchoReg3	= $03

; Equate statements for Servos
.equ		LookRT		=$F6		; Sonar directions
.equ		LookFwd		=$D9		
.equ		LookLFT		=$BF
.equ		FullLFT		=$B2		; Turning
.equ		SlipLFT		=$AF
.equ		Straight	=$A7
.equ		SlipRT		=$A3
.equ		FullRT		=$A1
.equ		StopPWM		=$FF
.equ		FwdSlow		=$d5		
.equ		FwdFast		=$d5		
.equ		Stop		=$CE
.equ		RevSlow		=$cb
.equ		RevFast		=$c9	
.equ		Turntime	=$FFFF		; Turning Delay
.equ		Revtime		=$FFFF		; Reverse Delay
.equ		NoPing		=$FFFF		; Wait for Servo to turn
.equ		MinDist		=20


.equ 		brake		= 1
.equ 		ArmDir		= 0
.equ 		MagOn		= 6

; ************************ ---------------------------------------
; ***** 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 	DDRA,mpr			; LCD


	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					; LEDs and TWI
	out		DDRC,mpr
	out   	PORTC,mpr
		
	ldi		mpr,0b11110011		; Set PD2 and PD3 to input
	out		DDRD,mpr			; Set PORTD to output	
		

;-----------------------------------------------------------------
; ***** Initialize I2C(TWI) Interface ***** ----------------------
;-----------------------------------------------------------------
	
; Set TWIBitRate for fclk=16Mhz

	ldi		mpr,11				; 100Khz=3.69MHz/(16+2*11) See Datasheet Pg202
	out		TWBR,mpr			; Note: This system clock does not support 400kHz


; Initialize TWCR Register
	clr		mpr
	ldi 	MPR,(1<<TWEN);
	out	 	TWCR,MPR 			; Initialize Two Wire Control Register

;	ldi		mpr,$01
;	out		TWAR,mpr
;-----------------------------------------------------------------
; ***** Initialize TC0,TC1A,TC1B,TC2 ***** -----------------------
;-----------------------------------------------------------------

	clr		mpr
	out		TIMSK,mpr			; Turn Off any Timer associated interupts


;-----Enable 16Bit PWM (Sonar Servo) Counter in 8Bit Mode---------


	ldi		mpr,0b11000001		; 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,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 Interrupts ***** --------------------------
;-----------------------------------------------------------------

	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,0b00001010		; Set ISC1:0 Sense Control bits [3:0] -> Falling Edge for Int0
								; Low level for Int1 (IR) -> ISR must fire as long as a 
								; bject is detected in the rear.
	out		MCUCR,mpr

	ldi		mpr,0b11100000		; Enable Interrupts 1-3
	out		GICR,mpr

;-----------------------------------------------------------------

; ************************ ---------------------------------------	
; ***** Main Program ***** ---------------------------------------
; ************************ ---------------------------------------
mainloop:
	
	ldi		mpr,FwdSlow				; Set default forward speed
	out		OCR0,mpr
	ldi		mpr,Straight			; Set default direction
	out		OCR2,mpr		
	ldi		mpr,LookFWD				; Set default Sonar Direction
	out		OCR1AL,mpr

	sei

	call	LEDs					; Update LEDs

	call 	Look					; For Debug
;
	sbrc	Obsreg,0				; If bit one is cleared from LOOK subroutine,
									; then no obstacle found.  Prgm will skip calling
									; Obstacle routine
	call	Obstacle
	
	rjmp 	mainloop






; *********************** ----------------------------------------
; ***** Subroutines ***** ----------------------------------------
; *********************** ----------------------------------------

;-----------------------------------------------------------------
;-----Look--------------------------------------------------------

Look:
; Start Error Rejection: Call ping 3 times to verify that an object is in path
;	before branching to obstcle routing
;Ping1:

	call	Get_PING				; Get sonar data
	subi	ECHO1L,MinDist			; object closer than MinDist inches?
	brsh	No_Obs					; ...no? Then branch is same or higher
;Ping2:
;	call	Get_PING				; Get sonar data
;	subi	ECHO1L,MinDist			; object closer than MinDist inches?
;	brsh	No_Obs					; ...no? Then branch is same or higher
;Ping3:
;	call	Get_PING				; Get sonar data
;	subi	ECHO1L,MinDist			; object closer than MinDist inches?
;	brsh	No_Obs					; ...no? Then branch is same or higher
	
	ldi		Obsreg,$1				; Found an Obstacle
	rjmp	End_look

No_Obs:
	clr		Obsreg					; Didn't find an obstacle

End_look:

	ret								
	

; -----Get_PING---------------------------------------------------

Get_Ping: 
.nolist
.include "ping.inc"
.list
;return instruction included in .inc file

;-----------------------------------------------------------------
;-----Obstacle----------------------------------------------------

Obstacle:



	cli							; Disable interrupts whil changing Output compare registers

	ldi		mpr,Stop			; StopPWM
	out		OCR0,mpr

	ldi		mpr,LookLFT			; Rotate Sonar Left
	out		OCR1AL,mpr	

	sei							; Reset Interrupts

	call	NoPingDelay			; Wait for servo to turn

	call 	Look		

	sbrc	Obsreg,0			; If bit one is cleared from LOOK subroutine,
								; then no obstacle found.  Prgm will skip looking 
								; right and break out
	rjmp	Right

	call 	Go_left				; ...else go left
	rjmp	End_Obstacle		; Exit subroutine

Right:

	cli							; Disable interrupts whil changing Output compare registers
	ldi		mpr,LookRT			; Rotate Sonar right
	out		OCR1AL,mpr	

	sei							; Renable Interrupts

	call	NoPingDelay			; Wait for servo to turn
	call 	NoPingDelay			; Must travel 180 degrees

	call 	Look	

	sbrc	Obsreg,0			; If bit one is cleared from LOOK subroutine,
								; then no obstacle found.  Prgm will skip reversing
								; and break out
	rjmp	Reverse
	
	call	Go_Right			; ...else turn right
	rjmp	End_Obstacle		; Exit Subroutine

Reverse:
	cli							; Disable interrupts whil changing Output compare registers

	ldi		mpr,LookFWD			; Reset Sonar Forward
	out		OCR1AL,mpr

	call	NoPingDelay			; Wait for servo to turn

	ldi		mpr,Straight		; Set direction clutch neutral
	out		OCR2,mpr

	ldi		mpr,FwdSlow			; Set reverse speed 2
	out		OCR0,mpr	
	
	sei							; Reenable Interrupts
			
	call	ReverseDelay

	cli							; Disable interrupts whil changing Output compare registers

	ldi		mpr,STOP			; Set Stop
	out		OCR0,mpr	
	
	sei							; Reenable Interrupts
			

 	rjmp	Obstacle			; Check left and right again for options
								

End_Obstacle:
	ret


;-----------------------------------------------------------------
;-----LEDs--------------------------------------------------------

LEDs:

	ret

;-----------------------------------------------------------------
;-----Go_Left-----------------------------------------------------

Go_Left:
;jmp		testz
	cli							; Disable interrupts whil changing Output compare registers

	ldi		mpr,LookFWD			; Reset Sonar Forward
	out		OCR1AL,mpr

	ldi		mpr,FullLFT			; Gradual Right turn (Set Direction Clutch)
	out		OCR2,mpr

	ldi		mpr,FwdSlow			; Set H-Bridge PWM
	out		OCR0,mpr
	
	sei							; Reenable Interrupts

	call 	TurnDelay			; Wait to complete 90Deg turn
	
	cli							; Disable interrupts whil changing Output compare registers

	ldi		mpr,Straight		; Go Straight (Set Direction Clutch)
	out		OCR2,mpr	

	sei							; Reenable Interrupts

	ret

;-----------------------------------------------------------------
;-----Go_Right----------------------------------------------------

Go_Right:


	cli							; Disable interrupts whil changing Output compare registers

	ldi		mpr,LookFWD			; Reset Sonar Forward
	out		OCR1AL,mpr

	ldi		mpr,FullRT			; Gradual Right turn (Set Direction Clutch)
	out		OCR2,mpr

	ldi		mpr,FwdSlow			; Set Servo PWM
	out		OCR0,mpr
	
	sei							; Reenable Interrupts

	call 	TurnDelay			; Wait to complete 90Deg turn

	cli							; Disable interrupts whil changing Output compare registers
	
	ldi		mpr,Straight		; Go Straight (Set Direction Clutch)
	out		OCR2,mpr	

	sei							; Reenable Interrupts

	ret

;-----------------------------------------------------------------
;-----Crawl_Reverse------------------------------------------------

Crawl_Reverse:

	cli							; Disable interrupts whil changing Output compare registers

	ldi		mpr,LookFWD			; Reset Sonar Forward
	out		OCR1AL,mpr

	ldi		mpr,Straight		; Set direction clutch neutral
	out		OCR2,mpr

	ldi		mpr,FwdSlow			; Set Servo PWM
	out		OCR0,mpr
	
	sei							; Reenable Interrupts	
			
	call	TurnDelay			; Keep going straight backwards

	cli							; Disable interrupts whil changing Output compare registers

	ldi		mpr,Stop			; Stop
	out		OCR0,mpr

	sei							; Reenable Interrupts

 	ret

;-----------------------------------------------------------------
;-----Crawl_Forward------------------------------------------------

Crawl_Forward:					

	cli							; Disable interrupts whil changing Output compare registers
	ldi		mpr,LookFWD			; Reset Sonar Forward
	out		OCR1AL,mpr

	ldi		mpr,Straight		; Set direction clutch neutral
	out		OCR2,mpr

	ldi		mpr,FwdSlow			; Set Servo Speed to slow
	out		OCR0,mpr
	
	sei							; Reenable Interrupts

	call	TurnDelay			; Keep going straight backwards

	cli							; Disable interrupts whil changing Output compare registers

	ldi		mpr,Stop			; Set H-Bridge PWM to stop
	out		OCR0,mpr

	sei							; Reenable Interrupts

 	ret

;-----------------------------------------------------------------
;-----TurnDelay---------------------------------------------------

TurnDelay:

	ldi		r24,low(Turntime)
	ldi 	r25,high(Turntime)		; Prepare register pair as counter
	ldi		mpr,$10

TurnLoop:
	sbiw 	r25:r24,1				; Subtract 1 from register pair
	brne	Turnloop				; 3 cycles for these instructions
									;   implements .05328ms delay
	dec		mpr
	brne	turnloop

	ret
;-----------------------------------------------------------------
;-----ReverseDelay------------------------------------------------

ReverseDelay:

	ldi		r24,low(Revtime)
	ldi 	r25,high(Revtime)		; Prepare register pair as counter

ReverseLoop:
	sbiw 	r25:r24,1				; Subtract 1 from register pair
	brne	Reverseloop				; 3 cycles for these instructions
									;   implements .05328ms delay

	ret

;-----------------------------------------------------------------
;-----NoPingDelay------------------------------------------------
NoPingDelay:

	ldi		r24,low(NoPing)
	ldi 	r25,high(NoPing)		; Prepare register pair as counter
	ldi		mpr3,$9		

NoPIngLoop:
	sbiw 	r25:r24,1				; Subtract 1 from register pair
	brne	NoPingloop				; 3 cycles for these instructions
									;   implements .05328ms delay
	dec		mpr3
	brne	nopingloop
;jmp	TESTz
	ret

	

; ***************************** ---------------------------------- 
; ***** Interupt Handlers ***** ----------------------------------
; ***************************** ----------------------------------

; External Interupts
IntV0:
	reti

	
IntV1:
	;	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
reti

	IntV2:		;Hall Interrupt->Acquires target and moves arm
	;*****------------------------------------------------------------
		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

	reti
;*****------------------------------------------------------------

TestZ:


	ldi		mpr,$55
	com		obsreg
	out		portA,obsreg

here: 	rjmp	here

;-----------------------------------------------------------------
delay1us:
	ldi		mpr,$ff
loopdelay1us:
	dec		mpr
	brne	loopdelay1us

	ret
;-----------------------------------------------------------------
delay5s:

	ldi		r24,$ff
	ldi		r25,$ff
	ldi		mpr,$9

delay5sLoop:
	sbiw	r25:r24,1
	brne	delay5sLoop
	dec		mpr
	brne	delay5sLoop
	ret
;------------------------------------------------------------------