'{$STAMP BS2}

'GP2D02 variables
  val01 var	byte
  cl1	con	4
  dt1	con	5
  input dt1
  high cl1
  val02	var	byte
  i	var	byte
  cl2	con	6
  dt2	con	7
  input dt2
  high cl2

'Eye unit variables
  r_tilt	con	0
  l_tilt	con	1
  r_pan	con	2
  l_pan	con	3
  sync	con	255
  pos	var	byte
  n24n	con	$418D

'Wheel stuff
  svor	con	7
  svol	con	6
  speed	con	80
  idle	con	126

'Counter stuff
  datap con 3   ' Data pin (I/O)
  datapin var in3
  clk con 2     ' Clk pin (output)
  output clk
  output datap

  fpx var word        ' Integer used by some routines
  fpb var byte        ' byte
  ' parameters for FCommand
    chan var nib
    register var nib
    clearreg var bit
    clearchan var bit

  leg	var	byte(16)
  leg_index	var	nib

main:

 gosub freset
 gosub FTotalReset
 Fpx=0
 gosub FPullup
 chan=101
 register=100
 clearchan=1
 gosub FCommand
 pause 1000
 clearchan=0
 clearreg=0

 leg_index=0

' gosub check_down

 movealong:
  gosub center

  gosub forward
  leg(leg_index) = 0	'0 = forward
  debug dec leg_index, tab, dec leg(leg_index), cr
  leg_index=leg_index+1

  gosub check_right

end


''''''''''''''''''''''''''''''''''''''''''''''
check_down:
  gosub bottom
  pause 1000
  gosub read01
  gosub read02
  if val01 < 120 or val02 < 120 then halt2
return

check_right:
  gosub read02
  if val02 >= 150 then yelp
  pause 100
gosub check_left

check_left:
  gosub read01
  if val01 >= 150 then yelp
  pause 100
gosub check_right

yelp:
  debug "LOOK OUT!!",cr
  gosub halt
  gosub FCommand
  leg(leg_index) = fpx
  debug dec leg_index, tab, dec leg(leg_index), cr
  leg_index=leg_index+1
  gosub resetcounter
  clearchan=0
  
  gosub spin_right
  leg(leg_index) = 2	'2 = right turn
  debug dec leg_index, tab, dec leg(leg_index), cr
  leg_index=leg_index+1
  
  if leg_index = 15 then countera 'increas by 4's to add a leg, don't forget the leg array size!

  counter:
    gosub FCommand
    if fpx >= 7 then nextleg
  goto counter
  countera:
    gosub FCommand
    if fpx >= 15 then head_home
  goto countera

nextleg:
  gosub FCommand
  leg(leg_index) = fpx
  debug dec leg_index, tab, dec leg(leg_index), cr
  leg_index=leg_index+1
  gosub resetcounter
  clearchan=0
  debug "start over",cr
  goto movealong

homenextleg:
  debug "homenextleg",cr
  gosub resetcounter
  clearchan=0
  leg_index=leg_index-2

  gosub spin_left
  counter4:
    gosub FCommand
    if fpx >= leg(leg_index) then head_home
  goto counter4

  goto head_home

head_home:
  debug "head_home",cr
  debug dec leg_index, tab, dec leg(leg_index), cr
  leg_index=leg_index-2
  debug dec leg_index, tab, dec leg(leg_index), cr
  gosub resetcounter
  clearchan=0
  gosub forward
  if leg_index = 1 then counter2a
  counter2:
    gosub FCommand
    if fpx >= leg(leg_index) then homenextleg
  goto counter2  
  counter2a:
    gosub FCommand
    if fpx >= leg(leg_index) then final_spin
  goto counter2a

final_spin:
  gosub resetcounter
  clearchan=0
  gosub spin_right
  counter3:
    gosub FCommand
    if fpx >= 15 then halt2
  goto counter3


'''''''''''''''''''''''''''''''''''''''''''''''

center:
  serout 8,n24n,[sync,r_pan,120]
  serout 8,n24n,[sync,l_pan,128]
  serout 8,n24n,[sync,r_tilt,128]
  serout 8,n24n,[sync,l_tilt,128]
return

bottom:
  serout 8,n24n,[sync,r_tilt,78]
  serout 8,n24n,[sync,l_tilt,178]
return

top:
  serout 8,n24n,[sync,r_tilt,158]
  serout 8,n24n,[sync,l_tilt,98]
return

read01:
  low cl1
  rl1:
  if in5 = 0 then rl1
  shiftin dt1, cl1, msbpost, [val01]
  high cl1
  pause 1
return

read02:
  low cl2
  rl2:
  if in7 = 0 then rl2
  shiftin dt2, cl2, msbpost, [val02]
  high cl2
  pause 1
return

halt:
  serout 8,n24n,[sync,svor,idle]
  serout 8,n24n,[sync,svol,idle]
return

halt2:
  debug "Emergancy Stop",cr
  serout 8,n24n,[sync,svor,idle]
  serout 8,n24n,[sync,svol,idle]
stop

forward:
  serout 8,n24n,[sync,svor,idle-speed]
  serout 8,n24n,[sync,svol,idle+speed]
return

backup:
  serout 8,n24n,[sync,svor,idle+speed]
  serout 8,n24n,[sync,svol,idle-speed]
return

spin_right:
  serout 8,n24n,[sync,svor,idle+speed]
  serout 8,n24n,[sync,svol,idle+speed]
return

spin_left:
  serout 8,n24n,[sync,svor,idle-speed]
  serout 8,n24n,[sync,svol,idle-speed]
return

resetcounter:
  clearchan=1
  gosub FCommand
return

' Reset the Pak7 I/O
FReset:
  LOW DATAP
  LOW CLK
  HIGH CLK
  HIGH DATAP
  LOW CLK     
return

FCommand:
' inputs chan = channel, register = register, clearreg and clearchan
  fpb=(clearchan<<7) + (clearreg<<6) + (register<<3) + chan
  gosub FSendByte

FReadWord:
  Shiftin datap,clk,MSBPRE,[fpx.lowbyte,fpx.highbyte]
  return

FTotalReset:
  fpb=$FF
FSendByte:
  Shiftout datap,clk,MSBFIRST,[fpb]
  return

FPullUp:
' set fpx to the pull up mask
  fpb=%11010000
Fscmd:
  gosub FSendByte
  fpb=fpx
  goto FSendByte