;**************************************************************************
;Name:		Robot Library
;Version:	0.1b
;Author:	Dan Kohn
;Date of Mod):	Dec 8, 2005
;
;IO Table:
;
;PortA		PA7	Output / Sonar Init
;		PA6	OC2 / RIGHT Motor (PWM for servo)
;		PA5	OC3 / LEFT Motor (PWM for servo)
;		PA4	OC4 / Multiplexed Servo Output ;mike code gripper motor
;		PA3	OC5 / USED FOR TRACE - BUFFALO
;		PA2
;		PA1	IC2 / Sonar Echo 
;		PA0	IC3/Clock for barcode
;
;PortE		PE7	Data for barcode
;		PE6
;		PE5
;		PE4
;		PE3	Line Track 3	Adr4  left   \
;		PE2	Line Track 2	Adr3  center	- looking from the back of robot
;		PE1	Line Track 1	Adr2  Right  /
;		PE0
;
;		
;PortD		PD5	
;		PD4
;		PD3	
;		PD2	
;
;		OUTPUT Circuit
;$B580		BIT0	LSB Servo Multiplexer 
;		BIT1	|
;		BIT2	MSB Servo Multiplexer
;		BIT3
;		BIT4
;		BIT5
;		BIT6
;		BIT7
;
;		Input Circuit
;$B590		BIT0
;		BIT1
;		BIT2
;		BIT3
;		BIT4
;		BIT5
;		BIT6
;		BIT7
;
;		Input Circuit
;$B5A0		BIT0
;		BIT1
;		BIT2
;		BIT3
;		BIT4
;		BIT5
;		BIT6
;		BIT7
;**************************************************************************
;**************************************************************************
		ORG $2000
		
		include Hc11.inc	;definds all HC11 Registers

		JMP MAIN

;Buffalo Equates
out2bsp 	equ $ffc1		; prints what is pointed to by x to the screen
outcrlf 	equ $ffc4		; outputs a carrage return and line feed
out1bsp 	equ $ffbe		; prints what is pointed to by x to the screen

;Right Motor Variables and Equates
TOC2IV		EQU $00DC		
On_cnts_R	RMB 2
Off_cnts_R	RMB 2


;Left Motor Variables and Equates
TOC3IV		EQU $00D9
On_cnts_L	RMB 2
Off_cnts_L	RMB 2


;Gipper Motor Variables and Equates			;mike code
TOC4IV		EQU $00D6
On_cnts_G	RMB 2
Off_cnts_G	RMB 2

;List of variables for Sonar 
Sonar_start	rmb 2
Sonar_end 	rmb 2
Sonar_ans	rmb 2

;Timer Overflow Counter
TOFIV		EQU $00D0
Sec_Temp	rmb 1		;counter 32.77mSec [0-15]
Sec_count	rmb 1		;counter 0.5Sec [0-255]

;Line Track Control Byte
LTCB		rmb 1

;Line Track Level
LTL		equ $94			; comparison number for more black and less is white? 


;Barcode Equates
IC3IV	 	EQU $00E2
Barcode  	RMB 5
BC_count 	RMB 1
Boxcode  	RMB 1
outa	 	equ $ffb8



MAIN		
		jsr go_stop		;Set up On/Off times for stop
		jsr TOC4_init		;init gripper motor
		jsr TOC3_init		;init left motor
		jsr TOC2_init		;init right motor
		jsr TOF_init		;init Timer Overflow (for 0.5 sec Timer)
		jsr A2D			;init A/D PE0..PE3
		ldaa #20		; pause robot for placement at beginning
		jsr pause
		jsr Start		;leave the block
 		jsr LeaveShoot		; moves robot from shoot to the plane 1 intersection		
					; spot to check if need to go to 1st plane
		jsr Plane1
		jsr GoToPlanes23
		jsr Plane2
		jsr Plane3


		jsr go_stop
		jmp $e000
