'SERIAL PIN CONSTANTS

r  con 0		 'RIGHT SERIAL PIN
l  con 1		 'LEFT SERIAL PIN

	'VERTICAL REFERENCE CONSTANTS

rfv con 9		 'RIGHT FRONT 	VERTICAL
rmv con 11		 'RIGHT MIDDLE 	VERTICAL
rbv con 13		 'RIGHT BACK 	VERTICAL
lfv con 1		 'LEFT 	FRONT 	VERTICAL
lmv con 3		 'LEFT 	MIDDLE 	VERTICAL
lbv con 5		 'LEFT 	BACK 	VERTICAL

	'HORIZONTAL REFERENCE CONSTANTS

rfh con 8		 'RIGHT FRONT 	HORIZONTAL
rmh con 10		 'RIGHT MIDDLE 	HORIZONTAL
rbh con 12		 'RIGHT BACK 	HORIZONTAL
lfh con 0		 'LEFT  FRONT 	HORIZONTAL
lmh con 2		 'LEFT  MIDDLE 	HORIZONTAL
lbh con 4		 'LEFT  BACK 	HORIZNOTAL

	'NECK REFERENCE CONSTANTS
ntwist con 6
ntilt  con 14
	'RIGHT SIDE GLOBALS

ntwmax var byte
ntimax var byte
ntwmin var byte
ntimin var byte
ntwmid var byte
ntimid var byte
rvmax var byte
rvmin var byte
rhmax var byte
rhmin var byte
rvavg var byte
rhavg var byte

'==========================================================
'==========================================================
	'EDIT BELOW FIELDS ONLY
'==========================================================
'==========================================================
	'MIDPOINT AND PAUSE CONSTANTS
m0 con 135 	     	 'VERTICAL MIDPOINT
m1 con 130       	 'HORIZONTAL MIDPOINT
m3 con 130
		 'DEFAULT PAUSE

	'NECK RANGE VARIABLES
ntwmax = 160
ntwmin =1
ntwmid =80
ntimax =10
ntimin =200
ntimid =80	

	'VERTICAL   RANGE VARIABLES
rvmax = 254    		 'HIGHEST VERTICAL POSITION
rvmin = 120  	     	 'LOWEST  VERTICAL POSITION

	'HORIZONTAL RANGE VARIABLES
rhmax = 190 	     	 'MOST FORWARD  HORIZONTAL POSITION
rhmin = 25	     	 'MOST BACKWARD HORIZONTAL POSITION

'IS BEST IF VALUES ARE ON EQUAL SIDES OF M0 AND M1
'==========================================================
'==========================================================
	'EDIT ABOVE FIELDS ONLY
'==========================================================
'==========================================================
		
rvavg = (rvmax+rvmin)/2  'AVERAGE VERTICAL   POSTION
rhavg = (rhmax+rhmin)/2  'AVERAGE HORIZONTAL POSITION

	'LEFTSIDE GLOBALS
	'(based off right side)

lvmax var byte
lvmin var byte
lhmax var byte
lhmin var byte
lvavg var byte
lhavg var byte

lvmax = m0+(m0-rvmax)    'HIGHEST  VERTICAL POSITION
lvmin = m0+(m0-rvmin)    'SMALLEST VERTICAL POSITION
lhmax = m1+(m1-rhmax)    'MOST FORWARD  HORIZONTAL POSITION
lhmin = m1+(m1-rhmin)    'MOST BACKWARD HORIZONTAL POSTION

lvavg = (lvmax+lvmin)/2  'AVERAGE VERTICAL   POSITION
lhavg = (lhmax+lhmin)/2  'AVERAGE HORIZONTAL POSITION

steps var byte		 'STEP COUNTER VARIABLE
temp var byte		 'SWAP VARIABLE
temp2 var byte 		 'SWAP VARIABLE
temp3 var byte 		 'SWAP VARIABLE
'*********************************************************
'*********************************************************
		'USER CODE FOLLOWS
'*********************************************************
'*********************************************************
rhmin=rhmin -10    'LIMP ADJUSTMENT
rhmax=rhmax +10
lhmin=lhmin-15
lhmax=lhmax+20


act var nib  		'CURRENT ACTION VARIABLE 0=W 1=L 2=R 3=B
templ var bit		'LEFT  SENSOR INPUT VARIABLE
tempr var bit	 	'RIGHT SENSOR INPUT VARIABLE
action var nib		'MAIN  PROCESSED SENSOR INPUT VARIABLE 
action1 var nib 	'LEFT  PROCESSED SENSOR INPUT VARIABLE
action2 var nib 	'RIGHT PROCESSED SENSOR INPUT VARIABLE
choice var bit		'BOOLEAN ALTER COURSE FLAG
complete var bit	'CYCLE COMPLETED FLAG
x var byte

x=ntimax
complete = 1
choice = 0
act = 0
dirs = %1110111111111111	'SET PIN 12 SENSOR IN

pause 1000			'WAIT FOR SENSOR TO SETTLE