;**************************************************************************
; this section moves the robot to the block shoot
;**************************************************************************
Start:		psha


		jsr go_forward		;Leaving Block
		ldaa #2
		jsr pause

		jsr LineTrack		; this line moves forward until the 1st plus intersection
		jsr go_forward		; moves forward for a short time to keep line track correct
		ldaa #2
		jsr pause
		jsr LineTrack
		jsr go_stop
					; kill switch or sonar goes here
		ldaa #2			
		jsr pause

		jsr go_Close		;call sub that close the gripper hand
		ldaa #4
		jsr pause
		
		jsr go_stop		;call stop for the gripper
		ldaa #2
		jsr pause

		jsr go_reverse		;Reverse for 1 Sec.
		ldaa #1
		jsr pause
		jsr go_stop
					; sorting barcode info here
		ldaa #1
		jsr pause

		pula
		rts
;**************************************************************************
; this section moves the robot away from the shoot to the intersection for
; turning to plane 1
;**************************************************************************
		
LeaveShoot:		psha
	

		jsr clockw		;180 Right Turn to leave the Package shoot
 		jsr LineTrack		; line track back to the 1st plus

		jsr go_forward		; this section makes the first turn
		ldaa #1			; *
		jsr pause		; *
		jsr go_stop		; *
		jsr counterclck		; *
		jsr go_stop		; *
		ldaa #1			; *
		jsr pause		; *

		jsr LineTrack   	; moves robot to the 1st t intersection
					; ends this section
		pula
		rts
;**************************************************************************
; this section moves the robot to the first plane and then moves it back to the 1st t intersection
;**************************************************************************
Plane1:		psha

		jsr go_forward
		ldaa #1
		jsr pause
		jsr go_stop
		ldaa #1
		jsr pause
		jsr clockw		;90 Right Turn from 1st T

		jsr go_stop
		ldaa #4
		jsr pause
		jsr Linetrack		;Stops at 2nd plus by Loading Zone 1
		jsr go_forward		
		ldaa #5
		jsr pause
		jsr go_stop
		ldaa #1
		jsr pause
		jsr Right90		; turns to back up to the airplane
		jsr go_stop		
		ldaa #1			
		jsr pause

		jsr go_reverse		;Reverses to Drop off
		ldaa #2			
		jsr pause
		jsr go_stop		;sonar goes here
		
			
		jsr go_Open		; for the simple robot must remove
		ldaa #2			; and replace with carrage dump section
		jsr pause		;*

		jsr go_forward		; moves back to the line
		ldaa #2			; *
		jsr pause		; *

firstl:		jsr LT_cont		; this section turns robot back to the line
		ldaa LTCB		; *
		cmpa #$07		; *
		bne  firstl		; *
secondl:	jsr LT_cont		; *
		ldaa LTCB		; *
		cmpa #$07		; *
		bne  secondl		; *
		jsr Right90		; pray this turns right


		jsr linetrack		; moves the robot forward to the 2nd plus
		jsr go_forward		; this section moves the robot back to the 
		ldaa #1		
		jsr pause
		jsr linetrack

		pula

		rts
;**************************************************************************
; this section just moves the robot from the first t intersection 
; to the 3rd plus intersection between robots 2 and 3
;
;
;******************************************************************************
GoToPlanes23:	psha
		jsr go_forward	
		ldaa #1
		jsr pause
		jsr clockw
		jsr linetrack
		pula
		rts
;
;			
;**************************************************************************
; this section moves to the second plane and back to the 3rd plus intersection
;**************************************************************************
Plane2:		psha
		
		jsr go_forward
		ldaa #1
		jsr pause
		jsr clockw		;90 Right Turn from 3rd plus
		
		jsr go_stop
		ldaa #4
		jsr pause
		jsr Linetrack		;Stops at 4th plus by Loading Zone 1
		jsr go_forward
		ldaa #2
		jsr pause
		jsr Linetrack		; moves forward until side of board
		ldaa #1
		jsr pause
		jsr go_reverse		; supposed to back up for a second
		Ldaa #1
		jsr Right90
		jsr go_stop
		ldaa #1
		jsr pause
		jsr go_reverse		;Reverses to Drop off
		ldaa #2
		jsr pause
		jsr go_stop			;sonar goes here

		jsr go_Open		; this section for the simple robot 
		ldaa #2			; this is where the carrage drop occurs
		jsr pause
		jsr go_forward