serout 1,16624,[255,ntwist,ntwmid] 'CENTER HEAD TWIST
serout 0,16624,[255,ntilt,x]	   'CENTER HEAD TILT	
pause 750

	'LOOP TO LOWER HEAD UNTIL IT SEES FLOOR(CALIBRATION)
for x=ntimax to ntimin step 5
serout 0,16624,[255,ntilt,x]
pause 50
gosub look
if action<>0 then clfloor
next

	'BACK HEAD UP A BIT TO CLEAR FLOOR
clfloor:
x=x-45
serout 0,16624,[255,ntilt,x]
pause 50


main:
		'IF WALKING AND COURSE CLEAR JUST WALK
	if act = 0 and choice = 0 then c0
		'IF NOT WALKING BUT COURSE CLEAR CHECK SURROUNDINGS
	if choice = 0 then check
		'ELSE IF NOT WALKING AND NOT CLEAR, RESET STATES
	act = 0
	choice = 0
	c0:
			'START TO WALK
		GOSUB walk
			'IF SOMETHING WAS FOUND DECIDE WHAT TO DO
		if choice = 1 then decide
GOTO main

look:
		'TOGGLE RIGHT LED 
	high 11 : pause 1
	low 11 : pause 1
	high 11 : pause 1
		'READ FROM SENSOR
	tempr = IN12	
	low 11

		'TOGGLE LEFT LED
	high 10 : pause 1
	low 10 : pause 1
	high 10 : pause 1
		'READ SENSOR
	templ = IN12
	low 10

		'EVALUATE INPUT
	action = 0
	if templ=1 then cont1
	action = action + 1
    cont1:
	if tempr=1 then cont2
	action = action +2
    cont2:
		'RESET COURSE CHANGE BOOLEAN
	choice = 0
		'SET COURSE FLAG BOOLEAN IF STUFF DETECTED
	if action <>0 then xchoice
	r1:	  
return

		'SET COURSE FLAG HIGH AND GO BACK
xchoice:
	choice = 1
	goto r1

	'SOMETHING FOUND IN THE WAY, DETERMINE WHAT TO DO
decide:
	if action = 1 then tr 		'TURN RIGHT
	if action = 2 then tl		'TURN LEFT
	if action = 3 then check	'EVAL OPTIONS
goto main

tr: 
	GOSUB turnright		'TURNRIGHT AND START OVER
goto main

tl:
	GOSUB turnleft		'TURNLEFT  AND STAR OVER
goto main

check:
		'TURN HEAD RIGHT
	serout 1,16624,[255,ntwist,ntwmax] 
	pause 250

		'CHECK STUFF OUT
	gosub look

		'STORE EVALUATED RIGHT DATA
	action1 = action

		'TURN HEAD LEFT
	serout 1,16624,[255,ntwist,ntwmin] 
	pause 250
	
		'CHECK STUFF OUT
	gosub look
	serout 1,16624,[255,ntwist,ntwmid] 
	pause 250
	gosub look

		'STORE EVALUATED LEFT DATA
	action2 = action

		'COMPARE LEFT AND RIGHT DATA

		'IF STUFF WAY OUT ON PERIPHERAL ONLY GO STRAIGHT
	if action=0 AND action1=2 AND action2=1 then gfd

		'CONVERT POSSIBLE 1 OR 2 COMBOS OF TO 1(SAME NUMBER)
	if action1<>2 then con1
	action1=1
	con1:
	if action2<>2 then con2
	action2=1
	con2:

		'IF SIMPLE LEFT TURN DO IT
	if action=1 then tl

		'IF SIMPLE RIGHT TURN DO IT
	if action=2 then tr

		'IF STUFF IN PERIPH AND BLOCKING THE COMPARE THEM
	if action1 > action2 then tl	'TURN LEFT
	if action1 < action2 then tr	'TURN RIGHT

		'IF NOTHING THERE, START OVER 
	if action=0 and action1=0 and action2=0 then gfd

		'IF COMPLETELY BLOCKED TURN LEFT
	if action=3 and action1 = action2 then tl
goto main

		'RESET STUFF AND START OVER
gfd:
	act = 0
	choice = 0
	goto main

		'BACKUP AND START OVER
bcp:
	gosub backup
goto main                               


	'TURN LEFT FUNCTION
turnleft:
		'IF NOT PREV TURNING LEFT, PUT ALL LEGS DOWN
	if act <> 1 then trst

	c1:
		'SET STATE VARIABLE
	act = 1
	
		'BACKUP LEFT HORIZ VALUES
	temp = lhmax	
	temp2 = lhmin
	temp3 = lhavg

		'COPY RIGHT HORIZ VALUES TO LEFT
	lhmax = rhmax	
	lhmin = rhmin
	lhavg = rhavg
	
		'CALL WALK
	GOSUB walk	
	
		'REVERT LEFT HORIZ VALUES	
	lhmax = temp	
	lhmin = temp2
	lhavg = temp3
return

		'CALL STAND AND GO BACK