AlmostThere:	jsr LT_cont		; the robot moves forward to the line here
		ldaa LTCB		; *
		cmpa #$07		; *
		bne  AlmostThere	; *
		jsr go_forward		; *
		ldaa #1			; *
		jsr pause		; *
		jsr clockw		; here turn towards the 4th plus
		jsr go_forward		
		jsr linetrack		; move to the 3rd plus intersection
		jsr go_forward				
		ldaa #1		
		jsr pause
		jsr linetrack		; here stops at the 3rd plus intersection

		pula

		rts
;******************************************************************************************************
; this section moves from the 3rd plus to the 3rd plane and back
;******************************************************************************************************
Plane3:
		jsr go_forward		; moves forward to make linetrack work
		ldaa #1
		jsr pause
		jsr Linetrack		; moves forward until 5th plus
		jsr go_forward		; moves forward so line track will work
		ldaa #1
		jsr pause
		jsr Linetrack		; moves forward to edge of board
		jsr go_stop
		jsr go_reverse		; supposed to move back a little from edge
		Ldaa #2
		jsr Left90		; turns to back up to back up to plane
		jsr go_stop
		ldaa #1
		jsr pause
		jsr go_reverse		;Reverses to Drop off
		ldaa #2
		jsr pause
		jsr go_stop			;sonar goes here
		jsr go_Open		; simple robot code here where comp code goes
		ldaa #2			; *
		jsr pause		; this section moves the robot back to the line
		jsr go_forward		; *
NotThereYet:	jsr LT_cont		; *
		ldaa LTCB		; *
		cmpa #$07		; *
		bne  NotThereYet	; *
		jsr go_forward		; *
		ldaa #1			; *
		jsr pause		; *
		jsr counterclck		; turns the robot back towards the 5th plus sign
		jsr go_forward		;

		jsr linetrack		; moves the robot to the 5th plus sign
		jsr go_forward		; moves robot past the 5th plus sign
		ldaa #1		
		jsr pause
		jsr linetrack		; moves robot to the 3rd plus sign
		pula
		rts
;**************************************************************************
;Name:		LineTrack
;Author:	Dominic Lawrence
;Date:		unknown
;Modifications:	
;		DK 3/4/06 
;		   add tests for LTCB=6 and LTCB = 3 to improve line
;		   following. 
;
;Process:	First it reads each analog line track sensor and
;		determines if the sensor is above or below the threshold
;		(LTL). 
;		
;		If it is lower than LTL, the corresponding bit in LTCB 
;		is set to indicate that sensor is on the white line.
;		
;		Then LTCB is tested and the appropriate sub routine is 
;		called to turn turned on/off the drive motors accordingly.
;**************************************************************************

LineTrack:	psha
		pshx
		
Lt_loop:	jsr Lt_cont
		
FirstT:		ldaa LTCB	; checks the center sensors
		cmpa #$02
		beq  FForward
		
		cmpa #$04	; checks sensors for if to far left
		beq  RTurn
		cmpa #$06
		beq  Rturn
			
		cmpa #$01	; checks sensors for if to far right
		beq  LTurn
		cmpa #$06
		beq  LTurn
		cmpa #$07
		beq  SStop
		
		jsr go_forward
		jmp Lt_loop
		
 
RTurn:		jsr  go_right  		; jumps to sub that moves robot to the right	
		jmp  Lt_loop

LTurn:		jsr  go_left		; jumps to sub that moves robot to the left
		jmp  Lt_loop

FForward:	jsr  go_Forward		; jumps to sub that moves robot forward
		jmp  Lt_loop

SStop:		jsr go_stop		; jumps to stop sub
		ldaa #4
		jsr pause 
		
		pulx		
		pula
		rts		
;**************************************************************************
;
;**************************************************************************
Lt_cont:	psha
		pshx

		Ldaa #$0
		Staa LTCB
		Ldaa Adr2
		Cmpa #LTL
		BHS  CheckCenter
		Ldx  #LTCB
		Bset 0,x #$04

CheckCenter:	Ldaa Adr3
		Cmpa #LTL
		BHS  CheckLeft
		Ldx  #LTCB
		Bset 0,x #$02

CheckLeft:	Ldaa Adr4
		cmpa #LTL
		BHS  lt_exit
		Ldx  #LTCB
		Bset 0,x #$01

lt_exit:		pulx
		pula
		
		rts