trst:
gosub stand
goto c1

	'TURN RIGHT FUNCTION
turnright:
		'IF NOT PREVIOUSLY TURNING RIGHT PUT ALL LEGS DOWN
	if act <>2 then tlst

		'SET STATE VARIABLE
	c2:
	act = 2

		'BACK UP RIGHT HORIZ VALUES
	temp = rhmax   
	temp2 = rhmin  
	temp3 = rhavg

		'COPY LEFT HORIZ VALUES TO RIGHT
	rhmax = lhmax  
	rhmin = lhmin
	rhavg = lhavg

		'CALL WALK
	GOSUB walk     
	
		'REVERT RIGHT VALUES
	rhmax = temp   
	rhmin = temp2
	rhavg = temp3
return

		'CALL STAND AND GO BACK
tlst:
gosub stand
goto c2:

	'WALK BACKWARD FUNCTION
backup:	
	'FIRST WALK BACKWARD
	
		'IF NOT PREVIOUSLY BACKING UP PUT ALL LEGS DOWN
	if act <> 3 then bust

		'SET STATE VARIABLE
	c3:
	act = 3

		'SWAP LEFT AND RIGHT HORIZ VALUES
	temp = lhmax    	
	lhmax = rhmax
	rhmax = temp
	temp = lhmin
	lhmin = rhmin
	rhmin = temp
	temp = lhavg
	lhavg = rhavg
	rhavg = temp 
	
		'CALL WALK

	GOSUB walk		

		'REVERT LEFT AND RIGHT HORIZ VALUES
	temp = rhmax	
	rhmax = lhmax
	lhmax = temp
	temp = rhmin
	rhmin = lhmin
	lhmin = temp
	temp = lhavg
	lhavg = rhavg
	rhavg = temp

	'THEN TURN LEFT
	goto tl
return

		'CALL STAND AND GO BACK
bust:
gosub stand
goto c3

walk:
		'IF CYCLE ENDED HALWAY LAST TIME ,PICK UP THERE
	if complete = 0 then conw
		'CHECK STUFF OUT
	GOSUB look
		
		'IF WALKING AND I SAW STUFF END CYCLE AND GO BACK
	if choice = 1 AND act = 0 THEN r2

		'Move group 1 back halfway	
	serout l,16624,[255,lfh,lhavg,255,lbh,lhavg]	
	serout r,16624,[255,rmh,rhavg]
		'Move group 2 forward and up
	serout l,16624,[255,lmh,lhavg,255,lmv,lvmax]
	serout r,16624,[255,rfh,rhavg,255,rbh,rhavg,255,rfv,rvmax,255,rbv,rvmax]
	pause m3
	
		'CHECK STUFF OUT AGAIN
	GOSUB look

		'IF WALKING AND I SAW STUFF END CYCLE AND GO BACK
	if choice = 1 AND act = 0 THEN r2
		'Move group1 back rest of way
	serout l,16624,[255,lfh,lhmin,255,lbh,lhmin]
	serout r,16624,[255,rmh,rhmin]
		'Move group 2 forward and down
	serout l,16624,[255,lmh,lhmax,255,lmv,lvmin]
	serout r,16624,[255,rfh,rhmax,255,rbh,rhmax,255,rfv,rvmin,255,rbv,rvmin]	
	pause m3

		'CHECK STUFF OUT AGAIN
	GOSUB look
		'IF TURNING AND PATH BECAME CLEAR END CYCLE GO BACK
	if choice = 0 AND act <>0 AND act<>4 THEN r3

		'IF WALKING AND I SAW STUFF END CYCLE AND GO BACK
	if choice = 1 AND act = 0 THEN r2
conw:
		'Move group1 forward and up
	serout l,16624,[255,lfh,lhavg,255,lbh,lhavg,255,lfv,lvmax,255,lbv,lvmax]
	serout r,16624,[255,rmh,rhavg,255,rmv,rvmax]
		'Move group 2 back halfway
	serout l,16624,[255,lmh,lhavg]
	serout r,16624,[255,rfh,rhavg,255,rbh,rhavg]
	pause m3

		'CHECK STUFF OUT AGAIN
	GOSUB look

		'IF WALKING AND I SAW STUFF END CYCLE AND GO BACK
	if choice = 1 AND act = 0 THEN r2

		'Move group1 forward and down
	serout l,16624,[255,lfh,lhmax,255,lbh,lhmax,255,lfv,lvmin,255,lbv,lvmin]
	serout r,16624,[255,rmh,rhmax,255,rmv,rvmin]
		'Move group 2 back halfway
	serout l,16624,[255,lmh,lhmin]
	serout r,16624,[255,rfh,rhmin,255,rbh,rhmin]
	pause m3
	complete = 1
	r2:
return		
r3:
	complete=0
return 

	'PUT ALL LEGS DOWN
stand:
	serout r,16624,[255,rfv,rvmin,255,rmv,rvmin,255,rbv,rvmin]
	serout l,16624,[255,lfv,lvmin,255,lmv,lvmin,255,lbv,lvmin]
return